User Tools

Site Tools


drexel_darwin_walking_gazebo

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

drexel_darwin_walking_gazebo [2016/11/06 20:33] (current)
dwallace created
Line 1: Line 1:
 +====== Darwin-OP Walking Simulation on DRC-Sim(Gazebo) ======
  
 +The trajectories generated from Matlab are exported to a controller program(shown below) to do the simulation of Darwin-OP Walking. Here is a video of the Darwin-OP walking on DRC-Sim(Gazebo). ​
 +
 +{{youtube>​O1FFN8TDpEA?​large}}\\ ​
 +
 +The controller program that does the above simulation
 +
 +<code c++>
 +// Author : Bharadwaj-Ramesh
 +// Date   : 14th April 2013
 +// Email  : [email protected]
 +#include "​gazebo/​common/​common.hh"​
 +#include "​gazebo/​physics/​physics.hh"​
 +#include "​gazebo/​common/​Events.hh"​
 +#include "​gazebo/​common/​PID.hh"​
 +// Darwin-OP walking.
 +// rename the plugin to darwin_plugin.cc and then build it 
 +
 +      float xp [143] = {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};​
 +      float footl_loc [] = {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 [] = {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.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.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 [] = {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 [] = {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 [] = {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 [] = {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.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.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 x1;
 +      float anklel; ​
 +      float ankler; ​
 +      float footl; ​
 +      float footr;
 +      float kneel; ​
 +      float kneer;
 +      float hipPl; ​
 +      float hipPr;
 +      float hipRl; ​
 +      float hipRr;  ​
 +
 +namespace gazebo
 +{
 +  class PID1Joints : public ModelPlugin
 +  {
 +    public: void Load(physics::​ModelPtr _model, sdf::​ElementPtr /*_sdf*/)
 +    {
 +
 +
 +      this->​model_ = _model;
 +      this->​target_position_ = 0.0; // position for all joints that do not move.
 +
 +// leg joints
 +
 +      this->​pid_j_ankle2_r.Init(10,​ 0.01, 4, 0, 0, 25, -25);
 +      this->​pid_j_ankle2_r.SetCmd(this->​target_position_);​
 +      this->​ankle2_r_joint_ = this->​model_->​GetJoint("​j_ankle2_r"​);​
 +
 +      this->​pid_j_ankle1_r.Init(20,​ 0.0, 9, 0, 0, 25, -25);
 +      this->​pid_j_ankle1_r.SetCmd(this->​target_position_);​
 +      this->​ankle1_r_joint_ = this->​model_->​GetJoint("​j_ankle1_r"​);​
 +
 +      this->​pid_j_tibia_r.Init(20,​ 0.0, 1, 0, 0, 25, -25);
 +      this->​pid_j_tibia_r.SetCmd(this->​target_position_);​
 +      this->​tibia_r_joint_ = this->​model_->​GetJoint("​j_tibia_r"​);​
 +
 +      this->​pid_j_thigh2_r.Init(20,​ 0.0, 9, 0, 0, 25, -25);
 +      this->​pid_j_thigh2_r.SetCmd(this->​target_position_);​
 +      this->​thigh2_r_joint_ = this->​model_->​GetJoint("​j_thigh2_r"​);​
 +
 +      this->​pid_j_thigh1_r.Init(45,​ 0.5, 18, 0, 0, 25, -25);
 +      this->​pid_j_thigh1_r.SetCmd(this->​target_position_);​
 +      this->​thigh1_r_joint_ = this->​model_->​GetJoint("​j_thigh1_r"​);​
 +// left leg
 +      this->​pid_j_ankle2_l.Init(10,​ 0.01, 4, 0, 0, 25, -25);
 +      this->​pid_j_ankle2_l.SetCmd(this->​target_position_);​
 +      this->​ankle2_l_joint_ = this->​model_->​GetJoint("​j_ankle2_l"​);​
 +
 +      this->​pid_j_ankle1_l.Init(20,​ 0.0, 9, 0, 0, 25, -25);
 +      this->​pid_j_ankle1_l.SetCmd(this->​target_position_);​
 +      this->​ankle1_l_joint_ = this->​model_->​GetJoint("​j_ankle1_l"​);​
 +
 +      this->​pid_j_tibia_l.Init(20,​ 0.0, 1, 0, 0, 25, -25);
 +      this->​pid_j_tibia_l.SetCmd(this->​target_position_);​
 +      this->​tibia_l_joint_ = this->​model_->​GetJoint("​j_tibia_l"​);​
 +
 +      this->​pid_j_thigh2_l.Init(20,​ 0.0, 9, 0, 0, 25, -25);
 +      this->​pid_j_thigh2_l.SetCmd(this->​target_position_);​
 +      this->​thigh2_l_joint_ = this->​model_->​GetJoint("​j_thigh2_l"​);​
 +
 +      this->​pid_j_thigh1_l.Init(45,​ 0.5, 18, 0, 0, 25, -25);
 +      this->​pid_j_thigh1_l.SetCmd(this->​target_position_);​
 +      this->​thigh1_l_joint_ = this->​model_->​GetJoint("​j_thigh1_l"​);​
 +// left shoulder and upper limb
 +      this->​pid_j_shoulder_l.Init(2,​ 0, 0, 0, 0, 25, -25);
 +      this->​pid_j_shoulder_l.SetCmd(this->​target_position_);​
 +      this->​shoulder_l_joint_ = this->​model_->​GetJoint("​j_shoulder_l"​);​
 +
 +      this->​pid_j_high_arm_l.Init(2,​ 0, 0, 0, 0, 25, -25);
 +      this->​pid_j_high_arm_l.SetCmd(this->​target_position_);​
 +      this->​high_arm_l_joint_ = this->​model_->​GetJoint("​j_high_arm_l"​);​
 +
 +      this->​pid_j_low_arm_l.Init(1,​ 0, 0, 0, 0, 25, -25);
 +      this->​pid_j_low_arm_l.SetCmd(this->​target_position_);​
 +      this->​low_arm_l_joint_ = this->​model_->​GetJoint("​j_low_arm_l"​);​
 +
 +      this->​pid_j_wrist_l.Init(1,​ 0, 0, 0, 0, 25, -25);
 +      this->​pid_j_wrist_l.SetCmd(this->​target_position_);​
 +      this->​wrist_l_joint_ = this->​model_->​GetJoint("​j_wrist_l"​);​
 +
 +      this->​pid_j_gripper_l.Init(1,​ 0, 0, 0, 0, 25, -25);
 +      this->​pid_j_gripper_l.SetCmd(this->​target_position_);​
 +      this->​gripper_l_joint_ = this->​model_->​GetJoint("​j_gripper_l"​);​
 +
 +// right hand
 +
 +      this->​pid_j_shoulder_r.Init(2,​ 0, .5, 0, 0, 25, -25);
 +      this->​pid_j_shoulder_r.SetCmd(this->​target_position_);​
 +      this->​shoulder_r_joint_ = this->​model_->​GetJoint("​j_shoulder_r"​);​
 +
 +      this->​pid_j_high_arm_r.Init(2,​ 0, .5, 0, 0, 25, -25);
 +      this->​pid_j_high_arm_r.SetCmd(this->​target_position_);​
 +      this->​high_arm_r_joint_ = this->​model_->​GetJoint("​j_high_arm_r"​);​
 +
 +      this->​pid_j_low_arm_r.Init(1,​ 0, 0.5, 0, 0, 25, -25);
 +      this->​pid_j_low_arm_r.SetCmd(this->​target_position_);​
 +      this->​low_arm_r_joint_ = this->​model_->​GetJoint("​j_low_arm_r"​);​
 +
 +      this->​pid_j_wrist_r.Init(1,​ 0, 0, 0, 0, 25, -25);
 +      this->​pid_j_wrist_r.SetCmd(this->​target_position_);​
 +      this->​wrist_r_joint_ = this->​model_->​GetJoint("​j_wrist_r"​);​
 +
 +      this->​pid_j_gripper_r.Init(1,​ 0, 0, 0, 0, 25, -25);
 +      this->​pid_j_gripper_r.SetCmd(this->​target_position_);​
 +      this->​gripper_r_joint_ = this->​model_->​GetJoint("​j_gripper_r"​);​
 +
 +      this->​pid_j_pan.Init(1,​ 0, 0, 0, 0, 25, -25);
 +      this->​pid_j_pan.SetCmd(this->​target_position_);​
 +      this->​pan_joint_ = this->​model_->​GetJoint("​j_pan"​);​
 +
 +      this->​pid_j_tilt.Init(1,​ 0, 0, 0, 0, 25, -25);
 +      this->​pid_j_tilt.SetCmd(this->​target_position_);​
 +      this->​tilt_joint_ = this->​model_->​GetJoint("​j_tilt"​);​
 +
 +      this->​pid_j_pelvis_l.Init(30,​ 0.03, 30, 0, 0, 25, -25);
 +      this->​pid_j_pelvis_l.SetCmd(this->​target_position_);​
 +      this->​pelvis_l_joint_ = this->​model_->​GetJoint("​j_pelvis_l"​);​
 +
 +      this->​pid_j_pelvis_r.Init(30,​ 0.03, 30, 0, 0, 25, -25);
 +      this->​pid_j_pelvis_r.SetCmd(this->​target_position_);​
 +      this->​pelvis_r_joint_ = this->​model_->​GetJoint("​j_pelvis_r"​);​
 +
 +      this->​last_update_time_ = this->​model_->​GetWorld()->​GetSimTime();​
 +      this->​update_connection_ = event::​Events::​ConnectWorldUpdateStart(
 +        boost::​bind(&​PID1Joints::​UpdatePID,​ this));
 +    }
 +    void UpdatePID()
 +    {
 +      common::​Time current_time = this->​model_->​GetWorld()->​GetSimTime();​
 +      k_ = k_ + 1 ;
 +      double error;
 +      double dt    = current_time.Double()
 +                   - this->​last_update_time_.Double();​
 +
 +        if (x1<1)
 +        {
 +          footl = lip(x1,​xp,​footl_loc);​
 +          footr = lip(x1,​xp,​footr_loc);​
 +          kneel = lip(x1,​xp,​kneel_loc);​
 +          kneer = lip(x1,​xp,​kneer_loc);​
 +          hipPl = lip(x1,​xp,​hipPl_loc);​
 +          hipPr = lip(x1,​xp,​hipPr_loc);​
 +
 +//          gzdbg << "​footl:​{"​ << footl << " ​ footr:​{"​ << footr << "​} ​ x : {" << x1 << "} \n";
 +//          gzdbg << "​kneel:​{"​ << kneel << " ​ kneer:​{"​ << kneer << "​} ​ x : {" << x1 << "} \n";
 +//          gzdbg << "​hipPl:​{"​ << hipPl << " ​ hipPr:​{"​ << hipPr << "​} ​ x : {" << x1 << "} \n";
 +
 +          ankle1_l(footl,​dt);​
 +          ankle1_r(footr,​dt);​
 +          tibia_l(kneel,​dt);​
 +          tibia_r(kneer,​dt);​
 +          thigh2_l(hipPl,​dt);​
 +          thigh2_r(hipPr,​dt);​
 +
 +          ankle2_l(0.0,​dt);​
 +          ankle2_r(0.0,​dt);​
 +          thigh1_l(0.0,​dt);​
 +          thigh1_r(0.0,​dt);​
 +        }
 +        else
 +        {
 +          footl = lip(x1,​xp,​footl_loc);​
 +          footr = lip(x1,​xp,​footr_loc);​
 +          kneel = lip(x1,​xp,​kneel_loc);​
 +          kneer = lip(x1,​xp,​kneer_loc);​
 +          hipPl = lip(x1,​xp,​hipPl_loc);​
 +          hipPr = lip(x1,​xp,​hipPr_loc);​
 +
 +          // hip swing movement
 +          anklel = lip(x1,​xp,​anklel_loc);​
 +          ankler = lip(x1,​xp,​ankler_loc);​
 +          hipRl = lip(x1,​xp,​hipRl_loc);​
 +          hipRr = lip(x1,​xp,​hipRr_loc); ​         ​
 +
 +//          gzdbg << "​footl:​{"​ << footl << " ​ footr:​{"​ << footr << "​} ​ x : {" << x1 << "} \n";
 +//          gzdbg << "​kneel:​{"​ << kneel << " ​ kneer:​{"​ << kneer << "​} ​ x : {" << x1 << "} \n";
 +//          gzdbg << "​hipPl:​{"​ << hipPl << " ​ hipPr:​{"​ << hipPr << "​} ​ x : {" << x1 << "} \n";
 +//          gzdbg << "​anklel:​{"​ << anklel << " ​ ankler:​{"​ << ankler << "​} ​ x : {" << x1 << "} \n";
 +//          gzdbg << "​hipRl:​{"​ << hipRl << " ​ hipRr:​{"​ << hipRr << "​} ​ x : {" << x1 << "} \n";
 +
 +          ankle2_l(anklel,​dt);​
 +          ankle2_r(ankler,​dt);​
 +          ankle1_l(footl,​dt);​
 +          ankle1_r(footr,​dt);​
 +          tibia_l(kneel,​dt);​
 +          tibia_r(kneer,​dt);​
 +          thigh2_l(hipPl,​dt);​
 +          thigh2_r(hipPr,​dt);​
 +          thigh1_l(hipRl,​dt);​
 +          thigh1_r(hipRr,​dt);​
 +
 +        }
 +
 +
 +// left arm
 +      error = this->​shoulder_l_joint_->​GetAngle(0).Radian()
 +                   - target_position_;​
 +      this->​pid_j_shoulder_l.Update(error,​ dt);
 +      this->​shoulder_l_joint_->​SetForce(0,​ this->​pid_j_shoulder_l.GetCmd());​
 +
 +
 +      error = this->​high_arm_l_joint_->​GetAngle(0).Radian()
 +                   - target_position_;​
 +      this->​pid_j_high_arm_l.Update(error,​ dt);
 +      this->​high_arm_l_joint_->​SetForce(0,​ this->​pid_j_high_arm_l.GetCmd());​
 +
 +      error = this->​low_arm_l_joint_->​GetAngle(0).Radian()
 +                   - target_position_ ;
 +      this->​pid_j_low_arm_l.Update(error,​ dt);
 +      this->​low_arm_l_joint_->​SetForce(0,​ this->​pid_j_low_arm_l.GetCmd());​
 +
 +
 +      error = this->​wrist_l_joint_->​GetAngle(0).Radian()
 +                   - target_position_;​
 +      this->​pid_j_wrist_l.Update(error,​ dt);
 +      this->​wrist_l_joint_->​SetForce(0,​ this->​pid_j_wrist_l.GetCmd());​
 +      ​
 +
 +      ​
 +      error = this->​gripper_l_joint_->​GetAngle(0).Radian()
 +                   - target_position_;​
 +      this->​pid_j_gripper_l.Update(error,​ dt);
 +      this->​gripper_l_joint_->​SetForce(0,​ this->​pid_j_gripper_l.GetCmd());​
 +
 +
 +// right arm 
 +      error = this->​shoulder_r_joint_->​GetAngle(0).Radian()
 +                   - target_position_;​
 +      this->​pid_j_shoulder_r.Update(error,​ dt);
 +      this->​shoulder_r_joint_->​SetForce(0,​ this->​pid_j_shoulder_r.GetCmd());​
 +
 +      error = this->​high_arm_r_joint_->​GetAngle(0).Radian()
 +                   - target_position_;​
 +      this->​pid_j_high_arm_r.Update(error,​ dt);
 +      this->​high_arm_r_joint_->​SetForce(0,​ this->​pid_j_high_arm_r.GetCmd());​
 +
 +       error = this->​low_arm_r_joint_->​GetAngle(0).Radian()
 +                 - target_position_ ;
 +       ​this->​pid_j_low_arm_r.Update(error,​ dt);
 +       ​this->​low_arm_r_joint_->​SetForce(0,​ this->​pid_j_low_arm_r.GetCmd());​
 +      error = this->​wrist_r_joint_->​GetAngle(0).Radian()
 +                   - target_position_;​
 +      this->​pid_j_wrist_r.Update(error,​ dt);
 +      this->​wrist_r_joint_->​SetForce(0,​ this->​pid_j_wrist_r.GetCmd());​
 +      ​
 +      error = this->​gripper_r_joint_->​GetAngle(0).Radian()
 +                   - target_position_;​
 +      this->​pid_j_gripper_r.Update(error,​ dt);
 +      this->​gripper_r_joint_->​SetForce(0,​ this->​pid_j_gripper_r.GetCmd());​
 +// head and pelvis ​
 +
 +      error = this->​pan_joint_->​GetAngle(0).Radian()
 +                   - target_position_;​
 +      this->​pid_j_pan.Update(error,​ dt);
 +      this->​pan_joint_->​SetForce(0,​ this->​pid_j_pan.GetCmd());​
 +
 +      error = this->​tilt_joint_->​GetAngle(0).Radian()
 +                   - target_position_;​
 +      this->​pid_j_tilt.Update(error,​ dt);
 +      this->​tilt_joint_->​SetForce(0,​ this->​pid_j_tilt.GetCmd());​
 +
 +      error = this->​pelvis_l_joint_->​GetAngle(0).Radian()
 +                   - target_position_;​
 +      this->​pid_j_pelvis_l.Update(error,​ dt);
 +      this->​pelvis_l_joint_->​SetForce(0,​ this->​pid_j_pelvis_l.GetCmd());​
 +//      gzdbg << " lHip :" << this->​pelvis_l_joint_->​GetAngle(0).Radian() << ". \n";
 +
 +      error = this->​pelvis_r_joint_->​GetAngle(0).Radian()
 +                   - target_position_;​
 +      this->​pid_j_pelvis_r.Update(error,​ dt);
 +      this->​pelvis_r_joint_->​SetForce(0,​ this->​pid_j_pelvis_r.GetCmd());​
 +//      gzdbg << " rHip :" << this->​pelvis_r_joint_->​GetAngle(0).Radian() << ". \n";
 +
 +      if (x1<1)
 +       x1 = x1 + 0.001;
 +      else if ((x1+0.1)<​143)
 +       x1 = x1 + 0.1;
 +      else
 +       x1 = 49;
 +      this->​last_update_time_ = current_time;​
 +
 +    }
 +
 +float lip(float x,float xp[],float yp[])
 +{
 +  float y;
 +  float xp0;
 +  float yp0;
 +  float xp1;
 +  float yp1;
 +  for (int i =0; i<=143; i++)
 +  {
 +    if (xp[i]<​=x)
 +    {
 +      xp0 = xp[i];
 +      yp0 = yp[i];
 +    }
 +    if (xp[i]>​x)
 +    {
 +      xp1 = xp[i];
 +      yp1 = yp[i];
 +      break;
 +    }
 +  }
 +//  gzdbg << "​xp0:​{"​ << xp0  << "} \n";
 +//  gzdbg << "​xp1:​{"​ << xp1  << "} \n";
 +//  gzdbg << "​yp0:​{"​ << yp0  << "} \n";
 +//  gzdbg << "​yp1:​{"​ << yp1  << "} \n";
 +  float m = (yp1-yp0)/​(xp1-xp0);​
 +  y = (yp0 +(m*(x-xp0)));​
 +  return (y);
 +}
 +
 +void ankle2_l(float position, double dt1)
 +{
 +  float error1;
 +  this->​pid_j_ankle2_l.SetCmd(position);​
 +  error1 = this->​ankle2_l_joint_->​GetAngle(0).Radian() - position ;
 +//  gzdbg << ​ "​Current anklel : " << this->​ankle2_l_joint_->​GetAngle(0).Radian() << "​\n";​
 +  this->​pid_j_ankle2_l.Update(error1,​ dt1);
 +  this->​ankle2_l_joint_->​SetForce(0,​ this->​pid_j_ankle2_l.GetCmd());​
 +}
 +
 +void ankle2_r(float position, double dt1)
 +{
 +  float error1;
 +  this->​pid_j_ankle2_r.SetCmd(position);​
 +  error1 = this->​ankle2_r_joint_->​GetAngle(0).Radian() - position ;
 +//  gzdbg << ​ "​Current ankler : " << this->​ankle2_r_joint_->​GetAngle(0).Radian() << "​\n";​
 +  this->​pid_j_ankle2_r.Update(error1,​ dt1);
 +  this->​ankle2_r_joint_->​SetForce(0,​ this->​pid_j_ankle2_r.GetCmd());​
 +}
 +
 +void ankle1_l(float position, double dt1)
 +{
 +  float error1;
 +  this->​pid_j_ankle1_l.SetCmd(position);​
 +  error1 = this->​ankle1_l_joint_->​GetAngle(0).Radian() - position ;
 +//  gzdbg << ​ "​Current footl : " << this->​ankle1_l_joint_->​GetAngle(0).Radian() << "​\n";​
 +  this->​pid_j_ankle1_l.Update(error1,​ dt1);
 +  this->​ankle1_l_joint_->​SetForce(0,​ this->​pid_j_ankle1_l.GetCmd());​
 +}
 +
 +void ankle1_r(float position, double dt1)
 +{
 +  float error1;
 +  this->​pid_j_ankle1_r.SetCmd(position);​
 +  error1 = this->​ankle1_r_joint_->​GetAngle(0).Radian() - position;
 +//  gzdbg << ​ "​Current footr : " << this->​ankle1_r_joint_->​GetAngle(0).Radian()<<​ "​\n";​
 +  this->​pid_j_ankle1_r.Update(error1,​ dt1);
 +  this->​ankle1_r_joint_->​SetForce(0,​ this->​pid_j_ankle1_r.GetCmd());​
 +}
 +
 +void tibia_l(float position, double dt1)
 +{
 +  float error1;
 +  this->​pid_j_tibia_l.SetCmd(position);​
 +  error1 = this->​tibia_l_joint_->​GetAngle(0).Radian() - position;
 +//  gzdbg << ​ "​Current kneel : " << this->​tibia_l_joint_->​GetAngle(0).Radian()<<​ "​\n";​
 +  this->​pid_j_tibia_l.Update(error1,​ dt1);
 +  this->​tibia_l_joint_->​SetForce(0,​ this->​pid_j_tibia_l.GetCmd());​
 +}
 +
 +void tibia_r(float position, double dt1)
 +{
 +  float error1;
 +  this->​pid_j_tibia_r.SetCmd(position);​
 +  error1 = this->​tibia_r_joint_->​GetAngle(0).Radian() - position;
 +//  gzdbg << ​ "​Current kneer : " << this->​tibia_r_joint_->​GetAngle(0).Radian()<<​ "​\n";​
 +  this->​pid_j_tibia_r.Update(error1,​ dt1);
 +  this->​tibia_r_joint_->​SetForce(0,​ this->​pid_j_tibia_r.GetCmd());​
 +}
 +
 +void thigh2_l(float position, double dt1)
 +{
 +  float error1;
 +  this->​pid_j_thigh2_l.SetCmd(position);​
 +  error1 = this->​thigh2_l_joint_->​GetAngle(0).Radian() - position;
 +//  gzdbg << ​ "​Current hipPl : " << this->​thigh2_l_joint_->​GetAngle(0).Radian()<<​ "​\n";​
 +  this->​pid_j_thigh2_l.Update(error1,​ dt1);
 +  this->​thigh2_l_joint_->​SetForce(0,​ this->​pid_j_thigh2_l.GetCmd());​
 +}
 +
 +void thigh2_r(float position, double dt1)
 +{
 +  float error1;
 +  this->​pid_j_thigh2_r.SetCmd(position);​
 +  error1 = this->​thigh2_r_joint_->​GetAngle(0).Radian() - position;
 +//  gzdbg << ​ "​Current hipPr : " << this->​thigh2_r_joint_->​GetAngle(0).Radian()<<​ "​\n";​
 +  this->​pid_j_thigh2_r.Update(error1,​ dt1);
 +  this->​thigh2_r_joint_->​SetForce(0,​ this->​pid_j_thigh2_r.GetCmd());​
 +}
 +
 +void thigh1_l(float position, double dt1)
 +{
 +  float error1;
 +  this->​pid_j_thigh1_l.SetCmd(position);​
 +  error1 = this->​thigh1_l_joint_->​GetAngle(0).Radian() - position;
 +//  gzdbg << ​ "​Current hipRl : " << this->​thigh1_l_joint_->​GetAngle(0).Radian()<<​ "​\n";​
 +  this->​pid_j_thigh1_l.Update(error1,​ dt1);
 +  this->​thigh1_l_joint_->​SetForce(0,​ this->​pid_j_thigh1_l.GetCmd());​
 +}
 +
 +void thigh1_r(float position, double dt1)
 +{
 +  float error1;
 +  this->​pid_j_thigh1_r.SetCmd(position);​
 +  error1 = this->​thigh1_r_joint_->​GetAngle(0).Radian() - position;
 +//  gzdbg << ​ "​Current hipRr : " << this->​thigh1_r_joint_->​GetAngle(0).Radian()<<​ "​\n";​
 +  this->​pid_j_thigh1_r.Update(error1,​ dt1);
 +  this->​thigh1_r_joint_->​SetForce(0,​ this->​pid_j_thigh1_r.GetCmd());​
 +}
 +
 +// PID controllers for each joint :
 +
 +// head
 +
 +    common::PID pid_j_pan;
 +    common::PID pid_j_tilt;
 +
 +// pelvis
 +
 +    common::PID pid_j_pelvis_l;​
 +    common::PID pid_j_pelvis_r;​
 +
 +
 +// leg joints
 +    common::PID pid_j_ankle2_r;​
 +    common::PID pid_j_ankle1_r;​
 +    common::PID pid_j_tibia_r;​
 +    common::PID pid_j_thigh2_r;​
 +    common::PID pid_j_thigh1_r;​
 +
 +    common::PID pid_j_ankle2_l;​
 +    common::PID pid_j_ankle1_l;​
 +    common::PID pid_j_tibia_l;​
 +    common::PID pid_j_thigh2_l;​
 +    common::PID pid_j_thigh1_l;​
 +
 +// left arm 
 +    common::PID pid_j_shoulder_l;​
 +    common::PID pid_j_high_arm_l;​
 +    common::PID pid_j_low_arm_l;​
 +    common::PID pid_j_wrist_l;​
 +    common::PID pid_j_gripper_l;​
 +
 +    common::PID pid_j_shoulder_r;​
 +    common::PID pid_j_high_arm_r;​
 +    common::PID pid_j_low_arm_r;​
 +    common::PID pid_j_wrist_r;​
 +    common::PID pid_j_gripper_r;​
 +
 +
 +// pid controllers for the leg joints
 +
 +    double target_position_;​
 +    double k_;
 +    double elbow_position;​
 +
 +// ** Pointers for each joints
 +
 +// head
 +    physics::​JointPtr pan_joint_;
 +    physics::​JointPtr tilt_joint_;​
 +
 +// pelvis
 +    physics::​JointPtr pelvis_l_joint_;​
 +    physics::​JointPtr pelvis_r_joint_;​
 +
 +
 +// legs
 +    physics::​JointPtr ankle2_r_joint_;​
 +    physics::​JointPtr ankle1_r_joint_;​
 +    physics::​JointPtr tibia_r_joint_;​
 +    physics::​JointPtr thigh2_r_joint_;​
 +    physics::​JointPtr thigh1_r_joint_;​
 +
 +    physics::​JointPtr ankle2_l_joint_;​
 +    physics::​JointPtr ankle1_l_joint_;​
 +    physics::​JointPtr tibia_l_joint_;​
 +    physics::​JointPtr thigh2_l_joint_;​
 +    physics::​JointPtr thigh1_l_joint_;​
 +
 +// arm 
 +
 +    physics::​JointPtr shoulder_l_joint_;​
 +    physics::​JointPtr high_arm_l_joint_;​
 +    physics::​JointPtr low_arm_l_joint_;​
 +    physics::​JointPtr wrist_l_joint_;​
 +    physics::​JointPtr gripper_l_joint_;​
 +
 +    physics::​JointPtr shoulder_r_joint_;​
 +    physics::​JointPtr high_arm_r_joint_;​
 +    physics::​JointPtr low_arm_r_joint_;​
 +    physics::​JointPtr wrist_r_joint_;​
 +    physics::​JointPtr gripper_r_joint_;​
 +
 +    physics::​ModelPtr model_;
 +    event::​ConnectionPtr update_connection_;​
 +    common::​Time last_update_time_;​
 +  };
 +
 +  // Register this plugin with the simulator
 +  GZ_REGISTER_MODEL_PLUGIN(PID1Joints)
 +}
 +
 +</​code>​
 +
 +===== Other Pages =====
 +
 +  * [[drexel_darwin_sim|Darwin-OP Simulation on Webots and DRC-Sim(Gazebo)]]
 +
 +  * [[drexel_darwin_zmp|ZMP based Cycloid walking algorithm for Darwin-OP]]
 +
 +  * [[drexel_darwin_walking_webots|Darwin-OP Walking Simulation on Webots]]
drexel_darwin_walking_gazebo.txt ยท Last modified: 2016/11/06 20:33 by dwallace