commit 65061f49a87b736c223281e5d55b4769ff5175dd Author: Alperen Date: Fri Mar 21 10:58:07 2025 +0100 Inital commit -Lab2 diff --git a/bin/CameraCalibration b/bin/CameraCalibration new file mode 100755 index 0000000..3d13acb Binary files /dev/null and b/bin/CameraCalibration differ diff --git a/bin/RobotServoing b/bin/RobotServoing new file mode 100755 index 0000000..48547b7 Binary files /dev/null and b/bin/RobotServoing differ diff --git a/data/color_params.yaml b/data/color_params.yaml new file mode 100644 index 0000000..dfda17b --- /dev/null +++ b/data/color_params.yaml @@ -0,0 +1,8 @@ +%YAML:1.0 +--- +lowH: 13 +highH: 28 +lowS: 145 +highS: 255 +lowV: 166 +highV: 255 diff --git a/data/default.xml b/data/default.xml new file mode 100644 index 0000000..ce8792a --- /dev/null +++ b/data/default.xml @@ -0,0 +1,46 @@ + + + + + 9 + 6 + + + 24 + + + "CHESSBOARD" + + + "0" + + 0 + + + 200 + + + 25 + + 1 + + 1 + + 0 + + + "./data/microsoft_livecam_hd3000.xml" + + 1 + + 1 + + 1 + + + diff --git a/data/microsoft_livecam_hd3000.xml b/data/microsoft_livecam_hd3000.xml new file mode 100644 index 0000000..6e30e31 --- /dev/null +++ b/data/microsoft_livecam_hd3000.xml @@ -0,0 +1,801 @@ + + +"Fri 21 Mar 2025 09:27:47 AM CET" +25 +640 +480 +9 +6 +24. +1. + +10 +0 + + 3 + 3 +
d
+ + 6.4255337987856933e+02 0. 3.0599103303969883e+02 0. + 6.4255337987856933e+02 2.4890050657835533e+02 0. 0. 1. + + 5 + 1 +
d
+ + -3.6862222382996651e-01 5.4039239357051927e-01 0. 0. + 5.7490593833122294e-01 +1.1800776344446415e+00 + + 25 + 1 +
f
+ + 4.46368992e-01 4.74848151e-01 4.66854095e-01 4.78679091e-01 + 5.68649769e-01 5.13346672e-01 4.26975638e-01 3.42881858e-01 + 3.47389936e-01 3.62793386e-01 3.83918703e-01 3.97116661e-01 + 1.28075409e+00 3.74014401e+00 1.58068585e+00 6.05669498e-01 + 4.64526922e-01 4.59366083e-01 4.01410282e-01 3.85575354e-01 + 8.09818804e-01 7.70252466e-01 5.83547652e-01 5.22580028e-01 + 3.37711191e+00 + + + 25 + 6 +
d
+ + -5.7431593890464989e-02 1.3029467717610399e-01 + -1.4674693486203274e+00 -9.7360966536635260e+01 + 8.2171905259136608e+01 4.2477091656427456e+02 + -3.6666920664906962e-02 1.4771675025327061e-01 + -1.4688457238091308e+00 -9.7525321015099493e+01 + 8.3198172952024464e+01 4.2256384356837935e+02 + -2.7725946511208202e-02 1.8923765639016457e-01 + -1.4648211155455908e+00 -9.8838991853909192e+01 + 8.2301717330212369e+01 4.2005733220270088e+02 3.7217939713951577e-02 + 2.4849598007105811e-01 -1.4488158383018686e+00 + -1.0045226355901009e+02 8.0251429726818088e+01 + 4.2967580575910512e+02 1.6224699828105241e-01 9.9278771306062080e-02 + -1.3815434180674284e+00 -9.8246062273019461e+01 + 5.7641587036254577e+01 4.0893849239280036e+02 2.1445855231256339e-01 + -6.4614842737479763e-02 -1.3381671809185784e+00 + -9.6533423266991079e+01 4.7344387309822082e+01 + 4.0521488218214540e+02 1.5590437982300218e-01 + -1.9808581393367394e-01 -1.3481557447295740e+00 + -9.3796679145560958e+01 4.7524625702831848e+01 + 4.1286585489046820e+02 2.7633307463230481e-02 + -2.6614547692078699e-01 -1.3682886085837740e+00 + -9.4955963203558582e+01 4.6825309461115275e+01 + 4.1829054980203750e+02 -1.6736587438844255e-01 + -3.7725702383945248e-01 -1.4115242723659911e+00 + -8.8249405676594250e+01 4.3340856493635606e+01 + 4.0527189231004417e+02 -3.8103359635426648e-01 + -4.9640956454034507e-01 -1.4498868121599970e+00 + -6.5620322752815795e+01 3.3078140790670126e+01 + 3.6995519937881068e+02 -6.4434103607262161e-01 + -3.8200151836118840e-01 -1.4362214423957094e+00 + -1.0121792720624782e+02 3.2245126093586713e+01 + 3.8968355803903808e+02 -7.5335329190473399e-01 + -5.0169060031450763e-01 -1.3878441123012553e+00 + -1.0288236774541983e+02 1.3466789207972456e+01 + 3.6424977042487927e+02 -8.1842846420309145e-01 + -6.4275168986938969e-01 -1.3766809444474220e+00 + -8.9526689993302412e+01 1.0015135300032092e+01 + 3.4276689054605635e+02 -8.8566426454787128e-01 + -7.0024785779959886e-01 -1.3538627645664303e+00 + -8.9620448385779554e+01 -1.4482701604047463e+00 + 3.2657095561039921e+02 -6.6030028251874506e-01 + -7.8040947256473192e-01 -1.3309285551353995e+00 + -7.7062986353048544e+01 -1.1310681881154341e+00 + 3.1478470080437398e+02 -5.2614624171681224e-01 + -8.1808682951495881e-01 -1.3391916669553690e+00 + -6.9672769114042410e+01 7.6192937873454083e+00 + 3.0796067436087378e+02 -3.8898869459659235e-01 + -8.2209847296860683e-01 -1.3441477130264459e+00 + -6.7628805040922472e+01 2.1900313178984515e+01 + 3.0662483207720618e+02 -1.4395492125176648e-01 + -9.0148557547095820e-01 -1.3334332792599211e+00 + -4.8703196547039006e+01 3.5217847091826037e+01 + 2.9295734414984918e+02 7.2152965562305799e-02 + -9.6473892383226756e-01 -1.2956643029275301e+00 + -3.4558440460790322e+01 5.0665576820870818e+01 + 2.9548593800380354e+02 2.1714418421075021e-01 + -1.0538093068041965e+00 -1.2345598365049912e+00 + -1.9392435357549946e+01 5.0099099908107299e+01 + 2.9053945595138788e+02 2.6960072121530176e-01 + -8.5080553892233401e-01 -1.3358384908793999e+00 + -4.2729723616700170e+01 7.5848342552128955e+01 + 3.1809378173438790e+02 5.5145854649796280e-02 + -4.4498149523556108e-01 -1.5915032100212350e+00 + -5.7818789579809746e+01 8.9212220015625931e+01 + 3.5634117396846824e+02 -4.6521582579577492e-01 + 1.6145602023653166e-01 -1.6279849784386189e+00 + -7.7636578986063867e+01 1.0058152225323030e+02 + 4.0488418585013170e+02 -8.3381742582532392e-01 + 4.8292857110365672e-01 -1.5202940606577400e+00 + -1.0071140738562387e+02 1.0348178243466556e+02 + 4.3162226501772841e+02 -9.6675154343092073e-01 + 5.7304649775913852e-01 -1.5399281913858436e+00 + -9.3102251581267780e+01 1.0660578243153054e+02 + 4.3266592036354433e+02 + + 25 + 54 +
"2f"
+ + 1.62141739e+02 3.69541840e+02 1.65150879e+02 3.35476776e+02 + 1.68121063e+02 3.00471252e+02 1.71282425e+02 2.64747345e+02 + 1.74327072e+02 2.28714584e+02 1.77460510e+02 1.92765808e+02 + 1.80659042e+02 1.56494324e+02 1.84204590e+02 1.20872429e+02 + 1.87486267e+02 8.57541122e+01 1.95518509e+02 3.74826935e+02 + 1.98597733e+02 3.40434235e+02 2.01640091e+02 3.05163635e+02 + 2.04939056e+02 2.68773956e+02 2.08154388e+02 2.32539429e+02 + 2.11461960e+02 1.95817429e+02 2.15061798e+02 1.59304886e+02 + 2.18423630e+02 1.23118767e+02 2.22158615e+02 8.75667038e+01 + 2.30240524e+02 3.79904205e+02 2.33411240e+02 3.45333191e+02 + 2.36525711e+02 3.09553406e+02 2.40046799e+02 2.73113800e+02 + 2.43286896e+02 2.36257965e+02 2.46623337e+02 1.99265198e+02 + 2.50326965e+02 1.62164902e+02 2.54038010e+02 1.25600159e+02 + 2.57655182e+02 8.96587753e+01 2.65955231e+02 3.84903381e+02 + 2.69301331e+02 3.49869995e+02 2.72679321e+02 3.14067719e+02 + 2.76204498e+02 2.77092834e+02 2.79507629e+02 2.40119812e+02 + 2.83118744e+02 2.02719254e+02 2.86954926e+02 1.65315811e+02 + 2.90568848e+02 1.28412079e+02 2.94451965e+02 9.22128601e+01 + 3.02443695e+02 3.89360016e+02 3.05757965e+02 3.54356140e+02 + 3.09384552e+02 3.18092194e+02 3.12920685e+02 2.81165283e+02 + 3.16428497e+02 2.43721375e+02 3.20115540e+02 2.06322571e+02 + 3.23856506e+02 1.68550827e+02 3.27650421e+02 1.31473999e+02 + 3.31478577e+02 9.50669708e+01 3.39424805e+02 3.93652557e+02 + 3.42886444e+02 3.58407593e+02 3.46521210e+02 3.22160492e+02 + 3.50259918e+02 2.84860321e+02 3.53832977e+02 2.47547531e+02 + 3.57548096e+02 2.09866333e+02 3.61391113e+02 1.72106476e+02 + 3.65170898e+02 1.34781601e+02 3.69134460e+02 9.81592712e+01 + 1.61127258e+02 3.71561859e+02 1.63810181e+02 3.37570007e+02 + 1.66579102e+02 3.02485596e+02 1.69437836e+02 2.66571442e+02 + 1.72268845e+02 2.30359146e+02 1.75222198e+02 1.93911880e+02 + 1.78184067e+02 1.57185699e+02 1.81323212e+02 1.20913063e+02 + 1.84408157e+02 8.50969620e+01 1.94683304e+02 3.76976196e+02 + 1.97632965e+02 3.42537628e+02 2.00448883e+02 3.07279602e+02 + 2.03512039e+02 2.70689056e+02 2.06398346e+02 2.34157562e+02 + 2.09544647e+02 1.97080597e+02 2.12813751e+02 1.59917084e+02 + 2.16090225e+02 1.23195274e+02 2.19479828e+02 8.68218994e+01 + 2.29581909e+02 3.81932404e+02 2.32533264e+02 3.47453552e+02 + 2.35597977e+02 3.11639526e+02 2.38829727e+02 2.74992157e+02 + 2.41917999e+02 2.37783493e+02 2.45187088e+02 2.00525284e+02 + 2.48562607e+02 1.62805954e+02 2.52162247e+02 1.25658905e+02 + 2.55623352e+02 8.90625687e+01 2.65498230e+02 3.86981628e+02 + 2.68702087e+02 3.52060577e+02 2.72117462e+02 3.16195923e+02 + 2.75402405e+02 2.79074310e+02 2.78595276e+02 2.41803162e+02 + 2.82106262e+02 2.04099152e+02 2.85697693e+02 1.66134933e+02 + 2.89312073e+02 1.28520386e+02 2.93080688e+02 9.15561218e+01 + 3.02265045e+02 3.91422089e+02 3.05492859e+02 3.56438843e+02 + 3.08904144e+02 3.20300354e+02 3.12434753e+02 2.83137421e+02 + 3.15857056e+02 2.45535477e+02 3.19467316e+02 2.07615692e+02 + 3.23232605e+02 1.69443497e+02 3.26960785e+02 1.31644531e+02 + 3.30697937e+02 9.43880615e+01 3.39427612e+02 3.95708466e+02 + 3.42848358e+02 3.60546234e+02 3.46483032e+02 3.24319489e+02 + 3.50131104e+02 2.86845917e+02 3.53696136e+02 2.49372299e+02 + 3.57401489e+02 2.11304108e+02 3.61242584e+02 1.72882721e+02 + 3.65093475e+02 1.35035355e+02 3.69009949e+02 9.75952530e+01 + 1.58476852e+02 3.70913025e+02 1.60703796e+02 3.37039703e+02 + 1.63317139e+02 3.01805023e+02 1.65747223e+02 2.65802612e+02 + 1.68386627e+02 2.29242279e+02 1.71091202e+02 1.92389679e+02 + 1.73731949e+02 1.54939438e+02 1.76691635e+02 1.17982697e+02 + 1.79613174e+02 8.13849030e+01 1.91978058e+02 3.76582123e+02 + 1.94524994e+02 3.42354401e+02 1.97246933e+02 3.06892029e+02 + 1.99860367e+02 2.70245148e+02 2.02594482e+02 2.33295135e+02 + 2.05593033e+02 1.95598038e+02 2.08704666e+02 1.57709564e+02 + 2.11845154e+02 1.20261978e+02 2.15242889e+02 8.30658188e+01 + 2.26714661e+02 3.82024658e+02 2.29490280e+02 3.47529388e+02 + 2.32422134e+02 3.11643524e+02 2.35476501e+02 2.74741425e+02 + 2.38449875e+02 2.37208862e+02 2.41545731e+02 1.99303253e+02 + 2.45030701e+02 1.60811951e+02 2.48451035e+02 1.22749809e+02 + 2.51954987e+02 8.51955948e+01 2.62699097e+02 3.87398560e+02 + 2.65781860e+02 3.52518585e+02 2.69143463e+02 3.16451050e+02 + 2.72430969e+02 2.79060913e+02 2.75525452e+02 2.41416901e+02 + 2.78962616e+02 2.02924149e+02 2.82625916e+02 1.64182785e+02 + 2.86281464e+02 1.25608902e+02 2.90129059e+02 8.76016235e+01 + 2.99641479e+02 3.92288849e+02 3.02965057e+02 3.57327606e+02 + 3.06399628e+02 3.20843292e+02 3.09903839e+02 2.83437378e+02 + 3.13372467e+02 2.45366531e+02 3.17033356e+02 2.06695374e+02 + 3.20856476e+02 1.67596664e+02 3.24700073e+02 1.28872787e+02 + 3.28629944e+02 9.05050049e+01 3.37080658e+02 3.96909698e+02 + 3.40639374e+02 3.61704132e+02 3.44413666e+02 3.25298401e+02 + 3.48134369e+02 2.87508209e+02 3.51801117e+02 2.49391602e+02 + 3.55697418e+02 2.10547089e+02 3.59671539e+02 1.71322708e+02 + 3.63761139e+02 1.32350510e+02 3.67943695e+02 9.36866379e+01 + 1.59014786e+02 3.65344360e+02 1.61139313e+02 3.32689056e+02 + 1.63258728e+02 2.98770844e+02 1.65411957e+02 2.63726746e+02 + 1.67579163e+02 2.27896515e+02 1.69864471e+02 1.91478271e+02 + 1.72238770e+02 1.54286346e+02 1.74785431e+02 1.17002388e+02 + 1.77395340e+02 7.98523331e+01 1.91923996e+02 3.71364227e+02 + 1.94285858e+02 3.38517883e+02 1.96646713e+02 3.04441864e+02 + 1.99269928e+02 2.68775909e+02 2.01698853e+02 2.32629715e+02 + 2.04443115e+02 1.95496506e+02 2.07362396e+02 1.57713348e+02 + 2.10295212e+02 1.20040215e+02 2.13356415e+02 8.23711929e+01 + 2.25867233e+02 3.77332977e+02 2.28666733e+02 3.44295074e+02 + 2.31551712e+02 3.09702911e+02 2.34496292e+02 2.74066498e+02 + 2.37365982e+02 2.37366638e+02 2.40375488e+02 1.99867096e+02 + 2.43678513e+02 1.61508423e+02 2.47156891e+02 1.23385735e+02 + 2.50524841e+02 8.51628799e+01 2.61237640e+02 3.83066132e+02 + 2.64403137e+02 3.49697662e+02 2.67601196e+02 3.15152893e+02 + 2.71058533e+02 2.78837891e+02 2.74324432e+02 2.42228882e+02 + 2.77664886e+02 2.04346512e+02 2.81503815e+02 1.65612411e+02 + 2.85239380e+02 1.27014969e+02 2.89173706e+02 8.84094238e+01 + 2.97234863e+02 3.88307983e+02 3.00681366e+02 3.55046082e+02 + 3.04413727e+02 3.20142853e+02 3.08175079e+02 2.83931763e+02 + 3.11776611e+02 2.46785553e+02 3.15709869e+02 2.08804077e+02 + 3.19786530e+02 1.69848251e+02 3.23976257e+02 1.30871445e+02 + 3.28179810e+02 9.20611420e+01 3.33642670e+02 3.93506287e+02 + 3.37703033e+02 3.59919495e+02 3.41684326e+02 3.25059540e+02 + 3.45835785e+02 2.88657379e+02 3.50015350e+02 2.51553543e+02 + 3.54340424e+02 2.13313644e+02 3.58753326e+02 1.74302597e+02 + 3.63327148e+02 1.35203079e+02 3.67882385e+02 9.59293823e+01 + 1.54637390e+02 3.37283386e+02 1.60331009e+02 3.02368683e+02 + 1.66013168e+02 2.66267120e+02 1.71800903e+02 2.29251587e+02 + 1.77610596e+02 1.91568253e+02 1.83563034e+02 1.53736771e+02 + 1.89598969e+02 1.15474762e+02 1.96036758e+02 7.75704803e+01 + 2.02351013e+02 4.02310333e+01 1.90354202e+02 3.44371185e+02 + 1.96334778e+02 3.09416870e+02 2.02429749e+02 2.73483673e+02 + 2.08586304e+02 2.36187790e+02 2.14804337e+02 1.98607147e+02 + 2.21266373e+02 1.60519974e+02 2.27767395e+02 1.22126869e+02 + 2.34358871e+02 8.40565033e+01 2.41324844e+02 4.64100342e+01 + 2.26565964e+02 3.50927551e+02 2.32896301e+02 3.16412903e+02 + 2.39348419e+02 2.80385132e+02 2.46022507e+02 2.43326660e+02 + 2.52521423e+02 2.05586853e+02 2.59365631e+02 1.67613297e+02 + 2.66251221e+02 1.29062332e+02 2.73329193e+02 9.08523407e+01 + 2.80320374e+02 5.32898636e+01 2.63260681e+02 3.57621918e+02 + 2.69893738e+02 3.22868500e+02 2.76684998e+02 2.87258850e+02 + 2.83700714e+02 2.50152420e+02 2.90723328e+02 2.12843094e+02 + 2.97714874e+02 1.74695221e+02 3.05086609e+02 1.36333130e+02 + 3.12294434e+02 9.82309875e+01 3.19978912e+02 6.04985695e+01 + 2.99794067e+02 3.63547424e+02 3.06692810e+02 3.29340912e+02 + 3.13889465e+02 2.93583771e+02 3.21159393e+02 2.57060699e+02 + 3.28470428e+02 2.19703430e+02 3.35916473e+02 1.81955292e+02 + 3.43347961e+02 1.43671814e+02 3.51040680e+02 1.05731987e+02 + 3.58570007e+02 6.81856461e+01 3.36183380e+02 3.69435516e+02 + 3.43429352e+02 3.35275146e+02 3.50717560e+02 2.99893372e+02 + 3.58270752e+02 2.63460266e+02 3.65836975e+02 2.26688370e+02 + 3.73436188e+02 1.89231949e+02 3.81211121e+02 1.51208267e+02 + 3.88923401e+02 1.13500938e+02 3.96970978e+02 7.60492325e+01 + 1.56335419e+02 3.22453827e+02 1.64307861e+02 2.86653442e+02 + 1.72328064e+02 2.50033829e+02 1.80431335e+02 2.12955124e+02 + 1.88573471e+02 1.75786743e+02 1.97016479e+02 1.38898834e+02 + 2.05311325e+02 1.02001434e+02 2.14059021e+02 6.60117493e+01 + 2.22693253e+02 3.05561428e+01 1.92234802e+02 3.30321747e+02 + 2.00352112e+02 2.94529205e+02 2.08532303e+02 2.58422791e+02 + 2.16775177e+02 2.21333160e+02 2.25315796e+02 1.84489044e+02 + 2.33663193e+02 1.47553497e+02 2.42437668e+02 1.10787392e+02 + 2.51024384e+02 7.47540054e+01 2.60087494e+02 3.97407990e+01 + 2.27969986e+02 3.37521362e+02 2.36328918e+02 3.02403046e+02 + 2.44599731e+02 2.66373657e+02 2.53227448e+02 2.29694901e+02 + 2.61587067e+02 1.93091690e+02 2.70431915e+02 1.56471283e+02 + 2.78891235e+02 1.19792152e+02 2.87813660e+02 8.41376495e+01 + 2.96584839e+02 4.91905212e+01 2.63784668e+02 3.44622803e+02 + 2.72209381e+02 3.09660431e+02 2.80604950e+02 2.74244507e+02 + 2.89244537e+02 2.37840424e+02 2.98022766e+02 2.01769882e+02 + 3.06490570e+02 1.65370575e+02 3.15294464e+02 1.29230103e+02 + 3.23909882e+02 9.36158066e+01 3.33005432e+02 5.89208488e+01 + 2.98958557e+02 3.50937836e+02 3.07311676e+02 3.16746582e+02 + 3.15691467e+02 2.81535583e+02 3.24343689e+02 2.45790771e+02 + 3.33045013e+02 2.09996094e+02 3.41716705e+02 1.74245331e+02 + 3.50288574e+02 1.38434982e+02 3.59152130e+02 1.03289574e+02 + 3.67761169e+02 6.87346954e+01 3.33408966e+02 3.57382782e+02 + 3.41733856e+02 3.23323547e+02 3.50124268e+02 2.88674408e+02 + 3.58667816e+02 2.53405029e+02 3.67412903e+02 2.18289978e+02 + 3.75834167e+02 1.82872879e+02 3.84442657e+02 1.47516296e+02 + 3.93098816e+02 1.12851669e+02 4.01786469e+02 7.86234741e+01 + 1.63499817e+02 3.20954987e+02 1.71276367e+02 2.85565918e+02 + 1.78897171e+02 2.49654846e+02 1.86843567e+02 2.13900192e+02 + 1.94516403e+02 1.78505142e+02 2.02474182e+02 1.43739243e+02 + 2.10350525e+02 1.09467323e+02 2.18367752e+02 7.64693222e+01 + 2.26355591e+02 4.44115372e+01 1.98550049e+02 3.28643250e+02 + 2.06227737e+02 2.93212708e+02 2.13927216e+02 2.57777435e+02 + 2.21602676e+02 2.21922775e+02 2.29385605e+02 1.86863892e+02 + 2.37085999e+02 1.52174744e+02 2.44981094e+02 1.18218887e+02 + 2.52628006e+02 8.51456528e+01 2.60583282e+02 5.32245216e+01 + 2.33498489e+02 3.35686584e+02 2.41139084e+02 3.00788208e+02 + 2.48539749e+02 2.65507355e+02 2.56266327e+02 2.30248138e+02 + 2.63738495e+02 1.95196793e+02 2.71528412e+02 1.60746994e+02 + 2.78966125e+02 1.26773857e+02 2.86658447e+02 9.41014786e+01 + 2.94203827e+02 6.22885170e+01 2.68346008e+02 3.42670563e+02 + 2.75619568e+02 3.07837433e+02 2.83110535e+02 2.73241364e+02 + 2.90504303e+02 2.37967484e+02 2.98085999e+02 2.03551666e+02 + 3.05316071e+02 1.69390228e+02 3.12703522e+02 1.35773315e+02 + 3.19968781e+02 1.03191162e+02 3.27509583e+02 7.15301132e+01 + 3.02434601e+02 3.48807648e+02 3.09459534e+02 3.14783569e+02 + 3.16586304e+02 2.80313202e+02 3.23868713e+02 2.45755814e+02 + 3.31197144e+02 2.11541916e+02 3.38351074e+02 1.77852234e+02 + 3.45297638e+02 1.44612213e+02 3.52437439e+02 1.12314491e+02 + 3.59442383e+02 8.09711456e+01 3.35810699e+02 3.54959503e+02 + 3.42667816e+02 3.21142609e+02 3.49419098e+02 2.87271362e+02 + 3.56436493e+02 2.53122177e+02 3.63532928e+02 2.19494339e+02 + 3.70261871e+02 1.86261505e+02 3.77073364e+02 1.53335022e+02 + 3.83844482e+02 1.21423225e+02 3.90709625e+02 9.02605362e+01 + 1.63202469e+02 3.19132782e+02 1.70751221e+02 2.84222748e+02 + 1.78180115e+02 2.49341721e+02 1.85608566e+02 2.14931519e+02 + 1.92750793e+02 1.81393906e+02 2.00107193e+02 1.48947769e+02 + 2.07104401e+02 1.17210365e+02 2.14265015e+02 8.68953400e+01 + 2.21158279e+02 5.77642784e+01 1.98090027e+02 3.26609772e+02 + 2.05272583e+02 2.91402496e+02 2.12482376e+02 2.56772217e+02 + 2.19487915e+02 2.22255997e+02 2.26532455e+02 1.88795135e+02 + 2.33328323e+02 1.56242935e+02 2.40255402e+02 1.24639313e+02 + 2.46788513e+02 9.42751236e+01 2.53534302e+02 6.52088470e+01 + 2.32874023e+02 3.33506104e+02 2.39856522e+02 2.98706787e+02 + 2.46599686e+02 2.63947357e+02 2.53426102e+02 2.29769913e+02 + 2.59940582e+02 1.96272720e+02 2.66627197e+02 1.63697937e+02 + 2.72890625e+02 1.32216110e+02 2.79405487e+02 1.01916771e+02 + 2.85521851e+02 7.28525848e+01 2.67942902e+02 3.40407379e+02 + 2.74377014e+02 3.05564362e+02 2.80832275e+02 2.71275909e+02 + 2.87239471e+02 2.37001053e+02 2.93548981e+02 2.03778275e+02 + 2.99601776e+02 1.71400970e+02 3.05630524e+02 1.39999207e+02 + 3.11524353e+02 1.09752525e+02 3.17487701e+02 8.06951599e+01 + 3.02362671e+02 3.46536957e+02 3.08379028e+02 3.12267334e+02 + 3.14377930e+02 2.77959106e+02 3.20343872e+02 2.44294083e+02 + 3.26305695e+02 2.11184753e+02 3.32049896e+02 1.79096222e+02 + 3.37482666e+02 1.47818649e+02 3.43113373e+02 1.17680855e+02 + 3.48450623e+02 8.87770844e+01 3.36375366e+02 3.52508301e+02 + 3.41914673e+02 3.18421875e+02 3.47349304e+02 2.84702881e+02 + 3.52849670e+02 2.51188599e+02 3.58406342e+02 2.18477402e+02 + 3.63619019e+02 1.86657898e+02 3.68736359e+02 1.55567825e+02 + 3.73842255e+02 1.25688362e+02 3.78937408e+02 9.67554550e+01 + 1.68754608e+02 3.15769012e+02 1.75585297e+02 2.80992004e+02 + 1.82197266e+02 2.46927017e+02 1.88570038e+02 2.14206085e+02 + 1.94619797e+02 1.82722992e+02 2.00557297e+02 1.52914551e+02 + 2.06271606e+02 1.24262093e+02 2.11941254e+02 9.73741684e+01 + 2.17229797e+02 7.16535110e+01 2.05102921e+02 3.22190491e+02 + 2.11252182e+02 2.86822327e+02 2.17192581e+02 2.52845764e+02 + 2.22824463e+02 2.19672409e+02 2.28375519e+02 1.88218536e+02 + 2.33560272e+02 1.58065308e+02 2.38828186e+02 1.29496994e+02 + 2.43692719e+02 1.02311966e+02 2.48651779e+02 7.66723251e+01 + 2.41715103e+02 3.27910461e+02 2.47147797e+02 2.92769958e+02 + 2.52278580e+02 2.58596558e+02 2.57390442e+02 2.25632339e+02 + 2.62193909e+02 1.93775757e+02 2.67018829e+02 1.63536148e+02 + 2.71294739e+02 1.34885406e+02 2.75711700e+02 1.07693069e+02 + 2.79742737e+02 8.20798798e+01 2.78631378e+02 3.33733948e+02 + 2.83238037e+02 2.98507416e+02 2.87650604e+02 2.64507538e+02 + 2.91936981e+02 2.31302994e+02 2.96239655e+02 1.99482559e+02 + 3.00147369e+02 1.69331802e+02 3.04001160e+02 1.40605103e+02 + 3.07570984e+02 1.13379639e+02 3.11315399e+02 8.75219498e+01 + 3.15243530e+02 3.38929779e+02 3.18882385e+02 3.04041473e+02 + 3.22551636e+02 2.69961182e+02 3.26153564e+02 2.37053452e+02 + 3.29627167e+02 2.05235046e+02 3.32982422e+02 1.75176575e+02 + 3.36051880e+02 1.46445404e+02 3.39196716e+02 1.19207199e+02 + 3.42034515e+02 9.33343124e+01 3.51393158e+02 3.43819885e+02 + 3.54330994e+02 3.09068390e+02 3.57085632e+02 2.75462250e+02 + 3.59897308e+02 2.42539124e+02 3.62728119e+02 2.11008102e+02 + 3.65270477e+02 1.81111816e+02 3.67717377e+02 1.52403412e+02 + 3.70143982e+02 1.25165863e+02 3.72558624e+02 9.92170410e+01 + 1.93610336e+02 3.05545410e+02 1.99474792e+02 2.70389069e+02 + 2.04775558e+02 2.37321396e+02 2.09960556e+02 2.06168015e+02 + 2.14627914e+02 1.76969666e+02 2.19247406e+02 1.50043884e+02 + 2.23458389e+02 1.24578026e+02 2.27607666e+02 1.01300583e+02 + 2.31481583e+02 7.93229828e+01 2.34216110e+02 3.09622925e+02 + 2.38552155e+02 2.74129364e+02 2.42697922e+02 2.40715103e+02 + 2.46585876e+02 2.09282639e+02 2.50324265e+02 1.79869568e+02 + 2.53687683e+02 1.52713593e+02 2.57147888e+02 1.27502655e+02 + 2.60230591e+02 1.03933601e+02 2.63298279e+02 8.20093536e+01 + 2.75100311e+02 3.13403076e+02 2.78167511e+02 2.77781830e+02 + 2.80902161e+02 2.44276978e+02 2.83608795e+02 2.12852768e+02 + 2.86162689e+02 1.83276001e+02 2.88583954e+02 1.55664124e+02 + 2.90786163e+02 1.30608566e+02 2.92950958e+02 1.06892830e+02 + 2.95078400e+02 8.51289368e+01 3.16370605e+02 3.16851135e+02 + 3.17833832e+02 2.81363129e+02 3.19394501e+02 2.47891235e+02 + 3.20767273e+02 2.16311432e+02 3.22260345e+02 1.86583435e+02 + 3.23456604e+02 1.59341766e+02 3.24672546e+02 1.34019806e+02 + 3.25785126e+02 1.10366837e+02 3.27034607e+02 8.83629303e+01 + 3.57108673e+02 3.20106964e+02 3.57221008e+02 2.84776398e+02 + 3.57329865e+02 2.51419769e+02 3.57504700e+02 2.19844498e+02 + 3.57686005e+02 1.90276154e+02 3.58068176e+02 1.62966843e+02 + 3.58184906e+02 1.37670029e+02 3.58407135e+02 1.14055473e+02 + 3.58575836e+02 9.19702377e+01 3.97300140e+02 3.22735413e+02 + 3.95931274e+02 2.87688202e+02 3.94737549e+02 2.54736038e+02 + 3.93727356e+02 2.23405884e+02 3.92922791e+02 1.93998917e+02 + 3.92009979e+02 1.66841202e+02 3.91262299e+02 1.41524002e+02 + 3.90448975e+02 1.17706207e+02 3.89737122e+02 9.55622253e+01 + 1.43116730e+02 3.00558441e+02 1.53245193e+02 2.69747162e+02 + 1.62525818e+02 2.40821014e+02 1.71375793e+02 2.13544281e+02 + 1.79320114e+02 1.88098862e+02 1.87190918e+02 1.64250504e+02 + 1.94379761e+02 1.41709778e+02 2.01332825e+02 1.21261070e+02 + 2.07769867e+02 1.01497513e+02 1.78465729e+02 3.02098511e+02 + 1.87414673e+02 2.70521393e+02 1.95832687e+02 2.40909073e+02 + 2.03657715e+02 2.13067581e+02 2.11054459e+02 1.87102463e+02 + 2.17792618e+02 1.62869751e+02 2.24405838e+02 1.40582367e+02 + 2.30580460e+02 1.19516129e+02 2.36360291e+02 1.00241722e+02 + 2.15371780e+02 3.03477509e+02 2.23344955e+02 2.71341278e+02 + 2.30593811e+02 2.41185684e+02 2.37551697e+02 2.12973648e+02 + 2.43737335e+02 1.86439102e+02 2.49576523e+02 1.61832596e+02 + 2.55448318e+02 1.39427292e+02 2.60640259e+02 1.18435623e+02 + 2.65713318e+02 9.88674774e+01 2.54166000e+02 3.04612823e+02 + 2.60807251e+02 2.71877289e+02 2.66944122e+02 2.41501877e+02 + 2.72592834e+02 2.12718460e+02 2.77825470e+02 1.85854126e+02 + 2.82709778e+02 1.61316727e+02 2.87486694e+02 1.38671616e+02 + 2.91805511e+02 1.17472549e+02 2.96048920e+02 9.79637833e+01 + 2.94244293e+02 3.05653931e+02 2.99307129e+02 2.72655670e+02 + 3.04198914e+02 2.41710068e+02 3.08591003e+02 2.12766754e+02 + 3.12605896e+02 1.85551666e+02 3.16421753e+02 1.60869308e+02 + 3.20165863e+02 1.38113754e+02 3.23444977e+02 1.16810020e+02 + 3.26628815e+02 9.72017593e+01 3.35082916e+02 3.06253693e+02 + 3.38851685e+02 2.72836182e+02 3.42178467e+02 2.41912430e+02 + 3.45337189e+02 2.12703629e+02 3.48147217e+02 1.85544815e+02 + 3.50797211e+02 1.60785889e+02 3.53349213e+02 1.37685501e+02 + 3.55613434e+02 1.16368996e+02 3.57842102e+02 9.65044327e+01 + 1.29686584e+02 2.71508972e+02 1.42632080e+02 2.43754288e+02 + 1.54532303e+02 2.18400070e+02 1.65662323e+02 1.94669861e+02 + 1.75706436e+02 1.72912415e+02 1.85322006e+02 1.53034439e+02 + 1.94160675e+02 1.34437408e+02 2.02539902e+02 1.17679214e+02 + 2.10197250e+02 1.01829529e+02 1.66971573e+02 2.72925537e+02 + 1.78573807e+02 2.44366104e+02 1.89307419e+02 2.18154984e+02 + 1.99243073e+02 1.93942551e+02 2.08301620e+02 1.71694229e+02 + 2.16749695e+02 1.51484192e+02 2.24717575e+02 1.33270401e+02 + 2.32059189e+02 1.16129395e+02 2.39148270e+02 1.00470116e+02 + 2.06206589e+02 2.74263611e+02 2.16347565e+02 2.44988968e+02 + 2.25663788e+02 2.18265366e+02 2.34396637e+02 1.93728806e+02 + 2.42190033e+02 1.70962723e+02 2.49359436e+02 1.50377579e+02 + 2.56311554e+02 1.32078232e+02 2.62688965e+02 1.14925850e+02 + 2.68557343e+02 9.92424850e+01 2.47472626e+02 2.75411285e+02 + 2.56121460e+02 2.45520554e+02 2.63788483e+02 2.18577026e+02 + 2.70919952e+02 1.93494308e+02 2.77289398e+02 1.70371323e+02 + 2.83376831e+02 1.49661957e+02 2.89138947e+02 1.31232742e+02 + 2.94314972e+02 1.13843384e+02 2.99244019e+02 9.81558456e+01 + 2.90429291e+02 2.76493683e+02 2.96874878e+02 2.46323380e+02 + 3.02948578e+02 2.18781357e+02 3.08433197e+02 1.93497589e+02 + 3.13362671e+02 1.70096970e+02 3.17960297e+02 1.49356506e+02 + 3.22375488e+02 1.30529358e+02 3.26387909e+02 1.13289413e+02 + 3.30095428e+02 9.73427200e+01 3.34362549e+02 2.76913086e+02 + 3.38878052e+02 2.46585983e+02 3.43044250e+02 2.19113968e+02 + 3.46725250e+02 1.93529694e+02 3.50205719e+02 1.70102539e+02 + 3.53347992e+02 1.49358337e+02 3.56318604e+02 1.30297348e+02 + 3.59021423e+02 1.12675056e+02 3.61570099e+02 9.66533813e+01 + 1.42234360e+02 2.66385254e+02 1.54873672e+02 2.41835754e+02 + 1.66318192e+02 2.19568985e+02 1.76754547e+02 1.99290833e+02 + 1.86235855e+02 1.80494659e+02 1.95091232e+02 1.63634399e+02 + 2.02956726e+02 1.48125549e+02 2.13519531e+02 1.28507324e+02 + 2.17517090e+02 1.20462318e+02 1.83331238e+02 2.67195251e+02 + 1.94150528e+02 2.41897095e+02 2.03889130e+02 2.19161148e+02 + 2.12743546e+02 1.98264496e+02 2.20774567e+02 1.79398041e+02 + 2.28171753e+02 1.62273132e+02 2.35159821e+02 1.47104309e+02 + 2.41383713e+02 1.32672485e+02 2.48767944e+02 1.16342857e+02 + 2.26444626e+02 2.67753967e+02 2.35168045e+02 2.42103806e+02 + 2.42884872e+02 2.18814301e+02 2.50178848e+02 1.98056305e+02 + 2.56416840e+02 1.78611664e+02 2.62289276e+02 1.61256088e+02 + 2.67741272e+02 1.46049789e+02 2.74772797e+02 1.26265335e+02 + 2.77745483e+02 1.17610947e+02 2.71398560e+02 2.68201935e+02 + 2.77705933e+02 2.42251465e+02 2.83459686e+02 2.18980453e+02 + 2.88604401e+02 1.97543335e+02 2.93273041e+02 1.77853012e+02 + 2.97521393e+02 1.60666443e+02 3.01549438e+02 1.45421967e+02 + 3.05184357e+02 1.31228790e+02 3.09588715e+02 1.14770149e+02 + 3.17451630e+02 2.68667023e+02 3.21243134e+02 2.42460403e+02 + 3.24629120e+02 2.18993896e+02 3.27825623e+02 1.97485580e+02 + 3.30582855e+02 1.77646744e+02 3.33187164e+02 1.60436249e+02 + 3.35583435e+02 1.45056732e+02 3.37913483e+02 1.30610596e+02 + 3.40079987e+02 1.15590462e+02 3.64258942e+02 2.68294952e+02 + 3.65437012e+02 2.42278488e+02 3.66458160e+02 2.18885437e+02 + 3.67465271e+02 1.97413467e+02 3.68379761e+02 1.77640808e+02 + 3.69255829e+02 1.60556442e+02 3.70041382e+02 1.44838867e+02 + 3.70728882e+02 1.30531738e+02 3.71820312e+02 1.14657974e+02 + 1.34330566e+02 2.45708557e+02 1.48402420e+02 2.24235168e+02 + 1.60924866e+02 2.04976471e+02 1.72554474e+02 1.87530762e+02 + 1.82790848e+02 1.71663727e+02 1.95580963e+02 1.52599136e+02 + 2.06069946e+02 1.37857407e+02 2.12169693e+02 1.28911362e+02 + 2.12169800e+02 1.28911209e+02 1.77195084e+02 2.45990112e+02 + 1.89154053e+02 2.23589539e+02 1.99869873e+02 2.03828629e+02 + 2.09638840e+02 1.85983414e+02 2.18386124e+02 1.70060425e+02 + 2.31220078e+02 1.47868271e+02 2.36334518e+02 1.38724823e+02 + 2.44110229e+02 1.25520493e+02 2.46376465e+02 1.21646156e+02 + 2.22136856e+02 2.45922882e+02 2.31731720e+02 2.23226730e+02 + 2.40337540e+02 2.03086456e+02 2.48233490e+02 1.85414185e+02 + 2.54880157e+02 1.68902725e+02 2.63810577e+02 1.49096085e+02 + 2.70186066e+02 1.35424896e+02 2.74643951e+02 1.26105766e+02 + 2.74643951e+02 1.26105629e+02 2.69259674e+02 2.45734146e+02 + 2.76208771e+02 2.22799957e+02 2.82481659e+02 2.02704712e+02 + 2.88011688e+02 1.84409821e+02 2.92936951e+02 1.67695755e+02 + 2.99789124e+02 1.46374985e+02 3.03544830e+02 1.35700104e+02 + 3.07226654e+02 1.23756950e+02 3.08596100e+02 1.19976395e+02 + 3.17587341e+02 2.45777527e+02 3.21624817e+02 2.22591660e+02 + 3.25272522e+02 2.02302200e+02 3.28510742e+02 1.83958786e+02 + 3.31356812e+02 1.67213196e+02 3.35186523e+02 1.47160309e+02 + 3.37503174e+02 1.33820297e+02 3.39788971e+02 1.23864548e+02 + 3.39789001e+02 1.23864449e+02 3.66761841e+02 2.44830307e+02 + 3.67808838e+02 2.21950775e+02 3.68707092e+02 2.01882004e+02 + 3.69541351e+02 1.83492416e+02 3.70338531e+02 1.66845810e+02 + 3.71252655e+02 1.45831833e+02 3.72062042e+02 1.33967987e+02 + 3.72348480e+02 1.22994377e+02 3.72348480e+02 1.22994148e+02 + 1.52511719e+02 2.45328049e+02 1.63713562e+02 2.19529907e+02 + 1.73801407e+02 1.96601166e+02 1.83203217e+02 1.76105362e+02 + 1.91438049e+02 1.57561691e+02 1.99254639e+02 1.41228668e+02 + 2.06317673e+02 1.26211838e+02 2.15539017e+02 1.07865303e+02 + 2.19254196e+02 9.97283936e+01 1.98730560e+02 2.52405365e+02 + 2.07528717e+02 2.25460648e+02 2.15509811e+02 2.01776764e+02 + 2.22635040e+02 1.80555832e+02 2.29290146e+02 1.61606827e+02 + 2.35259628e+02 1.44806976e+02 2.40714249e+02 1.30143524e+02 + 2.48388062e+02 1.09505959e+02 2.51866592e+02 1.01432892e+02 + 2.46323120e+02 2.59148743e+02 2.52411957e+02 2.31655090e+02 + 2.57896667e+02 2.07372818e+02 2.63091919e+02 1.86007812e+02 + 2.67522125e+02 1.66304688e+02 2.71737885e+02 1.48892975e+02 + 2.75587250e+02 1.34068298e+02 2.80798401e+02 1.14607689e+02 + 2.82852478e+02 1.05963882e+02 2.94768707e+02 2.65796600e+02 + 2.98139648e+02 2.37921143e+02 3.01096863e+02 2.13434662e+02 + 3.03819763e+02 1.91246750e+02 3.06424469e+02 1.71101669e+02 + 3.08749573e+02 1.53786118e+02 3.10910706e+02 1.38570450e+02 + 3.13780090e+02 1.17691261e+02 3.15416992e+02 1.09487663e+02 + 3.43276123e+02 2.72538483e+02 3.43546234e+02 2.44347412e+02 + 3.44058105e+02 2.19424347e+02 3.44501862e+02 1.96883759e+02 + 3.45172546e+02 1.76482193e+02 3.45517548e+02 1.58815750e+02 + 3.45938538e+02 1.43336563e+02 3.46429199e+02 1.28897247e+02 + 3.46743195e+02 1.13517105e+02 3.91160736e+02 2.78328949e+02 + 3.88719574e+02 2.50171265e+02 3.86575409e+02 2.25326721e+02 + 3.84862976e+02 2.02599075e+02 3.83453064e+02 1.82034592e+02 + 3.82098389e+02 1.64392349e+02 3.80744629e+02 1.48386078e+02 + 3.79273529e+02 1.27495476e+02 3.78863495e+02 1.18765305e+02 + 1.63909958e+02 2.64001190e+02 1.72790283e+02 2.33814987e+02 + 1.80739868e+02 2.06760635e+02 1.88243668e+02 1.82507309e+02 + 1.94726532e+02 1.60572845e+02 2.00948425e+02 1.41194931e+02 + 2.06504257e+02 1.23394531e+02 2.11774734e+02 1.07685921e+02 + 2.16827362e+02 9.17628479e+01 2.11325653e+02 2.73455475e+02 + 2.17708817e+02 2.42365845e+02 2.23431656e+02 2.14588013e+02 + 2.28671051e+02 1.89632965e+02 2.33492584e+02 1.67304749e+02 + 2.37844376e+02 1.47443878e+02 2.41918579e+02 1.29795105e+02 + 2.45529312e+02 1.13612984e+02 2.49064529e+02 9.90277405e+01 + 2.59318451e+02 2.82433746e+02 2.62873688e+02 2.50871796e+02 + 2.66364716e+02 2.22731583e+02 2.69415985e+02 1.97692368e+02 + 2.72291534e+02 1.74642792e+02 2.74842224e+02 1.54145447e+02 + 2.77174561e+02 1.36380295e+02 2.79376221e+02 1.19999329e+02 + 2.81514221e+02 1.03259514e+02 3.07264557e+02 2.91124908e+02 + 3.08326294e+02 2.59379395e+02 3.09208832e+02 2.31165024e+02 + 3.10138489e+02 2.05432236e+02 3.11087555e+02 1.81961029e+02 + 3.11838623e+02 1.61556427e+02 3.12504059e+02 1.43390701e+02 + 3.13213684e+02 1.26702057e+02 3.13742767e+02 1.11545509e+02 + 3.54367584e+02 2.99465973e+02 3.52564758e+02 2.67655945e+02 + 3.51193268e+02 2.39295746e+02 3.50022614e+02 2.13376648e+02 + 3.49162567e+02 1.89605408e+02 3.48133545e+02 1.69054901e+02 + 3.47146851e+02 1.50550964e+02 3.46331451e+02 1.33613281e+02 + 3.45447693e+02 1.14952110e+02 4.00186768e+02 3.06701935e+02 + 3.96080505e+02 2.75287537e+02 3.92203308e+02 2.46988831e+02 + 3.89089142e+02 2.21072845e+02 3.86360809e+02 1.97446457e+02 + 3.83662994e+02 1.76749512e+02 3.81122650e+02 1.57773392e+02 + 3.79000916e+02 1.40653732e+02 3.76988373e+02 1.25006622e+02 + 1.67264984e+02 2.93756348e+02 1.74796082e+02 2.58745117e+02 + 1.81625992e+02 2.27183365e+02 1.87993088e+02 1.98503937e+02 + 1.93616089e+02 1.72479141e+02 1.98969604e+02 1.49214523e+02 + 2.03721619e+02 1.27906677e+02 2.08283218e+02 1.09029572e+02 + 2.12409531e+02 9.14054031e+01 2.14444519e+02 3.04668915e+02 + 2.19656998e+02 2.68936340e+02 2.24325165e+02 2.36900681e+02 + 2.28579636e+02 2.07655609e+02 2.32551239e+02 1.81322556e+02 + 2.36179001e+02 1.57570633e+02 2.39357239e+02 1.36439224e+02 + 2.42373581e+02 1.17064072e+02 2.45252960e+02 9.94084396e+01 + 2.61463776e+02 3.14747101e+02 2.64202789e+02 2.79063934e+02 + 2.66735046e+02 2.46579895e+02 2.68976837e+02 2.17432617e+02 + 2.71158691e+02 1.90450607e+02 2.73079315e+02 1.66282669e+02 + 2.74614471e+02 1.44965164e+02 2.76259430e+02 1.25341171e+02 + 2.77631012e+02 1.07530609e+02 3.07909027e+02 3.24341553e+02 + 3.08381592e+02 2.88564270e+02 3.08488373e+02 2.56460083e+02 + 3.08840729e+02 2.26710510e+02 3.09361420e+02 1.99566513e+02 + 3.09588318e+02 1.75476532e+02 3.09563019e+02 1.53820526e+02 + 3.09727814e+02 1.33959442e+02 3.09894928e+02 1.15741348e+02 + 3.53003967e+02 3.33111328e+02 3.50887512e+02 2.97898560e+02 + 3.49131958e+02 2.65655457e+02 3.47595825e+02 2.36086090e+02 + 3.46419830e+02 2.08737152e+02 3.44957611e+02 1.84543198e+02 + 3.43564026e+02 1.62740250e+02 3.42368042e+02 1.42615952e+02 + 3.41285156e+02 1.24367348e+02 3.96391724e+02 3.40922729e+02 + 3.92307770e+02 3.06173340e+02 3.88346069e+02 2.74458923e+02 + 3.85169159e+02 2.44890869e+02 3.82321136e+02 2.17886475e+02 + 3.79395172e+02 1.93738373e+02 3.76611816e+02 1.71580124e+02 + 3.74269501e+02 1.51295776e+02 3.72033478e+02 1.32675217e+02 + 2.01081436e+02 3.24746216e+02 2.04208435e+02 2.83331696e+02 + 2.07104126e+02 2.45203064e+02 2.09888504e+02 2.10535324e+02 + 2.12477722e+02 1.78826340e+02 2.15046310e+02 1.50506210e+02 + 2.17248947e+02 1.24390984e+02 2.19272995e+02 1.01206505e+02 + 2.21356949e+02 7.96316376e+01 2.47435333e+02 3.37411652e+02 + 2.48483337e+02 2.95947906e+02 2.49496155e+02 2.58316345e+02 + 2.50370178e+02 2.23367584e+02 2.51356781e+02 1.91764297e+02 + 2.52223511e+02 1.63111191e+02 2.52761536e+02 1.37269836e+02 + 2.53447861e+02 1.13463715e+02 2.54217010e+02 9.19815369e+01 + 2.91990723e+02 3.48579529e+02 2.90849701e+02 3.08119568e+02 + 2.90096619e+02 2.70571960e+02 2.89174255e+02 2.36384369e+02 + 2.88601593e+02 2.04545532e+02 2.88172974e+02 1.75671494e+02 + 2.87122864e+02 1.49716034e+02 2.86474792e+02 1.25950745e+02 + 2.85840576e+02 1.04235306e+02 3.34297821e+02 3.58909637e+02 + 3.31447388e+02 3.19162201e+02 3.28590576e+02 2.82628662e+02 + 3.26352325e+02 2.48408554e+02 3.24415771e+02 2.16797806e+02 + 3.22435638e+02 1.88290924e+02 3.20179718e+02 1.62335342e+02 + 3.18400208e+02 1.38387817e+02 3.16645905e+02 1.16362068e+02 + 3.73898651e+02 3.67997681e+02 3.69229126e+02 3.29574707e+02 + 3.65082062e+02 2.93520599e+02 3.61318268e+02 2.60041870e+02 + 3.58236725e+02 2.28680786e+02 3.54840912e+02 2.00371307e+02 + 3.51563751e+02 1.74408203e+02 3.48685852e+02 1.50460052e+02 + 3.46009979e+02 1.28410889e+02 4.11186432e+02 3.76094940e+02 + 4.05084961e+02 3.38583679e+02 3.99354340e+02 3.03764648e+02 + 3.94455048e+02 2.70707214e+02 3.90048737e+02 2.40139923e+02 + 3.85647522e+02 2.12124771e+02 3.81380188e+02 1.86167343e+02 + 3.77645508e+02 1.62213242e+02 3.74087830e+02 1.40003403e+02 + 2.32172699e+02 3.56989410e+02 2.32763107e+02 3.12442505e+02 + 2.33520691e+02 2.70579224e+02 2.34422089e+02 2.31860962e+02 + 2.35326462e+02 1.96110733e+02 2.36160416e+02 1.63591675e+02 + 2.37014053e+02 1.33474411e+02 2.37614227e+02 1.06363846e+02 + 2.38507675e+02 8.11914520e+01 2.72691772e+02 3.69162262e+02 + 2.72083344e+02 3.25456268e+02 2.71411194e+02 2.84854584e+02 + 2.70765503e+02 2.46547714e+02 2.70518402e+02 2.11531433e+02 + 2.70121704e+02 1.79087692e+02 2.69414886e+02 1.49426987e+02 + 2.69039215e+02 1.22023537e+02 2.68663635e+02 9.70199738e+01 + 3.10627014e+02 3.79470520e+02 3.08425354e+02 3.37543335e+02 + 3.06560486e+02 2.97801788e+02 3.04533630e+02 2.60783112e+02 + 3.03271942e+02 2.26036209e+02 3.01935547e+02 1.93949524e+02 + 3.00032013e+02 1.64510193e+02 2.98587433e+02 1.37380005e+02 + 2.97242004e+02 1.12312775e+02 3.45784973e+02 3.88817352e+02 + 3.42412689e+02 3.48333862e+02 3.39163239e+02 3.10267181e+02 + 3.36394867e+02 2.73720734e+02 3.34139038e+02 2.39753998e+02 + 3.31602844e+02 2.08341782e+02 3.28759827e+02 1.79273132e+02 + 3.26541931e+02 1.52222519e+02 3.24369873e+02 1.27117119e+02 + 3.77898956e+02 3.96863708e+02 3.73389160e+02 3.58232513e+02 + 3.69293335e+02 3.21130463e+02 3.65501953e+02 2.85896790e+02 + 3.62513458e+02 2.52581085e+02 3.59181091e+02 2.21863602e+02 + 3.55571899e+02 1.93033905e+02 3.52600128e+02 1.66367416e+02 + 3.49687164e+02 1.41390610e+02 4.07737366e+02 4.04110626e+02 + 4.02382538e+02 3.66668152e+02 3.97158783e+02 3.31271790e+02 + 3.92707581e+02 2.96847748e+02 3.88794403e+02 2.64638702e+02 + 3.84693756e+02 2.34572922e+02 3.80574097e+02 2.06266708e+02 + 3.77172913e+02 1.79694901e+02 3.73617188e+02 1.54938705e+02 + 2.63544922e+02 3.58018890e+02 2.62853790e+02 3.11977936e+02 + 2.62210541e+02 2.68514038e+02 2.61521545e+02 2.28286392e+02 + 2.61313782e+02 1.91214264e+02 2.60809631e+02 1.57386597e+02 + 2.60537903e+02 1.26074959e+02 2.60103912e+02 9.78533707e+01 + 2.59988251e+02 7.17247086e+01 2.98334229e+02 3.71367615e+02 + 2.96450562e+02 3.26700409e+02 2.94700012e+02 2.85055511e+02 + 2.93076447e+02 2.45673203e+02 2.91839569e+02 2.09525894e+02 + 2.90424622e+02 1.76062073e+02 2.88753571e+02 1.45347610e+02 + 2.87470795e+02 1.16968506e+02 2.86265228e+02 9.10214767e+01 + 3.30040009e+02 3.82655212e+02 3.27071045e+02 3.40317078e+02 + 3.24359497e+02 2.99857330e+02 3.21556427e+02 2.62098694e+02 + 3.19572357e+02 2.26530151e+02 3.17566650e+02 1.93550659e+02 + 3.14791199e+02 1.63303818e+02 3.12745392e+02 1.35280655e+02 + 3.10638031e+02 1.09307442e+02 3.58759979e+02 3.92862762e+02 + 3.54984833e+02 3.52247589e+02 3.51251373e+02 3.13751404e+02 + 3.47974335e+02 2.76782104e+02 3.45383392e+02 2.42323532e+02 + 3.42355042e+02 2.10225388e+02 3.39047272e+02 1.80409042e+02 + 3.36287170e+02 1.52591049e+02 3.33557037e+02 1.26725632e+02 + 3.84657776e+02 4.01683929e+02 3.80056183e+02 3.63201813e+02 + 3.75659607e+02 3.25871094e+02 3.71769348e+02 2.90498047e+02 + 3.68626251e+02 2.56873932e+02 3.65042694e+02 2.25658051e+02 + 3.61168213e+02 1.96289719e+02 3.57855774e+02 1.68932541e+02 + 3.54650269e+02 1.43245087e+02 4.08570984e+02 4.09489471e+02 + 4.03372345e+02 3.72473297e+02 3.98224854e+02 3.37130341e+02 + 3.93733582e+02 3.02700653e+02 3.89867493e+02 2.70308350e+02 + 3.85678070e+02 2.39907669e+02 3.81554993e+02 2.11191589e+02 + 3.77900513e+02 1.84193832e+02 3.74370361e+02 1.58807205e+02 + 2.20197845e+02 3.99139465e+02 2.22223526e+02 3.53644958e+02 + 2.24292618e+02 3.09264069e+02 2.26446060e+02 2.66515717e+02 + 2.28544266e+02 2.25704315e+02 2.30653305e+02 1.87531174e+02 + 2.32616852e+02 1.51278305e+02 2.34421021e+02 1.18028862e+02 + 2.36391785e+02 8.67394485e+01 2.57412811e+02 4.04088409e+02 + 2.58497681e+02 3.60414368e+02 2.59665100e+02 3.17886200e+02 + 2.60770294e+02 2.76467712e+02 2.62072845e+02 2.37236084e+02 + 2.63186707e+02 1.99922943e+02 2.63876373e+02 1.64915039e+02 + 2.64780823e+02 1.32045853e+02 2.65697906e+02 1.01581474e+02 + 2.92221527e+02 4.07998474e+02 2.92223389e+02 3.66465698e+02 + 2.92427124e+02 3.25502075e+02 2.92467865e+02 2.85916168e+02 + 2.93066010e+02 2.47773163e+02 2.93483246e+02 2.11693634e+02 + 2.93164124e+02 1.77589096e+02 2.93277496e+02 1.45624313e+02 + 2.93221405e+02 1.15574692e+02 3.24325897e+02 4.11632721e+02 + 3.23559662e+02 3.71651886e+02 3.22718658e+02 3.32680328e+02 + 3.22291443e+02 2.94288452e+02 3.22168060e+02 2.57698669e+02 + 3.21481628e+02 2.22733704e+02 3.20468445e+02 1.89665680e+02 + 3.19762390e+02 1.58375092e+02 3.19229095e+02 1.29009079e+02 + 3.53801178e+02 4.14409515e+02 3.52224335e+02 3.76320221e+02 + 3.50696625e+02 3.38705261e+02 3.49397675e+02 3.02123749e+02 + 3.48646973e+02 2.66677124e+02 3.47380829e+02 2.33076721e+02 + 3.45761505e+02 2.00739258e+02 3.44508057e+02 1.70398361e+02 + 3.43245270e+02 1.41527206e+02 3.81359406e+02 4.16682617e+02 + 3.79076416e+02 3.80089142e+02 3.76564117e+02 3.44351837e+02 + 3.74770447e+02 3.08931580e+02 3.73274414e+02 2.75078766e+02 + 3.71321320e+02 2.42490143e+02 3.69307770e+02 2.11261978e+02 + 3.67611664e+02 1.81530960e+02 3.65863037e+02 1.53381958e+02 + 2.03271561e+02 4.05306244e+02 2.01220276e+02 3.64373993e+02 + 1.99398407e+02 3.23446930e+02 1.97734756e+02 2.82735046e+02 + 1.96373260e+02 2.43072266e+02 1.95085846e+02 2.04505661e+02 + 1.94005615e+02 1.66894836e+02 1.92815384e+02 1.31367111e+02 + 1.92087311e+02 9.72688599e+01 2.44407867e+02 4.02632996e+02 + 2.41880188e+02 3.62378998e+02 2.39401642e+02 3.22014832e+02 + 2.37227478e+02 2.81899384e+02 2.35273697e+02 2.42766815e+02 + 2.33402100e+02 2.04719101e+02 2.31535095e+02 1.68107452e+02 + 2.30010803e+02 1.32824677e+02 2.28471436e+02 9.94363785e+01 + 2.84815491e+02 3.99616150e+02 2.81520660e+02 3.59927246e+02 + 2.78572418e+02 3.20445160e+02 2.75641510e+02 2.81121857e+02 + 2.73263763e+02 2.42588669e+02 2.70897980e+02 2.05285660e+02 + 2.68312866e+02 1.69221802e+02 2.66206787e+02 1.34698090e+02 + 2.63967407e+02 1.01804161e+02 3.24013855e+02 3.96299164e+02 + 3.20286743e+02 3.57463135e+02 3.16583252e+02 3.18607208e+02 + 3.13370544e+02 2.80106995e+02 3.10359039e+02 2.42433289e+02 + 3.07316620e+02 2.05793625e+02 3.04208344e+02 1.70551270e+02 + 3.01421722e+02 1.36701950e+02 2.98761505e+02 1.04385239e+02 + 3.61547974e+02 3.92556427e+02 3.57230896e+02 3.54575134e+02 + 3.53121185e+02 3.16656494e+02 3.49212250e+02 2.78962616e+02 + 3.45746277e+02 2.42282471e+02 3.42191711e+02 2.06536880e+02 + 3.38568970e+02 1.71989212e+02 3.35318359e+02 1.38881042e+02 + 3.32100037e+02 1.07213776e+02 3.97583954e+02 3.88582764e+02 + 3.92690918e+02 3.51503723e+02 3.88011322e+02 3.14575012e+02 + 3.83644104e+02 2.77841125e+02 3.79651459e+02 2.42017029e+02 + 3.75559082e+02 2.07285019e+02 3.71579376e+02 1.73444916e+02 + 3.67787994e+02 1.41047668e+02 3.64284332e+02 1.09959122e+02 + 1.85824356e+02 4.03379486e+02 1.85505005e+02 3.67301880e+02 + 1.85317917e+02 3.31212097e+02 1.84902725e+02 2.94854675e+02 + 1.84422989e+02 2.59382599e+02 1.84103333e+02 2.24445160e+02 + 1.83634384e+02 1.90018433e+02 1.83435303e+02 1.57134369e+02 + 1.82874420e+02 1.24934097e+02 2.17389816e+02 4.03227142e+02 + 2.16644775e+02 3.66091888e+02 2.15888657e+02 3.28812500e+02 + 2.15325562e+02 2.91600677e+02 2.14436203e+02 2.55106445e+02 + 2.13761780e+02 2.19278915e+02 2.13326736e+02 1.84321075e+02 + 2.12626450e+02 1.50440338e+02 2.12159256e+02 1.18040031e+02 + 2.50856705e+02 4.02721039e+02 2.49711929e+02 3.64606171e+02 + 2.48664413e+02 3.26337097e+02 2.47723007e+02 2.88160980e+02 + 2.46449005e+02 2.50604660e+02 2.45374680e+02 2.13938980e+02 + 2.44615646e+02 1.78203568e+02 2.43710388e+02 1.43864319e+02 + 2.42818466e+02 1.10759201e+02 2.86474030e+02 4.01798828e+02 + 2.85076721e+02 3.62619995e+02 2.83628754e+02 3.23433472e+02 + 2.82272888e+02 2.84266937e+02 2.80462646e+02 2.45818497e+02 + 2.79202271e+02 2.08456009e+02 2.78053680e+02 1.72166290e+02 + 2.76687805e+02 1.37101883e+02 2.75480438e+02 1.03564873e+02 + 3.24063782e+02 4.00369843e+02 3.22099304e+02 3.60295959e+02 + 3.20329498e+02 3.20050812e+02 3.18307648e+02 2.80062927e+02 + 3.16261169e+02 2.40866486e+02 3.14478210e+02 2.02824677e+02 + 3.12850952e+02 1.65703491e+02 3.11209442e+02 1.30373413e+02 + 3.09444733e+02 9.64232941e+01 3.63104767e+02 3.98561584e+02 + 3.60744720e+02 3.57466583e+02 3.58522949e+02 3.16338989e+02 + 3.56188812e+02 2.75569824e+02 3.53691284e+02 2.35672104e+02 + 3.51464935e+02 1.96981171e+02 3.49313690e+02 1.59428024e+02 + 3.47081604e+02 1.23429016e+02 3.44975311e+02 8.93195724e+01 + 1.60794510e+02 3.97535675e+02 1.63410736e+02 3.63729034e+02 + 1.66071671e+02 3.30061890e+02 1.68596802e+02 2.96286682e+02 + 1.70933746e+02 2.63073242e+02 1.73468185e+02 2.30464584e+02 + 1.75922684e+02 1.98395767e+02 1.78644104e+02 1.67485947e+02 + 1.80884506e+02 1.37415939e+02 1.78408340e+02 3.99380127e+02 + 1.80698959e+02 3.63889862e+02 1.83193527e+02 3.28631195e+02 + 1.85588028e+02 2.93341217e+02 1.87742126e+02 2.58656494e+02 + 1.90329788e+02 2.24669235e+02 1.93187103e+02 1.91507233e+02 + 1.95563461e+02 1.59361130e+02 1.98268173e+02 1.28425476e+02 + 1.97556488e+02 4.01071014e+02 1.99807510e+02 3.64110138e+02 + 2.02208496e+02 3.27119293e+02 2.04658279e+02 2.90236298e+02 + 2.06593246e+02 2.53922668e+02 2.08779236e+02 2.18554199e+02 + 2.11721878e+02 1.84005325e+02 2.14414001e+02 1.50903229e+02 + 2.16888947e+02 1.18782341e+02 2.18787109e+02 4.02992889e+02 + 2.21118591e+02 3.64131042e+02 2.23454269e+02 3.25395172e+02 + 2.25675079e+02 2.86656952e+02 2.27394775e+02 2.48788971e+02 + 2.29673279e+02 2.11930115e+02 2.32725327e+02 1.76133591e+02 + 2.35173767e+02 1.41500183e+02 2.37641327e+02 1.08592697e+02 + 2.42512558e+02 4.04677460e+02 2.44596313e+02 3.63957489e+02 + 2.46888535e+02 3.23409027e+02 2.48939240e+02 2.82792877e+02 + 2.50532867e+02 2.43212311e+02 2.52849091e+02 2.04721161e+02 + 2.55543243e+02 1.67362213e+02 2.58031830e+02 1.31733505e+02 + 2.60302887e+02 9.75951538e+01 2.68299927e+02 4.06514984e+02 + 2.70401855e+02 3.63684906e+02 2.72738342e+02 3.20893921e+02 + 2.74715851e+02 2.78440002e+02 2.76332001e+02 2.36969910e+02 + 2.78547180e+02 1.96798798e+02 2.80980469e+02 1.58147141e+02 + 2.82904907e+02 1.21096489e+02 2.85263580e+02 8.60775833e+01 + 1.69868454e+02 4.01302826e+02 1.80481323e+02 3.66703400e+02 + 1.76497589e+02 3.33976532e+02 1.75277054e+02 3.01770294e+02 + 1.79896164e+02 2.68199615e+02 1.78845917e+02 2.38260742e+02 + 1.92686005e+02 1.98944901e+02 1.82780273e+02 1.77649658e+02 + 1.96812073e+02 1.38254776e+02 1.85613266e+02 4.01543457e+02 + 1.87646423e+02 3.66097046e+02 1.83616318e+02 3.32292969e+02 + 1.91781158e+02 2.95653931e+02 1.87757492e+02 2.64345886e+02 + 1.87791595e+02 2.33138382e+02 1.92686050e+02 1.98944839e+02 + 1.92292374e+02 1.70613846e+02 1.96812195e+02 1.38254608e+02 + 2.00652023e+02 4.01540649e+02 2.02492752e+02 3.64202118e+02 + 2.04476990e+02 3.27167755e+02 2.06504028e+02 2.90444244e+02 + 2.07842468e+02 2.54570251e+02 2.09426254e+02 2.19712631e+02 + 2.12159149e+02 1.85730423e+02 2.14101913e+02 1.53478455e+02 + 2.16252899e+02 1.22222687e+02 2.17426895e+02 4.01445770e+02 + 2.19307449e+02 3.62181610e+02 2.21147781e+02 3.22995453e+02 + 2.22743317e+02 2.84449402e+02 2.23802948e+02 2.46844559e+02 + 2.25629395e+02 2.10442978e+02 2.28052094e+02 1.75287888e+02 + 2.29833603e+02 1.41501633e+02 2.31697632e+02 1.09423561e+02 + 2.36401352e+02 4.01163788e+02 2.37942261e+02 3.59644684e+02 + 2.39569534e+02 3.18583344e+02 2.40992065e+02 2.77891968e+02 + 2.41777527e+02 2.38491760e+02 2.43452255e+02 2.00421021e+02 + 2.45519257e+02 1.63613235e+02 2.47254837e+02 1.28812103e+02 + 2.48998474e+02 9.53958511e+01 2.57023773e+02 4.00693359e+02 + 2.58434753e+02 3.56828674e+02 2.60114471e+02 3.13396393e+02 + 2.61339691e+02 2.70552155e+02 2.62112335e+02 2.29098145e+02 + 2.63492035e+02 1.89299179e+02 2.65199677e+02 1.51142349e+02 + 2.66458557e+02 1.14716064e+02 2.68024933e+02 8.04716110e+01 + diff --git a/include/DynamixelHandler.h b/include/DynamixelHandler.h new file mode 100644 index 0000000..c1d4ee6 --- /dev/null +++ b/include/DynamixelHandler.h @@ -0,0 +1,88 @@ +#if defined(__linux__) || defined(__APPLE__) +#include +#include +#include +#define STDIN_FILENO 0 +#elif defined(_WIN32) || defined(_WIN64) +#include +#endif + +#define _USE_MATH_DEFINES +#include + +// standard includes +#include +#include +#include +#include +#include + +// dynamixel sdk include +#include "dynamixel_sdk/dynamixel_sdk.h" + +// addresses of variables in the register +#define ADDR_XL320_CONTROL_MODE 11 +#define ADDR_XL320_TORQUE_ENABLE 24 +#define ADDR_XL320_GOAL_POSITION 30 +#define ADDR_XL320_GOAL_VELOCITY 32 +#define ADDR_XL320_PRESENT_POSITION 37 +#define ADDR_XL320_PRESENT_VELOCITY 39 +#define ADDR_XL320_HARDWARE_ERROR_STATUS 50 + +// rotation direction +#define ROT_DIRECTION_Q1 1 +#define ROT_DIRECTION_Q2 -1 +#define ROT_DIRECTION_QPEN 1 +// nb of joints +#define NB_JOINTS 3 + + +class DynamixelHandler +{ + +public: + DynamixelHandler(); + ~DynamixelHandler(); + +public: + bool openPort(); + void closePort(); + bool setBaudRate(int); + void setDeviceName(std::string); + void setProtocolVersion(float); + bool enableTorque(bool); + bool setControlMode(int iControlMode); + + bool readCurrentJointPosition(std::vector& vCurrentJointPosition); + bool readCurrentJointPosition(std::vector& vCurrentJointPosition); + bool sendTargetJointPosition(std::vector& vTargetJointPosition); + bool sendTargetJointPosition(std::vector& vTargetJointPosition); + bool sendTargetJointVelocity(std::vector& vTargetJointVelocity); + bool sendTargetJointVelocity(std::vector& vTargetJointVelocity); + int convertAngleToJointCmd(float fJointAngle); + float convertJointCmdToAngle(int iJointCmd); + int convertJointVelocityToJointCmd(float fJointVelocity); + +private: + std::string m_sDeviceName; + float m_fProtocolVersion; + int m_i32BaudRate; + + dynamixel::PortHandler* m_pPortHandler; + dynamixel::PacketHandler* m_pPacketHandler; + + bool m_bIsDeviceNameSet; + bool m_bIsProtocolVersionSet; + bool m_bIsPortOpened; + bool m_bIsBaudRateSet; + + int m_i32DxlCommunicationResult; // Communication result + uint8_t m_ui8DxlError; // Dynamixel error + std::vector m_vDxlCurrentPosition; // Present position + + float m_fMinJointCmd = 0; + float m_fMaxJointCmd = 1023; + float m_fMinJointAngle = -150.0f/180.0f*M_PI; + float m_fMaxJointAngle = 150.0f/180.0f*M_PI; + +}; \ No newline at end of file diff --git a/include/Kinematics.h b/include/Kinematics.h new file mode 100644 index 0000000..d465457 --- /dev/null +++ b/include/Kinematics.h @@ -0,0 +1,20 @@ +#define _USE_MATH_DEFINES +#include +#include +#include + +#include "opencv2/opencv.hpp" + +float deg2rad(float angle); + +float rad2deg(float angle); + +std::vector computeForwardKinematics(float q1, float q2, float L1, float L2); + +std::vector computeInverseKinematics(float x, float y, float L1, float L2); + +std::vector computeDifferentialKinematics(float q1, float q2, float L1, float L2); + +int computeJacobianMatrixRank(std::vector vJacobianMatrix, float threshold); + +cv::Mat computeInverseJacobianMatrix(std::vector vJacobianMatrix); diff --git a/lib/CameraCalibration.o b/lib/CameraCalibration.o new file mode 100644 index 0000000..3cf92c1 Binary files /dev/null and b/lib/CameraCalibration.o differ diff --git a/lib/DynamixelHandler.o b/lib/DynamixelHandler.o new file mode 100644 index 0000000..d0a886b Binary files /dev/null and b/lib/DynamixelHandler.o differ diff --git a/lib/Kinematics.o b/lib/Kinematics.o new file mode 100644 index 0000000..13400d5 Binary files /dev/null and b/lib/Kinematics.o differ diff --git a/lib/RobotServoing.o b/lib/RobotServoing.o new file mode 100644 index 0000000..a432b9c Binary files /dev/null and b/lib/RobotServoing.o differ diff --git a/makefile b/makefile new file mode 100644 index 0000000..1676b4e --- /dev/null +++ b/makefile @@ -0,0 +1,19 @@ +all: CameraCalibration Kinematics DynamixelHandler RobotServoing + +CameraCalibration: src/CameraCalibration.cpp + g++ -c src/CameraCalibration.cpp -o lib/CameraCalibration.o -I./include -I/usr/include/opencv4 + g++ lib/CameraCalibration.o -o bin/CameraCalibration -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4` + +Kinematics: src/Kinematics.cpp include/Kinematics.h + g++ -c src/Kinematics.cpp -o lib/Kinematics.o -I./include -I/usr/include/opencv4 + +DynamixelHandler: src/DynamixelHandler.cpp include/DynamixelHandler.h + g++ -c src/DynamixelHandler.cpp -o lib/DynamixelHandler.o -I./include -I/usr/local/include + +RobotServoing: src/RobotServoing.cpp + g++ -c src/RobotServoing.cpp -o lib/RobotServoing.o -I./include -I/usr/include/opencv4 + g++ -o bin/RobotServoing lib/RobotServoing.o lib/Kinematics.o lib/DynamixelHandler.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4` + +clean: + rm -f lib/*.o + rm -f bin/* diff --git a/src/CameraCalibration.cpp b/src/CameraCalibration.cpp new file mode 100644 index 0000000..8675465 --- /dev/null +++ b/src/CameraCalibration.cpp @@ -0,0 +1,769 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace cv; +using namespace std; + +class Settings +{ + public: + Settings() : goodInput(false) {} + enum Pattern { NOT_EXISTING, CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID }; + enum InputType { INVALID, CAMERA, VIDEO_FILE, IMAGE_LIST }; + + void write(FileStorage& fs) const //Write serialization for this class + { + fs << "{" + << "BoardSize_Width" << boardSize.width + << "BoardSize_Height" << boardSize.height + << "Square_Size" << squareSize + << "Calibrate_Pattern" << patternToUse + << "Calibrate_NrOfFrameToUse" << nrFrames + << "Calibrate_FixAspectRatio" << aspectRatio + << "Calibrate_AssumeZeroTangentialDistortion" << calibZeroTangentDist + << "Calibrate_FixPrincipalPointAtTheCenter" << calibFixPrincipalPoint + + << "Write_DetectedFeaturePoints" << writePoints + << "Write_extrinsicParameters" << writeExtrinsics + << "Write_gridPoints" << writeGrid + << "Write_outputFileName" << outputFileName + + << "Show_UndistortedImage" << showUndistorted + + << "Input_FlipAroundHorizontalAxis" << flipVertical + << "Input_Delay" << delay + << "Input" << input + << "}"; + } + + void read(const FileNode& node) //Read serialization for this class + { + node["BoardSize_Width" ] >> boardSize.width; + node["BoardSize_Height"] >> boardSize.height; + node["Calibrate_Pattern"] >> patternToUse; + node["Square_Size"] >> squareSize; + node["Calibrate_NrOfFrameToUse"] >> nrFrames; + node["Calibrate_FixAspectRatio"] >> aspectRatio; + node["Write_DetectedFeaturePoints"] >> writePoints; + node["Write_extrinsicParameters"] >> writeExtrinsics; + node["Write_gridPoints"] >> writeGrid; + node["Write_outputFileName"] >> outputFileName; + node["Calibrate_AssumeZeroTangentialDistortion"] >> calibZeroTangentDist; + node["Calibrate_FixPrincipalPointAtTheCenter"] >> calibFixPrincipalPoint; + node["Calibrate_UseFisheyeModel"] >> useFisheye; + node["Input_FlipAroundHorizontalAxis"] >> flipVertical; + node["Show_UndistortedImage"] >> showUndistorted; + node["Input"] >> input; + node["Input_Delay"] >> delay; + node["Fix_K1"] >> fixK1; + node["Fix_K2"] >> fixK2; + node["Fix_K3"] >> fixK3; + node["Fix_K4"] >> fixK4; + node["Fix_K5"] >> fixK5; + + validate(); + } + + void validate() + { + goodInput = true; + if (boardSize.width <= 0 || boardSize.height <= 0) + { + cerr << "Invalid Board size: " << boardSize.width << " " << boardSize.height << endl; + goodInput = false; + } + if (squareSize <= 10e-6) + { + cerr << "Invalid square size " << squareSize << endl; + goodInput = false; + } + if (nrFrames <= 0) + { + cerr << "Invalid number of frames " << nrFrames << endl; + goodInput = false; + } + + if (input.empty()) // Check for valid input + inputType = INVALID; + else + { + if (input[0] >= '0' && input[0] <= '9') + { + stringstream ss(input); + ss >> cameraID; + inputType = CAMERA; + } + else + { + if (isListOfImages(input) && readStringList(input, imageList)) + { + inputType = IMAGE_LIST; + nrFrames = (nrFrames < (int)imageList.size()) ? nrFrames : (int)imageList.size(); + } + else + inputType = VIDEO_FILE; + } + if (inputType == CAMERA) + inputCapture.open(cameraID, cv::CAP_V4L2); + if (inputType == VIDEO_FILE) + inputCapture.open(input); + if (inputType != IMAGE_LIST && !inputCapture.isOpened()) + inputType = INVALID; + } + if (inputType == INVALID) + { + cerr << " Input does not exist: " << input; + goodInput = false; + } + + flag = 0; + if(calibFixPrincipalPoint) flag |= CALIB_FIX_PRINCIPAL_POINT; + if(calibZeroTangentDist) flag |= CALIB_ZERO_TANGENT_DIST; + if(aspectRatio) flag |= CALIB_FIX_ASPECT_RATIO; + if(fixK1) flag |= CALIB_FIX_K1; + if(fixK2) flag |= CALIB_FIX_K2; + if(fixK3) flag |= CALIB_FIX_K3; + if(fixK4) flag |= CALIB_FIX_K4; + if(fixK5) flag |= CALIB_FIX_K5; + + if (useFisheye) + { + // the fisheye model has its own enum, so overwrite the flags + flag = fisheye::CALIB_FIX_SKEW | fisheye::CALIB_RECOMPUTE_EXTRINSIC; + if(fixK1) flag |= fisheye::CALIB_FIX_K1; + if(fixK2) flag |= fisheye::CALIB_FIX_K2; + if(fixK3) flag |= fisheye::CALIB_FIX_K3; + if(fixK4) flag |= fisheye::CALIB_FIX_K4; + if (calibFixPrincipalPoint) flag |= fisheye::CALIB_FIX_PRINCIPAL_POINT; + } + + calibrationPattern = NOT_EXISTING; + if (!patternToUse.compare("CHESSBOARD")) calibrationPattern = CHESSBOARD; + if (!patternToUse.compare("CIRCLES_GRID")) calibrationPattern = CIRCLES_GRID; + if (!patternToUse.compare("ASYMMETRIC_CIRCLES_GRID")) calibrationPattern = ASYMMETRIC_CIRCLES_GRID; + if (calibrationPattern == NOT_EXISTING) + { + cerr << " Camera calibration mode does not exist: " << patternToUse << endl; + goodInput = false; + } + atImageList = 0; + + } + + Mat nextImage() + { + Mat result; + if( inputCapture.isOpened() ) + { + Mat view0; + inputCapture >> view0; + view0.copyTo(result); + } + else if( atImageList < imageList.size() ) + result = imread(imageList[atImageList++], IMREAD_COLOR); + + return result; + } + + static bool readStringList( const string& filename, vector& l ) + { + l.clear(); + FileStorage fs(filename, FileStorage::READ); + if( !fs.isOpened() ) + return false; + FileNode n = fs.getFirstTopLevelNode(); + if( n.type() != FileNode::SEQ ) + return false; + FileNodeIterator it = n.begin(), it_end = n.end(); + for( ; it != it_end; ++it ) + l.push_back((string)*it); + return true; + } + + static bool isListOfImages( const string& filename) + { + string s(filename); + // Look for file extension + if( s.find(".xml") == string::npos && s.find(".yaml") == string::npos && s.find(".yml") == string::npos ) + return false; + else + return true; + } + + public: + Size boardSize; // The size of the board -> Number of items by width and height + Pattern calibrationPattern; // One of the Chessboard, circles, or asymmetric circle pattern + float squareSize; // The size of a square in your defined unit (point, millimeter,etc). + int nrFrames; // The number of frames to use from the input for calibration + float aspectRatio; // The aspect ratio + int delay; // In case of a video input + bool writePoints; // Write detected feature points + bool writeExtrinsics; // Write extrinsic parameters + bool writeGrid; // Write refined 3D target grid points + bool calibZeroTangentDist; // Assume zero tangential distortion + bool calibFixPrincipalPoint; // Fix the principal point at the center + bool flipVertical; // Flip the captured images around the horizontal axis + string outputFileName; // The name of the file where to write + bool showUndistorted; // Show undistorted images after calibration + string input; // The input -> + bool useFisheye; // use fisheye camera model for calibration + bool fixK1; // fix K1 distortion coefficient + bool fixK2; // fix K2 distortion coefficient + bool fixK3; // fix K3 distortion coefficient + bool fixK4; // fix K4 distortion coefficient + bool fixK5; // fix K5 distortion coefficient + + int cameraID; + vector imageList; + size_t atImageList; + VideoCapture inputCapture; + InputType inputType; + bool goodInput; + int flag; + + private: + string patternToUse; + + +}; + +static inline void read(const FileNode& node, Settings& x, const Settings& default_value = Settings()) +{ + if(node.empty()) + x = default_value; + else + x.read(node); +} + +enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 }; + +bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs, + vector > imagePoints, float grid_width, bool release_object); + +int main(int argc, char* argv[]) +{ + const String keys + = "{help h usage ? | | print this message }" + "{@settings |default.xml| input setting file }" + "{d | | actual distance between top-left and top-right corners of " + "the calibration grid }" + "{winSize | 11 | Half of search window for cornerSubPix }"; + CommandLineParser parser(argc, argv, keys); + parser.about("This is a camera calibration sample.\n" + "Usage: camera_calibration [configuration_file -- default ./default.xml]\n" + "Near the sample file you'll find the configuration file, which has detailed help of " + "how to edit it. It may be any OpenCV supported file format XML/YAML."); + if (!parser.check()) + { + parser.printErrors(); + return 0; + } + + if (parser.has("help")) + { + parser.printMessage(); + return 0; + } + + //! [file_read] + Settings s; + const string inputSettingsFile = parser.get(0); + FileStorage fs(inputSettingsFile, FileStorage::READ); // Read the settings + if (!fs.isOpened()) + { + cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << endl; + parser.printMessage(); + return -1; + } + fs["Settings"] >> s; + fs.release(); // close Settings file + //! [file_read] + + //FileStorage fout("settings.yml", FileStorage::WRITE); // write config as YAML + //fout << "Settings" << s; + + if (!s.goodInput) + { + cout << "Invalid input detected. Application stopping. " << endl; + return -1; + } + + int winSize = parser.get("winSize"); + + float grid_width = s.squareSize * (s.boardSize.width - 1); + bool release_object = false; + if (parser.has("d")) + { + grid_width = parser.get("d"); + release_object = true; + } + + vector > imagePoints; + Mat cameraMatrix, distCoeffs; + Size imageSize; + int mode = s.inputType == Settings::IMAGE_LIST ? CAPTURING : DETECTION; + clock_t prevTimestamp = 0; + const Scalar RED(0,0,255), GREEN(0,255,0); + const char ESC_KEY = 27; + + //! [get_input] + for(;;) + { + Mat view; + bool blinkOutput = false; + + view = s.nextImage(); + + //----- If no more image, or got enough, then stop calibration and show result ------------- + if( mode == CAPTURING && imagePoints.size() >= (size_t)s.nrFrames ) + { + if(runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints, grid_width, + release_object)) + mode = CALIBRATED; + else + mode = DETECTION; + } + if(view.empty()) // If there are no more images stop the loop + { + // if calibration threshold was not reached yet, calibrate now + if( mode != CALIBRATED && !imagePoints.empty() ) + runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints, grid_width, + release_object); + break; + } + //! [get_input] + + imageSize = view.size(); // Format input image. + if( s.flipVertical ) + flip( view, view, 0 ); + + //! [find_pattern] + vector pointBuf; + + bool found; + + int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE; + + if(!s.useFisheye) + { + // fast check erroneously fails with high distortions like fisheye + chessBoardFlags |= CALIB_CB_FAST_CHECK; + } + + switch( s.calibrationPattern ) // Find feature points on the input format + { + case Settings::CHESSBOARD: + found = findChessboardCorners( view, s.boardSize, pointBuf, chessBoardFlags); + break; + case Settings::CIRCLES_GRID: + found = findCirclesGrid( view, s.boardSize, pointBuf ); + break; + case Settings::ASYMMETRIC_CIRCLES_GRID: + found = findCirclesGrid( view, s.boardSize, pointBuf, CALIB_CB_ASYMMETRIC_GRID ); + break; + default: + found = false; + break; + } + + //! [find_pattern] + //! [pattern_found] + if ( found) // If done with success, + { + // improve the found corners' coordinate accuracy for chessboard + if( s.calibrationPattern == Settings::CHESSBOARD) + { + Mat viewGray; + cvtColor(view, viewGray, COLOR_BGR2GRAY); + cornerSubPix( viewGray, pointBuf, Size(winSize,winSize), + Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.0001 )); + } + + if( mode == CAPTURING && // For camera only take new samples after delay time + (!s.inputCapture.isOpened() || clock() - prevTimestamp > s.delay*1e-3*CLOCKS_PER_SEC) ) + { + imagePoints.push_back(pointBuf); + prevTimestamp = clock(); + blinkOutput = s.inputCapture.isOpened(); + } + + // Draw the corners. + drawChessboardCorners( view, s.boardSize, Mat(pointBuf), found ); + } + //! [pattern_found] + + //----------------------------- Output Text ------------------------------------------------ + //! [output_text] + string msg = (mode == CAPTURING) ? "100/100" : + mode == CALIBRATED ? "Calibrated" : "Press 'g' to start"; + int baseLine = 0; + Size textSize = getTextSize(msg, 1, 1, 1, &baseLine); + Point textOrigin(view.cols - 2*textSize.width - 10, view.rows - 2*baseLine - 10); + + if( mode == CAPTURING ) + { + if(s.showUndistorted) + msg = cv::format( "%d/%d Undist", (int)imagePoints.size(), s.nrFrames ); + else + msg = cv::format( "%d/%d", (int)imagePoints.size(), s.nrFrames ); + } + + putText( view, msg, textOrigin, 1, 1, mode == CALIBRATED ? GREEN : RED); + + if( blinkOutput ) + bitwise_not(view, view); + //! [output_text] + //------------------------- Video capture output undistorted ------------------------------ + //! [output_undistorted] + if( mode == CALIBRATED && s.showUndistorted ) + { + Mat temp = view.clone(); + if (s.useFisheye) + { + Mat newCamMat; + fisheye::estimateNewCameraMatrixForUndistortRectify(cameraMatrix, distCoeffs, imageSize, + Matx33d::eye(), newCamMat, 1); + cv::fisheye::undistortImage(temp, view, cameraMatrix, distCoeffs, newCamMat); + } + else + undistort(temp, view, cameraMatrix, distCoeffs); + } + //! [output_undistorted] + //------------------------------ Show image and check for input commands ------------------- + //! [await_input] + imshow("Image View", view); + char key = (char)waitKey(s.inputCapture.isOpened() ? 50 : s.delay); + + if( key == ESC_KEY ) + break; + + if( key == 'u' && mode == CALIBRATED ) + s.showUndistorted = !s.showUndistorted; + + if( s.inputCapture.isOpened() && key == 'g' ) + { + mode = CAPTURING; + imagePoints.clear(); + } + //! [await_input] + } + + // -----------------------Show the undistorted image for the image list ------------------------ + //! [show_results] + if( s.inputType == Settings::IMAGE_LIST && s.showUndistorted && !cameraMatrix.empty()) + { + Mat view, rview, map1, map2; + + if (s.useFisheye) + { + Mat newCamMat; + fisheye::estimateNewCameraMatrixForUndistortRectify(cameraMatrix, distCoeffs, imageSize, + Matx33d::eye(), newCamMat, 1); + fisheye::initUndistortRectifyMap(cameraMatrix, distCoeffs, Matx33d::eye(), newCamMat, imageSize, + CV_16SC2, map1, map2); + } + else + { + initUndistortRectifyMap( + cameraMatrix, distCoeffs, Mat(), + getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), imageSize, + CV_16SC2, map1, map2); + } + + for(size_t i = 0; i < s.imageList.size(); i++ ) + { + view = imread(s.imageList[i], IMREAD_COLOR); + if(view.empty()) + continue; + remap(view, rview, map1, map2, INTER_LINEAR); + imshow("Image View", rview); + char c = (char)waitKey(); + if( c == ESC_KEY || c == 'q' || c == 'Q' ) + break; + } + } + //! [show_results] + + return 0; +} + +//! [compute_errors] +static double computeReprojectionErrors( const vector >& objectPoints, + const vector >& imagePoints, + const vector& rvecs, const vector& tvecs, + const Mat& cameraMatrix , const Mat& distCoeffs, + vector& perViewErrors, bool fisheye) +{ + vector imagePoints2; + size_t totalPoints = 0; + double totalErr = 0, err; + perViewErrors.resize(objectPoints.size()); + + for(size_t i = 0; i < objectPoints.size(); ++i ) + { + if (fisheye) + { + fisheye::projectPoints(objectPoints[i], imagePoints2, rvecs[i], tvecs[i], cameraMatrix, + distCoeffs); + } + else + { + projectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix, distCoeffs, imagePoints2); + } + err = norm(imagePoints[i], imagePoints2, NORM_L2); + + size_t n = objectPoints[i].size(); + perViewErrors[i] = (float) std::sqrt(err*err/n); + totalErr += err*err; + totalPoints += n; + } + + return std::sqrt(totalErr/totalPoints); +} +//! [compute_errors] +//! [board_corners] +static void calcBoardCornerPositions(Size boardSize, float squareSize, vector& corners, + Settings::Pattern patternType /*= Settings::CHESSBOARD*/) +{ + corners.clear(); + + switch(patternType) + { + case Settings::CHESSBOARD: + case Settings::CIRCLES_GRID: + for( int i = 0; i < boardSize.height; ++i ) + for( int j = 0; j < boardSize.width; ++j ) + corners.push_back(Point3f(j*squareSize, i*squareSize, 0)); + break; + + case Settings::ASYMMETRIC_CIRCLES_GRID: + for( int i = 0; i < boardSize.height; i++ ) + for( int j = 0; j < boardSize.width; j++ ) + corners.push_back(Point3f((2*j + i % 2)*squareSize, i*squareSize, 0)); + break; + default: + break; + } +} +//! [board_corners] +static bool runCalibration( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs, + vector > imagePoints, vector& rvecs, vector& tvecs, + vector& reprojErrs, double& totalAvgErr, vector& newObjPoints, + float grid_width, bool release_object) +{ + //! [fixed_aspect] + cameraMatrix = Mat::eye(3, 3, CV_64F); + if( !s.useFisheye && s.flag & CALIB_FIX_ASPECT_RATIO ) + cameraMatrix.at(0,0) = s.aspectRatio; + //! [fixed_aspect] + if (s.useFisheye) + { + distCoeffs = Mat::zeros(4, 1, CV_64F); + } else + { + distCoeffs = Mat::zeros(8, 1, CV_64F); + } + + vector > objectPoints(1); + calcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0], s.calibrationPattern); + objectPoints[0][s.boardSize.width - 1].x = objectPoints[0][0].x + grid_width; + newObjPoints = objectPoints[0]; + + objectPoints.resize(imagePoints.size(),objectPoints[0]); + + //Find intrinsic and extrinsic camera parameters + double rms; + + if (s.useFisheye) + { + Mat _rvecs, _tvecs; + rms = fisheye::calibrate(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, _rvecs, + _tvecs, s.flag); + + rvecs.reserve(_rvecs.rows); + tvecs.reserve(_tvecs.rows); + for(int i = 0; i < int(objectPoints.size()); i++) + { + rvecs.push_back(_rvecs.row(i)); + tvecs.push_back(_tvecs.row(i)); + } + } else + { + int iFixedPoint = -1; + if (release_object) + iFixedPoint = s.boardSize.width - 1; + rms = calibrateCameraRO(objectPoints, imagePoints, imageSize, iFixedPoint, + cameraMatrix, distCoeffs, rvecs, tvecs, newObjPoints, + s.flag | CALIB_USE_LU); + } + + if (release_object) + { + cout << "New board corners: " << endl; + cout << newObjPoints[0] << endl; + cout << newObjPoints[s.boardSize.width - 1] << endl; + cout << newObjPoints[s.boardSize.width * (s.boardSize.height - 1)] << endl; + cout << newObjPoints.back() << endl; + } + + cout << "Re-projection error reported by calibrateCamera: "<< rms << endl; + + bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs); + + objectPoints.clear(); + objectPoints.resize(imagePoints.size(), newObjPoints); + totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, rvecs, tvecs, cameraMatrix, + distCoeffs, reprojErrs, s.useFisheye); + + return ok; +} + +// Print camera parameters to the output file +static void saveCameraParams( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs, + const vector& rvecs, const vector& tvecs, + const vector& reprojErrs, const vector >& imagePoints, + double totalAvgErr, const vector& newObjPoints ) +{ + FileStorage fs( s.outputFileName, FileStorage::WRITE ); + + time_t tm; + time( &tm ); + struct tm *t2 = localtime( &tm ); + char buf[1024]; + strftime( buf, sizeof(buf), "%c", t2 ); + + fs << "calibration_time" << buf; + + if( !rvecs.empty() || !reprojErrs.empty() ) + fs << "nr_of_frames" << (int)std::max(rvecs.size(), reprojErrs.size()); + fs << "image_width" << imageSize.width; + fs << "image_height" << imageSize.height; + fs << "board_width" << s.boardSize.width; + fs << "board_height" << s.boardSize.height; + fs << "square_size" << s.squareSize; + + if( !s.useFisheye && s.flag & CALIB_FIX_ASPECT_RATIO ) + fs << "fix_aspect_ratio" << s.aspectRatio; + + if (s.flag) + { + std::stringstream flagsStringStream; + if (s.useFisheye) + { + flagsStringStream << "flags:" + << (s.flag & fisheye::CALIB_FIX_SKEW ? " +fix_skew" : "") + << (s.flag & fisheye::CALIB_FIX_K1 ? " +fix_k1" : "") + << (s.flag & fisheye::CALIB_FIX_K2 ? " +fix_k2" : "") + << (s.flag & fisheye::CALIB_FIX_K3 ? " +fix_k3" : "") + << (s.flag & fisheye::CALIB_FIX_K4 ? " +fix_k4" : "") + << (s.flag & fisheye::CALIB_RECOMPUTE_EXTRINSIC ? " +recompute_extrinsic" : ""); + } + else + { + flagsStringStream << "flags:" + << (s.flag & CALIB_USE_INTRINSIC_GUESS ? " +use_intrinsic_guess" : "") + << (s.flag & CALIB_FIX_ASPECT_RATIO ? " +fix_aspectRatio" : "") + << (s.flag & CALIB_FIX_PRINCIPAL_POINT ? " +fix_principal_point" : "") + << (s.flag & CALIB_ZERO_TANGENT_DIST ? " +zero_tangent_dist" : "") + << (s.flag & CALIB_FIX_K1 ? " +fix_k1" : "") + << (s.flag & CALIB_FIX_K2 ? " +fix_k2" : "") + << (s.flag & CALIB_FIX_K3 ? " +fix_k3" : "") + << (s.flag & CALIB_FIX_K4 ? " +fix_k4" : "") + << (s.flag & CALIB_FIX_K5 ? " +fix_k5" : ""); + } + fs.writeComment(flagsStringStream.str()); + } + + fs << "flags" << s.flag; + + fs << "fisheye_model" << s.useFisheye; + + fs << "camera_matrix" << cameraMatrix; + fs << "distortion_coefficients" << distCoeffs; + + fs << "avg_reprojection_error" << totalAvgErr; + if (s.writeExtrinsics && !reprojErrs.empty()) + fs << "per_view_reprojection_errors" << Mat(reprojErrs); + + if(s.writeExtrinsics && !rvecs.empty() && !tvecs.empty() ) + { + CV_Assert(rvecs[0].type() == tvecs[0].type()); + Mat bigmat((int)rvecs.size(), 6, CV_MAKETYPE(rvecs[0].type(), 1)); + bool needReshapeR = rvecs[0].depth() != 1 ? true : false; + bool needReshapeT = tvecs[0].depth() != 1 ? true : false; + + for( size_t i = 0; i < rvecs.size(); i++ ) + { + Mat r = bigmat(Range(int(i), int(i+1)), Range(0,3)); + Mat t = bigmat(Range(int(i), int(i+1)), Range(3,6)); + + if(needReshapeR) + rvecs[i].reshape(1, 1).copyTo(r); + else + { + //*.t() is MatExpr (not Mat) so we can use assignment operator + CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1); + r = rvecs[i].t(); + } + + if(needReshapeT) + tvecs[i].reshape(1, 1).copyTo(t); + else + { + CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1); + t = tvecs[i].t(); + } + } + fs.writeComment("a set of 6-tuples (rotation vector + translation vector) for each view"); + fs << "extrinsic_parameters" << bigmat; + } + + if(s.writePoints && !imagePoints.empty() ) + { + Mat imagePtMat((int)imagePoints.size(), (int)imagePoints[0].size(), CV_32FC2); + for( size_t i = 0; i < imagePoints.size(); i++ ) + { + Mat r = imagePtMat.row(int(i)).reshape(2, imagePtMat.cols); + Mat imgpti(imagePoints[i]); + imgpti.copyTo(r); + } + fs << "image_points" << imagePtMat; + } + + if( s.writeGrid && !newObjPoints.empty() ) + { + fs << "grid_points" << newObjPoints; + } +} + +//! [run_and_save] +bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs, + vector > imagePoints, float grid_width, bool release_object) +{ + vector rvecs, tvecs; + vector reprojErrs; + double totalAvgErr = 0; + vector newObjPoints; + + bool ok = runCalibration(s, imageSize, cameraMatrix, distCoeffs, imagePoints, rvecs, tvecs, reprojErrs, + totalAvgErr, newObjPoints, grid_width, release_object); + cout << (ok ? "Calibration succeeded" : "Calibration failed") + << ". avg re projection error = " << totalAvgErr << endl; + + if (ok) + saveCameraParams(s, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, reprojErrs, imagePoints, + totalAvgErr, newObjPoints); + return ok; +} +//! [run_and_save] diff --git a/src/DynamixelHandler.cpp b/src/DynamixelHandler.cpp new file mode 100644 index 0000000..372d381 --- /dev/null +++ b/src/DynamixelHandler.cpp @@ -0,0 +1,407 @@ +#include "DynamixelHandler.h" + +DynamixelHandler::DynamixelHandler(): + m_sDeviceName(""), m_fProtocolVersion(0.0), m_i32BaudRate(0), + m_pPacketHandler(nullptr), m_pPortHandler(nullptr), + m_bIsDeviceNameSet(false), m_bIsProtocolVersionSet(false), m_bIsPortOpened(false), m_bIsBaudRateSet(false), + m_ui8DxlError(0), m_i32DxlCommunicationResult(COMM_TX_FAIL) +{ + +} + +DynamixelHandler::~DynamixelHandler() +{ + +} + +int DynamixelHandler::convertJointVelocityToJointCmd(float fJointVelocity) +{ + if (fJointVelocity == 0.0f) + return 0; + + float a = 0.0f; + float b = 0.0f; + if (fJointVelocity>0) + { + float l_fMaxJointCmd = 1023; + float l_fMinJointCmd = 0; + float l_fMaxJointVelocity = 114 * 60.0f * 2 * M_PI; + float l_fMinJointAngle = 0.0f; + // y = ax + b + a = (l_fMaxJointCmd-l_fMinJointCmd) / (l_fMaxJointVelocity - l_fMinJointAngle); + b = l_fMinJointCmd - a * l_fMinJointAngle; + } + + if (fJointVelocity<0) + { + float l_fMaxJointCmd = 2047; + float l_fMinJointCmd = 1024; + float l_fMaxJointVelocity = 0.0f; + float l_fMinJointAngle = -114 * 60.0f * 2 * M_PI; + // y = ax + b + a = (l_fMaxJointCmd-l_fMinJointCmd) / (l_fMaxJointVelocity - l_fMinJointAngle); + b = l_fMinJointCmd - a * l_fMinJointAngle; + } + + float jointCmd = a * fJointVelocity + b; + return (int)jointCmd; +} + +int DynamixelHandler::convertAngleToJointCmd(float fJointAngle) +{ + // y = ax + b + float a = (m_fMaxJointCmd-m_fMinJointCmd) / (m_fMaxJointAngle - m_fMinJointAngle); + float b = m_fMinJointCmd - a * m_fMinJointAngle; + float jointCmd = a * fJointAngle + b; + return (int)jointCmd; +} + +float DynamixelHandler::convertJointCmdToAngle(int iJointCmd) +{ + // y = ax + b + float a = (m_fMaxJointAngle - m_fMinJointAngle) / (m_fMaxJointCmd-m_fMinJointCmd); + float b = m_fMinJointAngle - a * m_fMinJointCmd; + float jointAngle = a * iJointCmd + b; + return jointAngle; +} + +bool DynamixelHandler::openPort() +{ + if (m_pPortHandler == nullptr) + { + std::cout << "[ERROR](DynamixelHandler::openPort) m_pPortHandler is null!" << std::endl; + m_bIsPortOpened = false; + return m_bIsPortOpened; + } + + if (!m_bIsDeviceNameSet) + { + std::cout << "[ERROR](DynamixelHandler::openPort) m_sDeviceName is not set!" << std::endl; + m_bIsPortOpened = false; + return m_bIsPortOpened; + } + + if (m_bIsPortOpened) + { + std::cout << "[WARNING](DynamixelHandler::openPort) port is already opened!" << std::endl; + return m_bIsPortOpened; + } + + if (m_pPortHandler->openPort()) + { + std::cout << "[INFO](DynamixelHandler::openPort) Succeeded to open the port!" << std::endl; + m_bIsPortOpened = true; + } + else + { + std::cout << "[ERROR](DynamixelHandler::openPort) Failed to open the port!" << std::endl; + m_bIsPortOpened = false; + } + return m_bIsPortOpened; +} + +void DynamixelHandler::closePort() +{ + if (m_pPortHandler == nullptr) + { + std::cout << "[ERROR](DynamixelHandler::closePort) m_pPortHandler is null!" << std::endl; + m_bIsPortOpened = false; + return; + } + + if (!m_bIsPortOpened) + { + std::cout << "[WARNING](DynamixelHandler::openPort) port is already closed!" << std::endl; + return; + } + + m_pPortHandler->closePort(); + + std::cout << "[INFO](DynamixelHandler::closePort) Succeeded to close the port!" << std::endl; + m_bIsPortOpened = false; +} + +bool DynamixelHandler::setBaudRate(int i32BaudRate) +{ + m_i32BaudRate = i32BaudRate; + + if (nullptr != m_pPortHandler) + { + if (m_pPortHandler->setBaudRate(m_i32BaudRate)) + { + std::cout << "[INFO](DynamixelHandler::setBaudRate) Succeeded to change the baudrate!" << std::endl; + m_bIsBaudRateSet = true; + } + else + { + std::cout << "[ERROR](DynamixelHandler::setBaudRate) Failed to change the baudrate!" << std::endl; + m_bIsBaudRateSet = false; + } + } + else + { + std::cout << "[ERROR](DynamixelHandler::setBaudRate) m_pPortHandler is null!" << std::endl; + m_bIsBaudRateSet = false; + } + return m_bIsBaudRateSet; +} + +void DynamixelHandler::setDeviceName(std::string sDeviceName) +{ + m_sDeviceName = sDeviceName; + m_bIsDeviceNameSet = true; + + if (nullptr != m_pPortHandler) + { + delete m_pPortHandler; + m_pPortHandler = nullptr; + } + + // Initialize PortHandler instance + m_pPortHandler = dynamixel::PortHandler::getPortHandler(m_sDeviceName.c_str()); +} + +void DynamixelHandler::setProtocolVersion(float fProtocolVersion) +{ + m_fProtocolVersion = fProtocolVersion; + m_bIsProtocolVersionSet = true; + + if (nullptr != m_pPacketHandler) + { + delete m_pPacketHandler; + m_pPacketHandler = nullptr; + } + + m_pPacketHandler = dynamixel::PacketHandler::getPacketHandler(m_fProtocolVersion); +} + +bool DynamixelHandler::readCurrentJointPosition(std::vector& vCurrentJointPosition) +{ + // Creates a vector of joint position + std::vector l_vCurrentJointPosition; + // Reads the current joint positions in motor command unit + bool bIsReadSuccessfull = this->readCurrentJointPosition(l_vCurrentJointPosition); + //std::cout << "l_vCurrentJointPosition= " << l_vCurrentJointPosition[0] << ", " << l_vCurrentJointPosition[1] << ", " << l_vCurrentJointPosition[2]<< std::endl; + + // q1 + vCurrentJointPosition.push_back(ROT_DIRECTION_Q1*convertJointCmdToAngle(l_vCurrentJointPosition[0])); + // qpen + vCurrentJointPosition.push_back(ROT_DIRECTION_QPEN*convertJointCmdToAngle(l_vCurrentJointPosition[1])); + // q2 + vCurrentJointPosition.push_back(ROT_DIRECTION_Q2*convertJointCmdToAngle(l_vCurrentJointPosition[2])); + + //std::cout << "vCurrentJointPosition= " << vCurrentJointPosition[0] << ", " << vCurrentJointPosition[1] << ", " << vCurrentJointPosition[2]<< std::endl; + + return bIsReadSuccessfull; +} + +bool DynamixelHandler::readCurrentJointPosition(std::vector& vCurrentJointPosition) +{ + bool bIsReadSuccessfull = false; + + for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++) + { + int dxl_comm_result = COMM_TX_FAIL; // Communication result + uint8_t dxl_error = 0; + uint16_t dxl_present_position = 0; + + dxl_comm_result = m_pPacketHandler->read2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_PRESENT_POSITION, &dxl_present_position, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + //std::cout << "[ERROR] " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl; + bIsReadSuccessfull = false; + } + else if (dxl_error != 0) + { + //std::cout << "[ERROR] " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl; + bIsReadSuccessfull = false; + } + else + { + vCurrentJointPosition.push_back(dxl_present_position); + bIsReadSuccessfull = true; + } + } + + return bIsReadSuccessfull; +} + +bool DynamixelHandler::sendTargetJointPosition(std::vector& vTargetJointPosition) +{ + // Checks if the input vector has the right size + if (vTargetJointPosition.size() != NB_JOINTS) + { + std::cout << "[ERROR] (sendTargetJointPosition) Input vector has not the right size!" << std::endl; + return false; + } + + // Creates a vector of motor commands + std::vector l_vTargetJointPosition; + // q1 + l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q1*vTargetJointPosition[0])); + // qpen + l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_QPEN*vTargetJointPosition[1])); + // q2 + l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q2*vTargetJointPosition[2])); + + //std::cout << "l_vTargetJointPosition= " << l_vTargetJointPosition[0] << ", " << l_vTargetJointPosition[1] << ", " << l_vTargetJointPosition[2]<< std::endl; + + // call the dxl sendTargetJointPosition + bool bIsSendSuccessfull = this->sendTargetJointPosition(l_vTargetJointPosition); + + return bIsSendSuccessfull; +} + +bool DynamixelHandler::sendTargetJointPosition(std::vector& vTargetJointPosition) +{ + bool bIsSendSuccessfull = false; + + // checks if the vector size is correct + if (vTargetJointPosition.size() != NB_JOINTS) + { + std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): Size of command vector is not correct: " << vTargetJointPosition.size() << " instead of " << NB_JOINTS << "!" << std::endl; + bIsSendSuccessfull = false; + } + + for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++) + { + int dxl_comm_result = COMM_TX_FAIL; // Communication result + uint8_t dxl_error = 0; + uint16_t dxl_present_position = 0; + dxl_comm_result = m_pPacketHandler->write2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_GOAL_POSITION, vTargetJointPosition[l_joint], &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + //std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl; + bIsSendSuccessfull = false; + } + else if (dxl_error != 0) + { + //std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl; + bIsSendSuccessfull = false; + } + else + { + bIsSendSuccessfull = true; + } + } + return bIsSendSuccessfull; + +} + +bool DynamixelHandler::sendTargetJointVelocity(std::vector& vTargetJointVelocity) +{ + // Checks if the input vector has the right size + if (vTargetJointVelocity.size() != NB_JOINTS) + { + std::cout << "[ERROR] (sendTargetJointVelocity) Input vector has not the right size!" << std::endl; + return false; + } + + // Creates a vector of motor commands + std::vector l_vTargetJointVelocity; + // q1 + l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q1*vTargetJointVelocity[0])); + // qpen + l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_QPEN*vTargetJointVelocity[1])); + // q2 + l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q2*vTargetJointVelocity[2])); + + std::cout << "l_vTargetJointVelocity= " << l_vTargetJointVelocity[0] << ", " << l_vTargetJointVelocity[1] << ", " << l_vTargetJointVelocity[2]<< std::endl; + + // call the dxl sendTargetJointPosition + bool bIsSendSuccessfull = this->sendTargetJointVelocity(l_vTargetJointVelocity); + + return bIsSendSuccessfull; +} + +bool DynamixelHandler::sendTargetJointVelocity(std::vector& vTargetJointVelocity) +{ + bool bIsSendSuccessfull = false; + + // checks if the vector size is correct + if (vTargetJointVelocity.size() != NB_JOINTS) + { + std::cout << "[ERROR] (DynamixelHandler::sendTargetJointVelocity): Size of command vector is not correct: " << vTargetJointVelocity.size() << " instead of " << NB_JOINTS << "!" << std::endl; + bIsSendSuccessfull = false; + } + + for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++) + { + int dxl_comm_result = COMM_TX_FAIL; // Communication result + uint8_t dxl_error = 0; + uint16_t dxl_present_position = 0; + dxl_comm_result = m_pPacketHandler->write2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_GOAL_VELOCITY, vTargetJointVelocity[l_joint], &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + //std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl; + bIsSendSuccessfull = false; + } + else if (dxl_error != 0) + { + //std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl; + bIsSendSuccessfull = false; + } + else + { + bIsSendSuccessfull = true; + } + } + return bIsSendSuccessfull; + +} + +bool DynamixelHandler::enableTorque(bool bEnableTorque) +{ + bool bIsSendSuccessfull = false; + + for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++) + { + int dxl_comm_result = COMM_TX_FAIL; // Communication result + uint8_t dxl_error = 0; + + dxl_comm_result = m_pPacketHandler->write1ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_TORQUE_ENABLE, bEnableTorque, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + //std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl; + bIsSendSuccessfull = false; + } + else if (dxl_error != 0) + { + //std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl; + bIsSendSuccessfull = false; + } + else + { + bIsSendSuccessfull = true; + } + } + return bIsSendSuccessfull; +} + +bool DynamixelHandler::setControlMode(int iControlMode) +{ + bool bIsSendSuccessfull = false; + + for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++) + { + int dxl_comm_result = COMM_TX_FAIL; // Communication result + uint8_t dxl_error = 0; + + dxl_comm_result = m_pPacketHandler->write1ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_CONTROL_MODE, iControlMode, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + //std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl; + bIsSendSuccessfull = false; + } + else if (dxl_error != 0) + { + //std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl; + bIsSendSuccessfull = false; + } + else + { + bIsSendSuccessfull = true; + } + } + return bIsSendSuccessfull; +} diff --git a/src/Kinematics.cpp b/src/Kinematics.cpp new file mode 100644 index 0000000..9c154fc --- /dev/null +++ b/src/Kinematics.cpp @@ -0,0 +1,161 @@ +#include "Kinematics.h" + + +float deg2rad(float angle) +{ + return angle/180.0*M_PI; +} + +float rad2deg(float angle) +{ + return angle*180.0/M_PI; +} + +std::vector computeForwardKinematics(float q1, float q2, float L1, float L2) +{ + float x = L1 * cos(q1) + L2 * cos(q1+q2); + float y = L1 * sin(q1) + L2 * sin(q1+q2); + std::cout << "[INFO] Forward Kinematics : (q1, q2)->(x, y) = (" << rad2deg(q1) << ", " << rad2deg(q2) << ")->(" << x << ", " << y << ")" << std::endl; + std::vector X; + X.push_back(x); + X.push_back(y); + + return X; +} + +std::vector computeInverseKinematics(float x, float y, float L1, float L2) +{ + std::vector qi; + + float cos_q2 = (x*x+y*y-(L1*L1+L2*L2)) / (2.0 * L1 * L2); + + std::cout << "[INFO] cos_q2= " << cos_q2 << std::endl; + + if (cos_q2 >1 | cos_q2 <-1) + { + qi.push_back(0.0); + std::cout << "[INFO] Inverse Kinematics: No solution!" << std::endl; + } + else if (cos_q2 == 1) + { + qi.push_back(1.0); + float q1 = atan2(y, x); + float q2 = 0; + std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl; + qi.push_back(q1); + qi.push_back(q2); + } + else if (cos_q2 == -1) + { + qi.push_back(1.0); + float q1 = atan2(y, x); + float q2 = M_PI; + std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl; + qi.push_back(q1); + qi.push_back(q2); + } + else + { + qi.push_back(2.0); + std::cout << "[INFO] Inverse Kinematics: Two solutions: "<< std::endl; + + float q2 = acos(cos_q2); + float q1 = (float)(atan2(y, x) - atan2(L2*sin(q2), L1+L2*cos_q2)); + std::cout << "\t(x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl; + qi.push_back(q1); + qi.push_back(q2); + + + q2 = -acos(cos_q2); + q1 = (float)(atan2(y, x) - atan2(L2*sin(q2), L1+L2*cos_q2)); + + std::cout << "\t(x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl; + qi.push_back(q1); + qi.push_back(q2); + } + + return qi; +} + +std::vector computeDifferentialKinematics(float q1, float q2, float L1, float L2) +{ + std::vector jacobian; + + float j11 = -L2*sin(q1+q2) - L1*sin(q1); + float j12 = -L2*sin(q1+q2); + float j21 = L2*cos(q1+q2) + L1*cos(q1); + float j22 = L2*cos(q1+q2); + + jacobian.push_back(j11); + jacobian.push_back(j12); + jacobian.push_back(j21); + jacobian.push_back(j22); + + return jacobian; +} + +int computeJacobianMatrixRank(std::vector vJacobianMatrix, float threshold) +{ + int rank = -1; + cv::Mat1f oJacobianMatrix(2, 2); + + if (vJacobianMatrix.size() == 4) + { + // Converts the Jacobian matrix from std::vector to cv::Mat + oJacobianMatrix.at(0, 0) = vJacobianMatrix[0]; + oJacobianMatrix.at(0, 1) = vJacobianMatrix[1]; + oJacobianMatrix.at(1, 0) = vJacobianMatrix[2]; + oJacobianMatrix.at(1, 1) = vJacobianMatrix[3]; + std::cout << "=====Jacobian Matrix=====" << std::endl; + std::cout << "[ " << oJacobianMatrix.at(0,0) << ", " << oJacobianMatrix.at(0,1) << " ]" << std::endl; + std::cout << "[ " << oJacobianMatrix.at(1,0) << ", " << oJacobianMatrix.at(1,1) << " ]" << std::endl; + // Computes the determinant of the Jacobian matrix + float determinant = abs(vJacobianMatrix[0] * vJacobianMatrix[3] - vJacobianMatrix[1]*vJacobianMatrix[2]); + std::cout << "=====Determinant of the Jacobian matrix=====" << std::endl << determinant << std::endl; + // Computes SVD + cv::Mat1f w, u, vt; + cv::SVD::compute(oJacobianMatrix, w, u, vt); + // Finds non zero singular values + cv::Mat1f nonZeroSingularValues = w/w.at(0,0) > threshold; + // Counts the number of non zero singular values + rank = cv::countNonZero(nonZeroSingularValues); + std::cout << "=====Rank of the Jacobian matrix=====" << std::endl << rank << " / " << oJacobianMatrix.rows << std::endl; + // Determines the inverse of the Jacobian matrix + cv::Mat oJacobianInverse = oJacobianMatrix.inv(); + std::cout << "=====Inverse of the Jacobian Matrix=====" << std::endl; + std::cout << "[ " << oJacobianInverse.at(0,0) << ", " << oJacobianInverse.at(0,1) << " ]" << std::endl; + std::cout << "[ " << oJacobianInverse.at(1,0) << ", " << oJacobianInverse.at(1,1) << " ]" << std::endl; + } + else + std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl; + + return rank; +} + +cv::Mat computeInverseJacobianMatrix(std::vector vJacobianMatrix) +{ + cv::Mat1f oJacobianMatrix(2, 2); + cv::Mat oJacobianInverse; + + if (vJacobianMatrix.size() == 4) + { + // Converts the Jacobian matrix from std::vector to cv::Mat + oJacobianMatrix.at(0, 0) = vJacobianMatrix[0]; + oJacobianMatrix.at(0, 1) = vJacobianMatrix[1]; + oJacobianMatrix.at(1, 0) = vJacobianMatrix[2]; + oJacobianMatrix.at(1, 1) = vJacobianMatrix[3]; + std::cout << "=====Jacobian Matrix=====" << std::endl; + std::cout << "[ " << oJacobianMatrix.at(0,0) << ", " << oJacobianMatrix.at(0,1) << " ]" << std::endl; + std::cout << "[ " << oJacobianMatrix.at(1,0) << ", " << oJacobianMatrix.at(1,1) << " ]" << std::endl; + // Determines the inverse of the Jacobian matrix + cv::invert(oJacobianMatrix, oJacobianInverse, cv::DECOMP_SVD); + //oJacobianInverse = oJacobianMatrix.inv(); + std::cout << "=====Inverse of the Jacobian Matrix=====" << std::endl; + std::cout << "[ " << oJacobianInverse.at(0,0) << ", " << oJacobianInverse.at(0,1) << " ]" << std::endl; + std::cout << "[ " << oJacobianInverse.at(1,0) << ", " << oJacobianInverse.at(1,1) << " ]" << std::endl; + } + else + std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl; + + return oJacobianInverse; +} \ No newline at end of file diff --git a/src/RedBallDetection.cpp b/src/RedBallDetection.cpp new file mode 100644 index 0000000..32e5f6a --- /dev/null +++ b/src/RedBallDetection.cpp @@ -0,0 +1,199 @@ +#include +#include +#include +#include +#include + +#include +#include +#include + +#define CAM_PARAMS_FILENAME "./data/microsoft_livecam_hd3000.xml" +#define COLOR_PARAMS_FILENAME "./data/color_params_data.xml" +#define FPS 30.0 +#define STRUCTURAL_ELEMENTS_SIZE 5 + +bool readCameraParameters(std::string filename, cv::Mat &camMatrix, cv::Mat & distCoeffs) +{ + cv::FileStorage fs(filename, cv::FileStorage::READ); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open the camera parameter file storage: " << filename << " !"<< std::endl; + return false; + } + + fs["camera_matrix"] >> camMatrix; + fs["distortion_coefficients"] >> distCoeffs; + + return true; +} + +bool writeColorParameters(std::string filename, int iLowH, int iHighH, int iLowS, int iHighS, int iLowV, int iHighV) +{ + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open the file storage: " << filename << " !"<< std::endl; + return false; + } + fs << "lowH" << iLowH; + fs << "highH" << iHighH; + + fs << "lowS" << iLowS; + fs << "highS" << iHighS; + + fs << "lowV" << iLowV; + fs << "highV" << iHighV; + + // releases the writer + fs.release(); + + return true; +} + +int main(int argc, char** argv) +{ + // initializes main parameters + std::string sCameraParamFilename = CAM_PARAMS_FILENAME; + std::string sColorParamFilename = COLOR_PARAMS_FILENAME; + int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE; + float fFPS = FPS; + + // updates main parameters from arguments + int opt; + while ((opt = getopt (argc, argv, ":i:f:o:s:")) != -1) + { + switch (opt) + { + case 'o': + sColorParamFilename = optarg; + break; + case 'f': + fFPS = atof(optarg); + break; + case 'i': + sCameraParamFilename = optarg; + break; + case 's': + iStructuralElementSize = atoi(optarg); + break; + case '?': + if (optopt == 'o' || optopt == 'f' || optopt == 'i' || optopt == 's') + fprintf (stderr, "Option -%c requires an argument.\n", optopt); + else if (isprint (optopt)) + fprintf (stderr, "Unknown option `-%c'.\n", optopt); + else + fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt); + return 1; + default: + abort (); + } + } + + // distorted/undistorted image + bool bIsImageUndistorted = true; + + // reads camera intrinsic parameters + cv::Mat cameraMatrix, distCoeffs; + bool isCamParamsSet = readCameraParameters(sCameraParamFilename, cameraMatrix, distCoeffs); + + // checks if the camera parameters were successfully read + if (!isCamParamsSet) + { + std::cout << "[WARNING] Camera intrinsic parameters could not be loaded!" << std::endl; + } + + // creates a camera grabber + cv::VideoCapture cap(0, cv::CAP_V4L2); + + // checks if the camera was successfully opened + if (!cap.isOpened()) + { + std::cout << "[ERROR] Cannot open the webcam" << std::endl; + return 1; + } + + cv::namedWindow("Control", cv::WINDOW_AUTOSIZE); //create a window called "Control" + + // sets min/max value for HSV color representation + int iLowH = 0; + int iHighH = 179; + + int iLowS = 0; + int iHighS = 255; + + int iLowV = 0; + int iHighV = 255; + + // creates trackbars in "Control" window + cv::createTrackbar("LowH", "Control", &iLowH, 179); //Hue (0 - 179) + cv::createTrackbar("HighH", "Control", &iHighH, 179); + + cv::createTrackbar("LowS", "Control", &iLowS, 255); //Saturation (0 - 255) + cv::createTrackbar("HighS", "Control", &iHighS, 255); + + cv::createTrackbar("LowV", "Control", &iLowV, 255); //Value (0 - 255) + cv::createTrackbar("HighV", "Control", &iHighV, 255); + + while (true) + { + cv::Mat imgOriginal; + + bool bSuccess = cap.read(imgOriginal); // read a new frame from video + + if (!bSuccess) //if not success, break loop + { + std::cout << "[WARNING] Cannot read a frame from video stream" << std::endl; + break; + } + + if (bIsImageUndistorted && isCamParamsSet) + { + cv::Mat temp = imgOriginal.clone(); + cv::undistort(temp, imgOriginal, cameraMatrix, distCoeffs); + } + + //Convert the captured frame from BGR to HSV + cv::Mat imgHSV; + cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV); + + //Threshold the image based on the trackbar values + cv::Mat imgThresholded; + inRange(imgHSV, cv::Scalar(iLowH, iLowS, iLowV), cv::Scalar(iHighH, iHighS, iHighV), imgThresholded); + + //morphological opening (remove small objects from the foreground) + cv::erode(imgThresholded, imgThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) ); + cv::dilate( imgThresholded, imgThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) ); + + //morphological closing (fill small holes in the foreground) + cv::dilate( imgThresholded, imgThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) ); + cv::erode(imgThresholded, imgThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) ); + + cv::imshow("Thresholded Image", imgThresholded); //show the thresholded image + cv::imshow("Original", imgOriginal); //show the original image + + // waits for awhile depending on the FPS value + // checks if ESC was pressed to exit + char key = (char)cv::waitKey(1000.0/fFPS); + //wait for 'esc' key press for 30ms. If 'esc' key is pressed, break loop + if (key == 27) + { + std::cout << "[INFO] esc key is pressed by user -> Shuting down!" << std::endl; + break; + } + if (key == 'u') + { + bIsImageUndistorted = !bIsImageUndistorted; + std::cout << "[INFO] Image undistorted: " << bIsImageUndistorted<< std::endl; + } + if (key == 's') + { + writeColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV); + std::cout << "[INFO] Color parameters saved to file: " << sColorParamFilename << std::endl; + } + + + } + + return 0; +} diff --git a/src/RedBallTracking.cpp b/src/RedBallTracking.cpp new file mode 100644 index 0000000..a8d9770 --- /dev/null +++ b/src/RedBallTracking.cpp @@ -0,0 +1,227 @@ +#include +#include +#include +#include +#include + +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/imgproc/imgproc.hpp" +#include + +#define CAM_PARAMS_FILENAME "./data/microsoft_livecam_hd3000.xml" +#define COLOR_PARAMS_FILENAME "./data/color_params_data.xml" +#define FPS 30.0 +#define STRUCTURAL_ELEMENTS_SIZE 5 +#define AREA_THRESOLD 1000 + +using namespace cv; +using namespace std; + +bool readCameraParameters(std::string filename, cv::Mat &camMatrix, cv::Mat & distCoeffs) +{ + cv::FileStorage fs(filename, cv::FileStorage::READ); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open the camera parameter file storage: " << filename << " !"<< std::endl; + return false; + } + + fs["camera_matrix"] >> camMatrix; + fs["distortion_coefficients"] >> distCoeffs; + + return true; +} + +bool readColorParameters(std::string filename, int& iLowH, int& iHighH, int& iLowS, int& iHighS, int& iLowV, int& iHighV) +{ + cv::FileStorage fs(filename, cv::FileStorage::READ); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open the color paramter file storage: " << filename << " !"<< std::endl; + return false; + } + + fs["lowH"] >> iLowH; + fs["highH"] >> iHighH; + + fs["lowS"] >> iLowS; + fs["highS"] >> iHighS; + + fs["lowV"] >> iLowV; + fs["highV"] >> iHighV; + + return true; +} + + int main( int argc, char** argv ) + { + // initializes main parameters + std::string sCameraParamFilename = CAM_PARAMS_FILENAME; + std::string sColorParamFilename = COLOR_PARAMS_FILENAME; + float fFPS = FPS; + int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE; + int iAreaThresold = AREA_THRESOLD; + + // updates main parameters from arguments + int opt; + while ((opt = getopt (argc, argv, ":c:f:s:a:i:")) != -1) + { + switch (opt) + { + case 'c': + sColorParamFilename = optarg; + break; + case 'f': + fFPS = atof(optarg); + break; + case 's': + iStructuralElementSize = atoi(optarg); + break; + case 'a': + iAreaThresold = atoi(optarg); + break; + case 'i': + sCameraParamFilename = optarg; + break; + case '?': + if (optopt == 'c' || optopt == 'f' || optopt == 's' || optopt == 'a') + fprintf (stderr, "Option -%c requires an argument.\n", optopt); + else if (isprint (optopt)) + fprintf (stderr, "Unknown option `-%c'.\n", optopt); + else + fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt); + return 1; + default: + abort (); + } + } + + // reads color parameters from the file storage + int iLowH, iHighH, iLowS, iHighS, iLowV, iHighV; + bool isColorParamsSet = readColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV); + + // checks if the color parameters were successfully read + if (!isColorParamsSet) + { + std::cout << "[ERROR] Color parameters could not be loaded!" << std::endl; + return -1; + } + + // distorted/undistorted image + bool bIsImageUndistorted = true; + + // reads camera intrinsic parameters + cv::Mat cameraMatrix, distCoeffs; + bool isCamParamsSet = readCameraParameters(sCameraParamFilename, cameraMatrix, distCoeffs); + + // checks if the camera parameters were successfully read + if (!isCamParamsSet) + { + std::cout << "[WARNING] Camera intrinsic parameters could not be loaded!" << std::endl; + } + + // creates a camera grabber + VideoCapture cap(0, cv::CAP_V4L2); //capture the video from webcam + + // checks if the camera was successfully opened + if ( !cap.isOpened() ) // if not success, exit program + { + cout << "[ERROR] Could not open the camera!" << endl; + return -1; + } + + // inits previous x,y location of the ball + int iLastX = -1; + int iLastY = -1; + + // captures a temporary image from the camera + Mat imgTmp; + cap.read(imgTmp); + + // creates a black image with the size as the camera output + Mat imgLines = Mat::zeros( imgTmp.size(), CV_8UC3 ); + + // main loop launched every FPS + while (true) + { + // reads a new frame from video + cv::Mat imgOriginal; + bool bSuccess = cap.read(imgOriginal); + + // checks if a new frame was grabbed + if (!bSuccess) //if not success, break loop + { + std::cout << "[WARNING] Could not read a new frame from video stream" << std::endl; + break; + } + + if (bIsImageUndistorted && isCamParamsSet) + { + cv::Mat temp = imgOriginal.clone(); + cv::undistort(temp, imgOriginal, cameraMatrix, distCoeffs); + } + + // converts the captured frame from BGR to HSV + cv::Mat imgHSV; + cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV); + + // thresholds the image based on the trackbar values + cv::Mat imgThresholded; + inRange(imgHSV, cv::Scalar(iLowH, iLowS, iLowV), cv::Scalar(iHighH, iHighS, iHighV), imgThresholded); + + // applies morphological opening (removes small objects from the foreground) + cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) ); + cv::dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) ); + + // applies morphological closing (removes small holes from the foreground) + cv::dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) ); + cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) ); + + // calculates the moments of the thresholded image + Moments oMoments = moments(imgThresholded); + double dM01 = oMoments.m01; + double dM10 = oMoments.m10; + double dArea = oMoments.m00; + + // if the area <= iAreaThresold, considers that the there are no object in the image and it's because of the noise, the area is not zero + if (dArea > iAreaThresold) + { + // calculates the position of the ball + int posX = dM10 / dArea; + int posY = dM01 / dArea; + + if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0) + { + // draww a red line from the previous point to the current point + line(imgLines, Point(posX, posY), Point(iLastX, iLastY), Scalar(0,0,255), 2); + } + + // stores the current position for enxt frame + iLastX = posX; + iLastY = posY; + } + + // displays the thresholded image + imshow("Thresholded Image", imgThresholded); + + // shows the original image with the tracking (red) lines + imgOriginal = imgOriginal + imgLines; + imshow("Original", imgOriginal); + + // waits for awhile depending on the FPS value + char key = (char)cv::waitKey(1000.0/fFPS); + // checks if ESC was pressed to exit + if (key == 27) // if 'esc' key is pressed, break loop + { + std::cout << "[INFO] esc key is pressed by user -> Shuting down!" << std::endl; + break; + } + if (key == 'u') + { + bIsImageUndistorted = !bIsImageUndistorted; + std::cout << "[INFO] Image undistorted: " << bIsImageUndistorted<< std::endl; + } + } + + return 0; +} \ No newline at end of file diff --git a/src/ReferenceTransform.cpp b/src/ReferenceTransform.cpp new file mode 100644 index 0000000..3002b4c --- /dev/null +++ b/src/ReferenceTransform.cpp @@ -0,0 +1,31 @@ +#include +#include + +// Gerçek dünya dönüşüm oranlarını tanımla (Bunları ölçüm yaparak ayarla!) +#define RATIO_CM_PER_PIXEL_X 0.1 // X yönünde cm/pixel oranı +#define RATIO_CM_PER_PIXEL_Y 0.1 // Y yönünde cm/pixel oranı +#define IMAGE_WIDTH 640 // Kameranın çözünürlüğünü buraya yaz +#define IMAGE_HEIGHT 480 + +// Piksellerden Gerçek Dünya Koordinatlarına Dönüşüm Fonksiyonu +cv::Point2f pixelToWorld(int u, int v) { + float x = (v - IMAGE_HEIGHT / 2) * RATIO_CM_PER_PIXEL_Y; + float y = (u - IMAGE_WIDTH / 2) * RATIO_CM_PER_PIXEL_X; + return cv::Point2f(x, y); +} + +int main() { + int u, v; + + // Kullanıcıdan piksel koordinatlarını al + std::cout << "Enter pixel coordinates (u v): "; + std::cin >> u >> v; + + // Koordinat dönüşümünü uygula + cv::Point2f worldCoord = pixelToWorld(u, v); + + // Sonucu ekrana yazdır + std::cout << "World coordinates (cm): X = " << worldCoord.x << ", Y = " << worldCoord.y << std::endl; + + return 0; +} diff --git a/src/RobotServoing.cpp b/src/RobotServoing.cpp new file mode 100644 index 0000000..220f9f5 --- /dev/null +++ b/src/RobotServoing.cpp @@ -0,0 +1,167 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/imgproc/imgproc.hpp" +#include + +#include "Kinematics.h" +#include "DynamixelHandler.h" + +#define CAM_PARAMS_FILENAME "./data/microsoft_livecam_hd3000.xml" +#define COLOR_PARAMS_FILENAME "./data/color_params_data.xml" +#define FPS 30.0 +#define STRUCTURAL_ELEMENTS_SIZE 5 +#define AREA_THRESOLD 1000 +#define ROBOT_L1 5 +#define ROBOT_L2 6 +#define RATIO_CM_PER_PIXEL_X 0.1 // X ekseninde dönüşüm oranı +#define RATIO_CM_PER_PIXEL_Y 0.1 // Y ekseninde dönüşüm oranı + +using namespace cv; +using namespace std; + +DynamixelHandler _oDxlHandler; +std::string _robotDxlPortName = "/dev/ttyUSB0"; +float _robotDxlProtocol = 2.0; +int _robotDxlBaudRate = 1000000; + +void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate) +{ + std::cout << "=== Initialization of the Dynamixel Motor communication ===" << std::endl; + dxlHandler.setDeviceName(portName); + dxlHandler.setProtocolVersion(protocol); + dxlHandler.openPort(); + dxlHandler.setBaudRate(baudRate); + dxlHandler.enableTorque(true); + std::cout << std::endl; +} + +void closeRobot(DynamixelHandler& dxlHandler) +{ + dxlHandler.enableTorque(false); + dxlHandler.closePort(); +} + +// Pikselden dünya koordinatlarına dönüştürme fonksiyonu +cv::Point2f pixelToWorld(int u, int v, int img_width, int img_height) +{ + float x = (v - img_height / 2) * RATIO_CM_PER_PIXEL_Y; + float y = (u - img_width / 2) * RATIO_CM_PER_PIXEL_X; + return cv::Point2f(x, y); +} + +int main(int argc, char** argv) +{ + float L1 = ROBOT_L1; + float L2 = ROBOT_L2; + float qpen = deg2rad(-90); // in rad + std::string sCameraParamFilename = CAM_PARAMS_FILENAME; + std::string sColorParamFilename = COLOR_PARAMS_FILENAME; + float fFPS = FPS; + int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE; + int iAreaThresold = AREA_THRESOLD; + + initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate); + + // Kamera açılıyor + VideoCapture cap(0, cv::CAP_V4L2); + if (!cap.isOpened()) + { + cout << "[ERROR] Could not open the camera!" << endl; + return -1; + } + + // İlk görüntü alınıyor + Mat imgTmp; + cap.read(imgTmp); + int img_width = imgTmp.size().width; + int img_height = imgTmp.size().height; + + int iLastX = -1, iLastY = -1; + + while (true) + { + Mat imgOriginal; + bool bSuccess = cap.read(imgOriginal); + if (!bSuccess) + { + cout << "[WARNING] Could not read a new frame from video stream" << endl; + break; + } + + // Görüntüyü HSV formatına çevir + cv::Mat imgHSV; + cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV); + + // Renk eşikleme + cv::Mat imgThresholded; + inRange(imgHSV, cv::Scalar(0, 100, 100), cv::Scalar(10, 255, 255), imgThresholded); + + // Morfolojik işlemler + cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5))); + cv::dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5))); + + // Moment hesaplama + Moments oMoments = moments(imgThresholded); + double dM01 = oMoments.m01; + double dM10 = oMoments.m10; + double dArea = oMoments.m00; + + int posX = -1, posY = -1; + if (dArea > iAreaThresold) + { + posX = dM10 / dArea; + posY = dM01 / dArea; + + if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0) + { + line(imgOriginal, Point(posX, posY), Point(iLastX, iLastY), Scalar(0, 0, 255), 2); + } + + iLastX = posX; + iLastY = posY; + } + + // Piksel koordinatlarını dünya koordinatlarına çevir + cv::Point2f worldCoord = pixelToWorld(posX, posY, img_width, img_height); + float x = worldCoord.x; + float y = worldCoord.y; + + cout << "(pixel -> cm) = (" << posX << ", " << posY << ") -> (" << x << ", " << y << ")" << endl; + + // Ters kinematik hesaplama + std::vector qi = computeInverseKinematics(x, y, L1, L2); + + // Robot eklemleri hareket ettir + if (qi.size() >= 3) + { + std::vector vTargetJointPosition; + vTargetJointPosition.push_back(qi[1]); + vTargetJointPosition.push_back(qpen); + vTargetJointPosition.push_back(qi[2]); + _oDxlHandler.sendTargetJointPosition(vTargetJointPosition); + } + + // Görüntü gösterme + imshow("Original", imgOriginal); + + // Kullanıcı girişini kontrol et + char key = (char)cv::waitKey(1000.0 / fFPS); + if (key == 27) // 'ESC' tuşu çıkışı sağlıyor + { + cout << "[INFO] ESC key pressed -> Shutting down!" << endl; + break; + } + } + + // Robot bağlantısını kapat + closeRobot(_oDxlHandler); + + return 0; +}