Once these trajectories were generated. The generated angular points were saved to a csv file to be imported onto the controlling programs on the simulators.
Darwin-OP walking on Webots
This is the controller program on webots that does it :
#include "Symmetry.hpp" #include <webots/Servo.hpp> #include <cstdlib> #include <cmath> #include <iostream> #include <fstream> using namespace webots; using namespace std; static const char *servoNames[NSERVOS] = { "ShoulderR" /*ID1 */, "ShoulderL" /*ID2 */, "ArmUpperR" /*ID3 */, "ArmUpperL" /*ID4 */, "ArmLowerR" /*ID5 */, "ArmLowerL" /*ID6 */, "PelvYR" /*ID7 */, "PelvYL" /*ID8 */, "PelvR" /*ID9 */, "PelvL" /*ID10*/, "LegUpperR" /*ID11*/, "LegUpperL" /*ID12*/, "LegLowerR" /*ID13*/, "LegLowerL" /*ID14*/, "AnkleR" /*ID15*/, "AnkleL" /*ID16*/, "FootR" /*ID17*/, "FootL" /*ID18*/, "Neck" /*ID19*/, "Head" /*ID20*/ }; //Constructor Symmetry::Symmetry(): Robot() { //Get time step // mTimeStep = getBasicTimeStep(); mTimeStep = 1; //Get the two RGB_LEDs mEyeLED = getLED("EyeLed"); mHeadLED = getLED("HeadLed"); //Get all the 20 Servos and enable them for(int i= 0; i < NSERVOS; i++) { mServos[i] = getServo(servoNames[i]); mServos[i]->enablePosition(mTimeStep); } } //Destructor Symmetry::~Symmetry() { } //Step function void Symmetry::myStep() { int ret = step(mTimeStep); if (ret == -1) exit(EXIT_SUCCESS); } //Wait function void Symmetry::wait(int ms) { double startTime = getTime(); double s = (double) ms / 1000.0; while (s + startTime >= getTime()) myStep(); } // function containing the main feedback loop void Symmetry::run() { double position = 0; float xp[145] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80,81,82,83,84,85,86,87,88,89,90,91,92,93,94,95,96,97,98,99,100,101,102,103,104,105,106,107,108,109,110,111,112,113,114,115,116,117,118,119,120,121,122,123,124,125,126,127,128,129,130,131,132,133,134,135,136,137,138,139,140,141,142,143,144}; float footl_loc [] = {-1.22,0,-0.52886,-0.53511,-0.53511,-0.53511,-0.53511,-0.53511,-0.53511,-0.53515,-0.5353,-0.53553,-0.53585,-0.53626,-0.53674,-0.5373,-0.53793,-0.53862,-0.53938,-0.5402,-0.54107,-0.54199,-0.54296,-0.54396,-0.54501,-0.54609,-0.5472,-0.54833,-0.54949,-0.55067,-0.55185,-0.55305,-0.55426,-0.55546,-0.55667,-0.55787,-0.55906,-0.56023,-0.5614,-0.56254,-0.56365,-0.56474,-0.5658,-0.56683,-0.56782,-0.56876,-0.56966,-0.57052,-0.57132,-0.57206,-0.57275,-0.57286,-0.57497,-0.57702,-0.57899,-0.58091,-0.58276,-0.61682,-0.64912,-0.67956,-0.708,-0.7343,-0.75832,-0.7799,-0.79891,-0.81523,-0.82876,-0.83943,-0.8472,-0.8521,-0.85415,-0.85346,-0.85016,-0.84442,-0.83643,-0.82642,-0.81462,-0.80127,-0.78658,-0.77075,-0.75396,-0.73635,-0.718,-0.69898,-0.6793,-0.65894,-0.63784,-0.61591,-0.59304,-0.56909,-0.54392,-0.51735,-0.48922,-0.48676,-0.48881,-0.4909,-0.49306,-0.49526,-0.49477,-0.49719,-0.49952,-0.50179,-0.50398,-0.5061,-0.50816,-0.51016,-0.5121,-0.51398,-0.51582,-0.51761,-0.51935,-0.52106,-0.52272,-0.52436,-0.52595,-0.52753,-0.52907,-0.5306,-0.5321,-0.53359,-0.53506,-0.53652,-0.53797,-0.53942,-0.54087,-0.54232,-0.54377,-0.54522,-0.54668,-0.54816,-0.54965,-0.55115,-0.55267,-0.55422,-0.55579,-0.55738,-0.559,-0.56066,-0.56234,-0.56407,-0.56583,-0.56762,-0.56947,-0.57135,-0.57329}; float footr_loc [] = {1.22,0,0.52886,0.53511,0.53511,0.53511,0.53511,0.53511,0.53511,0.56636,0.59597,0.62388,0.65005,0.67438,0.69681,0.71726,0.73564,0.75188,0.76595,0.77778,0.78738,0.79473,0.79986,0.80282,0.80366,0.80248,0.79936,0.79441,0.78773,0.77944,0.76963,0.75839,0.74581,0.73193,0.7168,0.70044,0.68285,0.66403,0.64394,0.62254,0.59977,0.57557,0.54985,0.52253,0.4935,0.4901,0.49113,0.4921,0.49301,0.49386,0.49465,0.49477,0.49719,0.49952,0.50179,0.50398,0.5061,0.50816,0.51016,0.5121,0.51398,0.51582,0.51761,0.51935,0.52106,0.52272,0.52436,0.52595,0.52753,0.52907,0.5306,0.5321,0.53359,0.53506,0.53652,0.53797,0.53942,0.54087,0.54232,0.54377,0.54522,0.54668,0.54816,0.54965,0.55115,0.55267,0.55422,0.55579,0.55738,0.559,0.56066,0.56234,0.56407,0.56583,0.56762,0.56947,0.57135,0.57329,0.57286,0.57497,0.57702,0.57899,0.58091,0.58276,0.61682,0.64912,0.67956,0.708,0.7343,0.75832,0.7799,0.79891,0.81523,0.82876,0.83943,0.8472,0.8521,0.85415,0.85346,0.85016,0.84442,0.83643,0.82642,0.81462,0.80127,0.78658,0.77075,0.75396,0.73635,0.718,0.69898,0.6793,0.65894,0.63784,0.61591,0.59304,0.56909,0.54392,0.51735,0.48922,0.48676,0.48881,0.4909,0.49306,0.49526}; float anklel_loc [] = {0,0,0,0,0.01538,0.030685,0.045839,0.06077,0.075406,0.089678,0.10352,0.11687,0.12966,0.14185,0.15338,0.16419,0.17426,0.18354,0.192,0.1996,0.20632,0.21214,0.21703,0.22098,0.22398,0.22601,0.22708,0.22718,0.2263,0.22446,0.22165,0.21788,0.21318,0.20754,0.201,0.19357,0.18528,0.17616,0.16625,0.15557,0.14418,0.13212,0.11944,0.1062,0.092453,0.078261,0.063691,0.048813,0.033697,0.018416,0.003045,-0.012341,-0.027666,-0.042857,-0.057838,-0.072538,-0.086887,-0.10082,-0.11427,-0.12718,-0.13949,-0.15115,-0.16211,-0.17233,-0.18177,-0.19039,-0.19817,-0.20506,-0.21106,-0.21613,-0.22027,-0.22346,-0.22569,-0.22695,-0.22724,-0.22655,-0.2249,-0.22228,-0.2187,-0.21418,-0.20873,-0.20237,-0.19511,-0.18699,-0.17803,-0.16827,-0.15774,-0.14649,-0.13456,-0.122,-0.10886,-0.09521,-0.0811,-0.0666,-0.051777,-0.036703,-0.021448,-0.0060893,0.0092996,0.024643,0.039866,0.054894,0.069655,0.084079,0.098099,0.11165,0.12467,0.1371,0.1489,0.16,0.17037,0.17997,0.18875,0.1967,0.20377,0.20994,0.2152,0.21953,0.2229,0.22532,0.22677,0.22726,0.22676,0.2253,0.22287,0.21949,0.21515,0.20988,0.2037,0.19662,0.18866,0.17987,0.17026,0.15988,0.14877,0.13697,0.12453,0.1115,0.097949,0.083925,0.069497,0.054732,0.039701,0.024477,0.0091326}; float ankler_loc [] = {0,0,0,0,0.01538,0.030685,0.045839,0.06077,0.075406,0.089678,0.10352,0.11687,0.12966,0.14185,0.15338,0.16419,0.17426,0.18354,0.192,0.1996,0.20632,0.21214,0.21703,0.22098,0.22398,0.22601,0.22708,0.22718,0.2263,0.22446,0.22165,0.21788,0.21318,0.20754,0.201,0.19357,0.18528,0.17616,0.16625,0.15557,0.14418,0.13212,0.11944,0.1062,0.092453,0.078261,0.063691,0.048813,0.033697,0.018416,0.003045,-0.012341,-0.027666,-0.042857,-0.057838,-0.072538,-0.086887,-0.10082,-0.11427,-0.12718,-0.13949,-0.15115,-0.16211,-0.17233,-0.18177,-0.19039,-0.19817,-0.20506,-0.21106,-0.21613,-0.22027,-0.22346,-0.22569,-0.22695,-0.22724,-0.22655,-0.2249,-0.22228,-0.2187,-0.21418,-0.20873,-0.20237,-0.19511,-0.18699,-0.17803,-0.16827,-0.15774,-0.14649,-0.13456,-0.122,-0.10886,-0.09521,-0.0811,-0.0666,-0.051777,-0.036703,-0.021448,-0.0060893,0.0092996,0.024643,0.039866,0.054894,0.069655,0.084079,0.098099,0.11165,0.12467,0.1371,0.1489,0.16,0.17037,0.17997,0.18875,0.1967,0.20377,0.20994,0.2152,0.21953,0.2229,0.22532,0.22677,0.22726,0.22676,0.2253,0.22287,0.21949,0.21515,0.20988,0.2037,0.19662,0.18866,0.17987,0.17026,0.15988,0.14877,0.13697,0.12453,0.1115,0.097949,0.083925,0.069497,0.054732,0.039701,0.024477,0.0091326}; float kneel_loc [] = {-2.25,0,-1.0577,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0701,-1.0701,-1.0701,-1.0701,-1.07,-1.07,-1.07,-1.0699,-1.0699,-1.0698,-1.0697,-1.0696,-1.0696,-1.0695,-1.0694,-1.0693,-1.0692,-1.0691,-1.069,-1.0689,-1.0688,-1.0686,-1.0685,-1.0684,-1.0683,-1.0682,-1.0681,-1.068,-1.0678,-1.0677,-1.0676,-1.0676,-1.0673,-1.067,-1.0667,-1.0664,-1.066,-1.1283,-1.1871,-1.2428,-1.2951,-1.3443,-1.3902,-1.4328,-1.4721,-1.5081,-1.5406,-1.5696,-1.595,-1.6167,-1.6347,-1.6488,-1.659,-1.6653,-1.6676,-1.666,-1.6603,-1.6507,-1.6372,-1.6198,-1.5986,-1.5738,-1.5453,-1.5133,-1.4779,-1.439,-1.3969,-1.3514,-1.3027,-1.2508,-1.1957,-1.1372,-1.0754,-1.0666,-1.0668,-1.0671,-1.0674,-1.0677,-1.0676,-1.0679,-1.0682,-1.0684,-1.0687,-1.0689,-1.069,-1.0692,-1.0693,-1.0695,-1.0696,-1.0697,-1.0698,-1.0699,-1.07,-1.07,-1.0701,-1.0701,-1.0701,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0701,-1.0701,-1.07,-1.07,-1.0699,-1.0698,-1.0698,-1.0697,-1.0696,-1.0695,-1.0693,-1.0692,-1.0691,-1.0689,-1.0687,-1.0685,-1.0683,-1.0681,-1.0678,-1.0676}; float kneer_loc [] = {2.25,0,1.0577,1.0702,1.0702,1.0702,1.0702,1.0702,1.0702,1.1326,1.1917,1.2475,1.2999,1.3491,1.3949,1.4374,1.4765,1.512,1.544,1.5725,1.5973,1.6183,1.6356,1.6491,1.6588,1.6646,1.6666,1.6647,1.6589,1.6493,1.6359,1.6188,1.5979,1.5734,1.5452,1.5135,1.4783,1.4397,1.3977,1.3523,1.3037,1.2517,1.1965,1.138,1.0761,1.067,1.0672,1.0673,1.0674,1.0675,1.0676,1.0676,1.0679,1.0682,1.0684,1.0687,1.0689,1.069,1.0692,1.0693,1.0695,1.0696,1.0697,1.0698,1.0699,1.07,1.07,1.0701,1.0701,1.0701,1.0702,1.0702,1.0702,1.0702,1.0702,1.0702,1.0702,1.0702,1.0701,1.0701,1.07,1.07,1.0699,1.0698,1.0698,1.0697,1.0696,1.0695,1.0693,1.0692,1.0691,1.0689,1.0687,1.0685,1.0683,1.0681,1.0678,1.0676,1.0676,1.0673,1.067,1.0667,1.0664,1.066,1.1283,1.1871,1.2428,1.2951,1.3443,1.3902,1.4328,1.4721,1.5081,1.5406,1.5696,1.595,1.6167,1.6347,1.6488,1.659,1.6653,1.6676,1.666,1.6603,1.6507,1.6372,1.6198,1.5986,1.5738,1.5453,1.5133,1.4779,1.439,1.3969,1.3514,1.3027,1.2508,1.1957,1.1372,1.0754,1.0666,1.0668,1.0671,1.0674,1.0677}; float hipPl_loc [] = {1.15,0,0.52886,0.53511,0.53511,0.53511,0.53511,0.53511,0.53511,0.53506,0.53491,0.53468,0.53436,0.53395,0.53346,0.5329,0.53227,0.53156,0.5308,0.52997,0.52908,0.52814,0.52715,0.52611,0.52503,0.52391,0.52276,0.52157,0.52036,0.51912,0.51787,0.5166,0.51531,0.51402,0.51273,0.51143,0.51014,0.50886,0.50759,0.50634,0.50511,0.5039,0.50273,0.50158,0.50048,0.49942,0.4984,0.49744,0.49653,0.49568,0.4949,0.49477,0.49235,0.48999,0.48769,0.48545,0.48326,0.51144,0.53803,0.56319,0.58712,0.60995,0.63184,0.65289,0.67319,0.69282,0.71181,0.73014,0.74776,0.7646,0.78051,0.79534,0.80888,0.82091,0.83122,0.83955,0.84569,0.84943,0.85059,0.84904,0.84468,0.83745,0.82733,0.81436,0.79858,0.78011,0.75905,0.73553,0.70971,0.68173,0.65174,0.61987,0.58623,0.5798,0.57804,0.57623,0.57436,0.57243,0.57286,0.57074,0.56867,0.56665,0.56468,0.56276,0.56088,0.55905,0.55725,0.5555,0.55378,0.5521,0.55045,0.54883,0.54723,0.54566,0.54412,0.54259,0.54108,0.53958,0.5381,0.53662,0.53515,0.53369,0.53222,0.53076,0.52928,0.52781,0.52632,0.52481,0.5233,0.52176,0.5202,0.51861,0.517,0.51536,0.51368,0.51196,0.5102,0.5084,0.50655,0.50466,0.5027,0.50069,0.49862,0.49649,0.49429}; // these are my calculated hip position to keep the body vertical float hipPr_loc [] = {-1.15,0,-0.52886,-0.53511,-0.53511,-0.53511,-0.53511,-0.53511,-0.53511,-0.56628,-0.59575,-0.62359,-0.6499,-0.67472,-0.69813,-0.72015,-0.74082,-0.76014,-0.7781,-0.7947,-0.80988,-0.82359,-0.83577,-0.84633,-0.85516,-0.86217,-0.86725,-0.87029,-0.87121,-0.8699,-0.86631,-0.86038,-0.85209,-0.84143,-0.82841,-0.81306,-0.79546,-0.77565,-0.75373,-0.72978,-0.70389,-0.67615,-0.64664,-0.61543,-0.58257,-0.57692,-0.57604,-0.57519,-0.5744,-0.57366,-0.57297,-0.57286,-0.57074,-0.56867,-0.56665,-0.56468,-0.56276,-0.56088,-0.55905,-0.55725,-0.5555,-0.55378,-0.5521,-0.55045,-0.54883,-0.54723,-0.54566,-0.54412,-0.54259,-0.54108,-0.53958,-0.5381,-0.53662,-0.53515,-0.53369,-0.53222,-0.53076,-0.52928,-0.52781,-0.52632,-0.52481,-0.5233,-0.52176,-0.5202,-0.51861,-0.517,-0.51536,-0.51368,-0.51196,-0.5102,-0.5084,-0.50655,-0.50466,-0.5027,-0.50069,-0.49862,-0.49649,-0.49429,-0.49477,-0.49235,-0.48999,-0.48769,-0.48545,-0.48326,-0.51144,-0.53803,-0.56319,-0.58712,-0.60995,-0.63184,-0.65289,-0.67319,-0.69282,-0.71181,-0.73014,-0.74776,-0.7646,-0.78051,-0.79534,-0.80888,-0.82091,-0.83122,-0.83955,-0.84569,-0.84943,-0.85059,-0.84904,-0.84468,-0.83745,-0.82733,-0.81436,-0.79858,-0.78011,-0.75905,-0.73553,-0.70971,-0.68173,-0.65174,-0.61987,-0.58623,-0.5798,-0.57804,-0.57623,-0.57436,-0.57243}; float hipRl_loc [] = {0,0,0,0,0.01538,0.030685,0.045839,0.06077,0.075406,0.089678,0.10352,0.11687,0.12966,0.14185,0.15338,0.16419,0.17426,0.18354,0.192,0.1996,0.20632,0.21214,0.21703,0.22098,0.22398,0.22601,0.22708,0.22718,0.2263,0.22446,0.22165,0.21788,0.21318,0.20754,0.201,0.19357,0.18528,0.17616,0.16625,0.15557,0.14418,0.13212,0.11944,0.1062,0.092453,0.078261,0.063691,0.048813,0.033697,0.018416,0.003045,-0.012341,-0.027666,-0.042857,-0.057838,-0.072538,-0.086887,-0.10082,-0.11427,-0.12718,-0.13949,-0.15115,-0.16211,-0.17233,-0.18177,-0.19039,-0.19817,-0.20506,-0.21106,-0.21613,-0.22027,-0.22346,-0.22569,-0.22695,-0.22724,-0.22655,-0.2249,-0.22228,-0.2187,-0.21418,-0.20873,-0.20237,-0.19511,-0.18699,-0.17803,-0.16827,-0.15774,-0.14649,-0.13456,-0.122,-0.10886,-0.09521,-0.0811,-0.0666,-0.051777,-0.036703,-0.021448,-0.0060893,0.0092996,0.024643,0.039866,0.054894,0.069655,0.084079,0.098099,0.11165,0.12467,0.1371,0.1489,0.16,0.17037,0.17997,0.18875,0.1967,0.20377,0.20994,0.2152,0.21953,0.2229,0.22532,0.22677,0.22726,0.22676,0.2253,0.22287,0.21949,0.21515,0.20988,0.2037,0.19662,0.18866,0.17987,0.17026,0.15988,0.14877,0.13697,0.12453,0.1115,0.097949,0.083925,0.069497,0.054732,0.039701,0.024477,0.0091326}; float hipRr_loc [] = {0,0,0,0,0.01538,0.030685,0.045839,0.06077,0.075406,0.089678,0.10352,0.11687,0.12966,0.14185,0.15338,0.16419,0.17426,0.18354,0.192,0.1996,0.20632,0.21214,0.21703,0.22098,0.22398,0.22601,0.22708,0.22718,0.2263,0.22446,0.22165,0.21788,0.21318,0.20754,0.201,0.19357,0.18528,0.17616,0.16625,0.15557,0.14418,0.13212,0.11944,0.1062,0.092453,0.078261,0.063691,0.048813,0.033697,0.018416,0.003045,-0.012341,-0.027666,-0.042857,-0.057838,-0.072538,-0.086887,-0.10082,-0.11427,-0.12718,-0.13949,-0.15115,-0.16211,-0.17233,-0.18177,-0.19039,-0.19817,-0.20506,-0.21106,-0.21613,-0.22027,-0.22346,-0.22569,-0.22695,-0.22724,-0.22655,-0.2249,-0.22228,-0.2187,-0.21418,-0.20873,-0.20237,-0.19511,-0.18699,-0.17803,-0.16827,-0.15774,-0.14649,-0.13456,-0.122,-0.10886,-0.09521,-0.0811,-0.0666,-0.051777,-0.036703,-0.021448,-0.0060893,0.0092996,0.024643,0.039866,0.054894,0.069655,0.084079,0.098099,0.11165,0.12467,0.1371,0.1489,0.16,0.17037,0.17997,0.18875,0.1967,0.20377,0.20994,0.2152,0.21953,0.2229,0.22532,0.22677,0.22726,0.22676,0.2253,0.22287,0.21949,0.21515,0.20988,0.2037,0.19662,0.18866,0.17987,0.17026,0.15988,0.14877,0.13697,0.12453,0.1115,0.097949,0.083925,0.069497,0.054732,0.039701,0.024477,0.0091326}; float x = 0; float anklel; float ankler; float footl; float footr; float kneel; float kneer; float hipPl; float hipPr; float hipRl; float hipRr; // for(int i= 0; i < 50; i++) // { // anklel_loc[i] = -anklel_loc[i]; // ankler_loc[i] = -ankler_loc[i]; // hipRl_loc[i] = -hipRl_loc[i]; // hipRr_loc[i] = -hipRr_loc[i]; // } myStep(); for(int i= 0; i < NSERVOS; i++) { position = mServos[i]->getPosition(); cout << i ; cout << "[" << position << "]\n"; } while (1) { if (x<2) { footl = lip(x,xp,footl_loc); footr = lip(x,xp,footr_loc); kneel = lip(x,xp,kneel_loc); kneer = lip(x,xp,kneer_loc); hipPl = lip(x,xp,hipPl_loc); hipPr = lip(x,xp,hipPr_loc); mServos[15]->setPosition(footl); mServos[14]->setPosition(footr); mServos[13]->setPosition(kneel); mServos[12]->setPosition(kneer); mServos[11]->setPosition(hipPl); mServos[10]->setPosition(hipPr); // cout << y ; // position = mServos[0]->getPosition(); // cout << "[" << position << "]\n"; // if (position >= 2.1) // {sign = (-1);} // if (position <= 0) // {sign = (1);} // position = position + (sign)*0.1; // mServos[0]->setPosition(y); // x = x + 0.02; } else if (x<144) { footl = lip(x,xp,footl_loc); footr = lip(x,xp,footr_loc); kneel = lip(x,xp,kneel_loc); kneer = lip(x,xp,kneer_loc); hipPl = lip(x,xp,hipPl_loc); hipPr = lip(x,xp,hipPr_loc); // hip swing movement anklel = lip(x,xp,anklel_loc); ankler = lip(x,xp,ankler_loc); hipRl = lip(x,xp,hipRl_loc); hipRr = lip(x,xp,hipRr_loc); cout << hipRr ; mServos[17]->setPosition(anklel); mServos[16]->setPosition(ankler); mServos[15]->setPosition(footl); mServos[14]->setPosition(footr); mServos[13]->setPosition(kneel); mServos[12]->setPosition(kneer); mServos[11]->setPosition(hipPl); mServos[10]->setPosition(hipPr); mServos[9]->setPosition(hipRl); mServos[8]->setPosition(hipRr); } if (x<2) x = x+ 0.01; else if(x<144) x = x + 3; else x = 51; // step myStep(); } } float Symmetry::lip(float x,float xp[],float yp[]) { float y=0; float xp0=0; float yp0=0; float xp1=0; float yp1=0; for (int i =0; i<=145; i++) { if (xp[i]<=x) { xp0 = xp[i]; yp0 = yp[i]; } if (xp[i]>x) { xp1 = xp[i]; yp1 = yp[i]; break; } } float m = (yp1-yp0)/(xp1-xp0); y = (yp0 +(m*(x-xp0))); return (y); }