kinematics_dynamics.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2018 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 
17 /*
18  * kinematics_dynamics.cpp
19  *
20  * Created on: June 7, 2016
21  * Author: SCH
22  */
23 
24 #include <iostream>
25 #include "thormang3_kinematics_dynamics/kinematics_dynamics.h"
26 
27 namespace thormang3
28 {
29 
30 KinematicsDynamics::KinematicsDynamics() {}
31 KinematicsDynamics::~KinematicsDynamics() {}
32 
33 KinematicsDynamics::KinematicsDynamics(TreeSelect tree)
34 {
35  for (int id=0; id<=ALL_JOINT_ID; id++)
36  thormang3_link_data_[id] = new LinkData();
37 
38  if ( tree == WholeBody )
39  {
40  thormang3_link_data_[0]->name_ = "base";
41  thormang3_link_data_[0]->parent_ = -1;
42  thormang3_link_data_[0]->sibling_ = -1;
43  thormang3_link_data_[0]->child_ = 38;
44  thormang3_link_data_[0]->mass_ = 0.0;
45  thormang3_link_data_[0]->relative_position_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
46  thormang3_link_data_[0]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
47  thormang3_link_data_[0]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
48  thormang3_link_data_[0]->joint_limit_max_ = 100.0;
49  thormang3_link_data_[0]->joint_limit_min_ = -100.0;
50  thormang3_link_data_[0]->inertia_ = robotis_framework::getInertiaXYZ( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 );
51 
52  /* ----- passive joint -----*/
53 
54  thormang3_link_data_[38]->name_ = "passive_x";
55  thormang3_link_data_[38]->parent_ = 0;
56  thormang3_link_data_[38]->sibling_ = -1;
57  thormang3_link_data_[38]->child_ = 39;
58  thormang3_link_data_[38]->mass_ = 0.0;
59  thormang3_link_data_[38]->relative_position_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
60  thormang3_link_data_[38]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
61  thormang3_link_data_[38]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
62  thormang3_link_data_[38]->joint_limit_max_ = 100.0;
63  thormang3_link_data_[38]->joint_limit_min_ = -100.0;
64  thormang3_link_data_[38]->inertia_ = robotis_framework::getInertiaXYZ( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 );
65 
66  thormang3_link_data_[39]->name_ = "passive_y";
67  thormang3_link_data_[39]->parent_ = 38;
68  thormang3_link_data_[39]->sibling_ = -1;
69  thormang3_link_data_[39]->child_ = 40;
70  thormang3_link_data_[39]->mass_ = 0.0;
71  thormang3_link_data_[39]->relative_position_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
72  thormang3_link_data_[39]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
73  thormang3_link_data_[39]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
74  thormang3_link_data_[39]->joint_limit_max_ = 100.0;
75  thormang3_link_data_[39]->joint_limit_min_ = -100.0;
76  thormang3_link_data_[39]->inertia_ = robotis_framework::getInertiaXYZ( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 );
77 
78  thormang3_link_data_[40]->name_ = "passive_z";
79  thormang3_link_data_[40]->parent_ = 39;
80  thormang3_link_data_[40]->sibling_ = -1;
81  thormang3_link_data_[40]->child_ = 43;
82  thormang3_link_data_[40]->mass_ = 0.0;
83  thormang3_link_data_[40]->relative_position_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.723 ); // 0.0 , 0.0 , 0.801
84  thormang3_link_data_[40]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
85  thormang3_link_data_[40]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
86  thormang3_link_data_[40]->joint_limit_max_ = 100.0;
87  thormang3_link_data_[40]->joint_limit_min_ = -100.0;
88  thormang3_link_data_[40]->inertia_ = robotis_framework::getInertiaXYZ( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 );
89 
90  thormang3_link_data_[41]->name_ = "passive_roll";
91  thormang3_link_data_[41]->parent_ = 42;
92  thormang3_link_data_[41]->sibling_ = -1;
93  thormang3_link_data_[41]->child_ = 44;
94  thormang3_link_data_[41]->mass_ = 0.0;
95  thormang3_link_data_[41]->relative_position_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
96  thormang3_link_data_[41]->joint_axis_ = robotis_framework::getTransitionXYZ( 1.0 , 0.0 , 0.0 );
97  thormang3_link_data_[41]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
98  thormang3_link_data_[41]->joint_limit_max_ = 2.0 * M_PI;
99  thormang3_link_data_[41]->joint_limit_min_ = -2.0 * M_PI;
100  thormang3_link_data_[41]->inertia_ = robotis_framework::getInertiaXYZ( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 );
101 
102  thormang3_link_data_[42]->name_ = "passive_pitch";
103  thormang3_link_data_[42]->parent_ = 43;
104  thormang3_link_data_[42]->sibling_ = -1;
105  thormang3_link_data_[42]->child_ = 41;
106  thormang3_link_data_[42]->mass_ = 0.0;
107  thormang3_link_data_[42]->relative_position_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
108  thormang3_link_data_[42]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 1.0 , 0.0 );
109  thormang3_link_data_[42]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
110  thormang3_link_data_[42]->joint_limit_max_ = 2.0 * M_PI;
111  thormang3_link_data_[42]->joint_limit_min_ = -2.0 * M_PI;
112  thormang3_link_data_[42]->inertia_ = robotis_framework::getInertiaXYZ( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 );
113 
114  thormang3_link_data_[43]->name_ = "passive_yaw";
115  thormang3_link_data_[43]->parent_ = 40;
116  thormang3_link_data_[43]->sibling_ = -1;
117  thormang3_link_data_[43]->child_ = 42;
118  thormang3_link_data_[43]->mass_ = 0.0;
119  thormang3_link_data_[43]->relative_position_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
120  thormang3_link_data_[43]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 1.0 );
121  thormang3_link_data_[43]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
122  thormang3_link_data_[43]->joint_limit_max_ = 2.0 * M_PI;
123  thormang3_link_data_[43]->joint_limit_min_ = -2.0 * M_PI;
124  thormang3_link_data_[43]->inertia_ = robotis_framework::getInertiaXYZ( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 );
125 
126  /* ----- body -----*/
127 
128  // pelvis_link
129  thormang3_link_data_[44]->name_ = "pelvis";
130  thormang3_link_data_[44]->parent_ = 41;
131  thormang3_link_data_[44]->sibling_ = -1;
132  thormang3_link_data_[44]->child_ = 27;
133  thormang3_link_data_[44]->mass_ = 6.869;
134  thormang3_link_data_[44]->relative_position_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
135  thormang3_link_data_[44]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
136  thormang3_link_data_[44]->center_of_mass_ = robotis_framework::getTransitionXYZ( -0.011 , 0.000 , 0.058 );
137  thormang3_link_data_[44]->joint_limit_max_ = 100.0;
138  thormang3_link_data_[44]->joint_limit_min_ = -100.0;
139  thormang3_link_data_[44]->inertia_ = robotis_framework::getInertiaXYZ( 0.03603 , 0.00000 , 0.00016 , 0.02210 , 0.00000 , 0.03830 );
140 
141  // chest_link
142  thormang3_link_data_[27]->name_ = "torso_y";
143  thormang3_link_data_[27]->parent_ = 44;
144  thormang3_link_data_[27]->sibling_ = 15;
145  thormang3_link_data_[27]->child_ = 28;
146  thormang3_link_data_[27]->mass_ = 5.383;
147  thormang3_link_data_[27]->relative_position_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.1705 );
148  thormang3_link_data_[27]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 1.0 );
149  thormang3_link_data_[27]->center_of_mass_ = robotis_framework::getTransitionXYZ( -0.007 , 0.000 , 0.109 );
150  thormang3_link_data_[27]->joint_limit_max_ = 0.6 * M_PI;
151  thormang3_link_data_[27]->joint_limit_min_ = -0.6 * M_PI;
152  thormang3_link_data_[27]->inertia_ = robotis_framework::getInertiaXYZ( 0.04710 , 0.00000 , 0.00036 , 0.02554 , 0.00000 , 0.03094 );
153 
154  /* ----- head -----*/
155 
156  // head_yaw
157  thormang3_link_data_[28]->name_ = "head_y";
158  thormang3_link_data_[28]->parent_ = 27;
159  thormang3_link_data_[28]->sibling_ = 1;
160  thormang3_link_data_[28]->child_ = 29;
161  thormang3_link_data_[28]->mass_ = 0.087;
162  thormang3_link_data_[28]->relative_position_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.229 );
163  thormang3_link_data_[28]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 1.0 );
164  thormang3_link_data_[28]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.000 , -0.002 , 0.010 );
165  thormang3_link_data_[28]->joint_limit_max_ = 0.5 * M_PI;
166  thormang3_link_data_[28]->joint_limit_min_ = -0.5 * M_PI;
167  thormang3_link_data_[28]->inertia_ = robotis_framework::getInertiaXYZ( 0.00011 , 0.00000 , 0.00000 , 0.00003 , 0.00000 , 0.00012 );
168 
169  // head_pitch
170  thormang3_link_data_[29]->name_ = "head_p";
171  thormang3_link_data_[29]->parent_ = 28;
172  thormang3_link_data_[29]->sibling_ = -1;
173  thormang3_link_data_[29]->child_ = -1;
174  thormang3_link_data_[29]->mass_ = 0.724;
175  thormang3_link_data_[29]->relative_position_ = robotis_framework::getTransitionXYZ( 0.0 , -0.04500 , 0.03900 );
176  thormang3_link_data_[29]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 1.0 , 0.0 );
177  thormang3_link_data_[29]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.009 , 0.046 , 0.022 );
178  thormang3_link_data_[29]->joint_limit_max_ = 0.5 * M_PI;
179  thormang3_link_data_[29]->joint_limit_min_ = -0.5 * M_PI;
180  thormang3_link_data_[29]->inertia_ = robotis_framework::getInertiaXYZ( 0.00113 , 0.00001 , -0.00005 , 0.00114 , 0.00002 , 0.00084 );
181 
182  /*----- right arm -----*/
183 
184  // right arm shoulder pitch 1
185  thormang3_link_data_[1]->name_ = "r_arm_sh_p1";
186  thormang3_link_data_[1]->parent_ = 27;
187  thormang3_link_data_[1]->sibling_ = 2;
188  thormang3_link_data_[1]->child_ = 3;
189  thormang3_link_data_[1]->mass_ = 0.194;
190  thormang3_link_data_[1]->relative_position_ = robotis_framework::getTransitionXYZ( 0.000 , -0.152 , 0.160 );
191  thormang3_link_data_[1]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , -1.0 , 0.0 );
192  thormang3_link_data_[1]->center_of_mass_ = robotis_framework::getTransitionXYZ( -0.003 , -0.020 , -0.005 );
193  thormang3_link_data_[1]->joint_limit_max_ = 0.65 * M_PI;
194  thormang3_link_data_[1]->joint_limit_min_ = -0.65 * M_PI;
195  thormang3_link_data_[1]->inertia_ = robotis_framework::getInertiaXYZ( 0.00018 , 0.0 , 0.0 , 0.00058 , -0.00004 , 0.00057 );
196 
197  // right arm shoulder roll
198  thormang3_link_data_[3]->name_ = "r_arm_sh_r";
199  thormang3_link_data_[3]->parent_ = 1;
200  thormang3_link_data_[3]->sibling_ = -1;
201  thormang3_link_data_[3]->child_ = 5;
202  thormang3_link_data_[3]->mass_ = 0.875;
203  thormang3_link_data_[3]->relative_position_ = robotis_framework::getTransitionXYZ( 0.057 , -0.060 , -0.039 );
204  thormang3_link_data_[3]->joint_axis_ = robotis_framework::getTransitionXYZ( -1.0 , 0.0 , 0.0 );
205  thormang3_link_data_[3]->center_of_mass_ = robotis_framework::getTransitionXYZ( -0.060 , -0.002 , 0.000 );
206  thormang3_link_data_[3]->joint_limit_max_ = 0.65 * M_PI;
207  thormang3_link_data_[3]->joint_limit_min_ = -0.65 * M_PI;
208  thormang3_link_data_[3]->inertia_ = robotis_framework::getInertiaXYZ( 0.00043 , 0.00000 , 0.00000 , 0.00112 , 0.00000 , 0.00113 );
209 
210  // right arm shoulder pitch 2
211  thormang3_link_data_[5]->name_ = "r_arm_sh_p2";
212  thormang3_link_data_[5]->parent_ = 3;
213  thormang3_link_data_[5]->sibling_ = -1;
214  thormang3_link_data_[5]->child_ = 7;
215  thormang3_link_data_[5]->mass_ = 1.122;
216  thormang3_link_data_[5]->relative_position_ = robotis_framework::getTransitionXYZ( -0.057 , -0.033 , 0.000 );
217  thormang3_link_data_[5]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , -1.0 , 0.0 );
218  thormang3_link_data_[5]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.000 , -0.073 , 0.000 );
219  thormang3_link_data_[5]->joint_limit_max_ = 0.65 * M_PI;
220  thormang3_link_data_[5]->joint_limit_min_ = -0.65 * M_PI;
221  thormang3_link_data_[5]->inertia_ = robotis_framework::getInertiaXYZ( 0.00277 , 0.00002 , -0.00001 , 0.00090 , 0.00004 , 0.00255 );
222 
223  // right arm elbow yaw
224  thormang3_link_data_[7]->name_ = "r_arm_el_y";
225  thormang3_link_data_[7]->parent_ = 5;
226  thormang3_link_data_[7]->sibling_ = -1;
227  thormang3_link_data_[7]->child_ = 9;
228  thormang3_link_data_[7]->mass_ = 1.357;
229  thormang3_link_data_[7]->relative_position_ = robotis_framework::getTransitionXYZ( 0.03000 , -0.18700 , 0.05700 );
230  thormang3_link_data_[7]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , -1.0 );
231  thormang3_link_data_[7]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.042 , -0.012 , -0.058 );
232  thormang3_link_data_[7]->joint_limit_max_ = 0.45 * M_PI;
233  thormang3_link_data_[7]->joint_limit_min_ = -0.45 * M_PI;
234  thormang3_link_data_[7]->inertia_ = robotis_framework::getInertiaXYZ( 0.00152 , 0.00100 , -0.00006 , 0.00560 , 0.00002 , 0.00528 );
235 
236  // right arm wrist roll
237  thormang3_link_data_[9]->name_ = "r_arm_wr_r";
238  thormang3_link_data_[9]->parent_ = 7;
239  thormang3_link_data_[9]->sibling_ = -1;
240  thormang3_link_data_[9]->child_ = 11;
241  thormang3_link_data_[9]->mass_ = 0.087;
242  thormang3_link_data_[9]->relative_position_ = robotis_framework::getTransitionXYZ( 0.171 , -0.030 , -0.057 );
243  thormang3_link_data_[9]->joint_axis_ = robotis_framework::getTransitionXYZ( 1.0 , 0.0 , 0.0 );
244  thormang3_link_data_[9]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.010 , 0.000 , -0.002 );
245  thormang3_link_data_[9]->joint_limit_max_ = 0.9 * M_PI;
246  thormang3_link_data_[9]->joint_limit_min_ = -0.9 * M_PI;
247  thormang3_link_data_[9]->inertia_ = robotis_framework::getInertiaXYZ( 0.00012 , 0.00000 , 0.00000 , 0.00011 , 0.00000 , 0.00003 );
248 
249  // right arm wrist yaw
250  thormang3_link_data_[11]->name_ = "r_arm_wr_y";
251  thormang3_link_data_[11]->parent_ = 9;
252  thormang3_link_data_[11]->sibling_ = -1;
253  thormang3_link_data_[11]->child_ = 13;
254  thormang3_link_data_[11]->mass_ = 0.768;
255  thormang3_link_data_[11]->relative_position_ = robotis_framework::getTransitionXYZ( 0.039 , 0.000 , 0.045 );
256  thormang3_link_data_[11]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , -1.0 );
257  thormang3_link_data_[11]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.023 , -0.001 , -0.046 );
258  thormang3_link_data_[11]->joint_limit_max_ = 0.45 * M_PI;
259  thormang3_link_data_[11]->joint_limit_min_ = -0.45 * M_PI;
260  thormang3_link_data_[11]->inertia_ = robotis_framework::getInertiaXYZ( 0.00059 , 0.00002 , -0.00002 , 0.00078 , 0.00000 , 0.00078 );
261 
262  // right arm wrist pitch
263  thormang3_link_data_[13]->name_ = "r_arm_wr_p";
264  thormang3_link_data_[13]->parent_ = 11;
265  thormang3_link_data_[13]->sibling_ = -1;
266  thormang3_link_data_[13]->child_ = 31;
267  thormang3_link_data_[13]->mass_ = 0.565;
268  thormang3_link_data_[13]->relative_position_ = robotis_framework::getTransitionXYZ( 0.045 , 0.045 , -0.045 );
269  thormang3_link_data_[13]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 1.0 , 0.0 );
270  thormang3_link_data_[13]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.065 , -0.045 , 0.000 );
271  thormang3_link_data_[13]->joint_limit_max_ = 0.45 * M_PI;
272  thormang3_link_data_[13]->joint_limit_min_ = -0.45 * M_PI;
273  thormang3_link_data_[13]->inertia_ = robotis_framework::getInertiaXYZ( 0.00047 , 0.00001 , 0.00000 , 0.00042 , 0.00000 , 0.00058 );
274 
275  // right arm gripper
276  thormang3_link_data_[31]->name_ = "r_arm_grip";
277  thormang3_link_data_[31]->parent_ = 13;
278  thormang3_link_data_[31]->sibling_ = 33;
279  thormang3_link_data_[31]->child_ = -1;
280  thormang3_link_data_[31]->mass_ = 0.013;
281  thormang3_link_data_[31]->relative_position_ = robotis_framework::getTransitionXYZ( 0.088 , -0.058 , 0.000 );
282  thormang3_link_data_[31]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 1.0 );
283  thormang3_link_data_[31]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
284  thormang3_link_data_[31]->joint_limit_max_ = 0.5 * M_PI;
285  thormang3_link_data_[31]->joint_limit_min_ = -0.5 * M_PI;
286  thormang3_link_data_[31]->inertia_ = robotis_framework::getInertiaXYZ( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 );
287 
288  // right arm gripper 1
289  thormang3_link_data_[33]->name_ = "r_arm_grip_1";
290  thormang3_link_data_[33]->parent_ = 13;
291  thormang3_link_data_[33]->sibling_ = 35;
292  thormang3_link_data_[33]->child_ = -1;
293  thormang3_link_data_[33]->mass_ = 0.013;
294  thormang3_link_data_[33]->relative_position_ = robotis_framework::getTransitionXYZ( 0.088 , -0.032 , 0.000 );
295  thormang3_link_data_[33]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 1.0 );
296  thormang3_link_data_[33]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
297  thormang3_link_data_[33]->joint_limit_max_ = 0.5 * M_PI;
298  thormang3_link_data_[33]->joint_limit_min_ = -0.5 * M_PI;
299  thormang3_link_data_[33]->inertia_ = robotis_framework::getInertiaXYZ( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 );
300 
301  // right arm end effector
302  thormang3_link_data_[35]->name_ = "r_arm_end";
303  thormang3_link_data_[35]->parent_ = 13;
304  thormang3_link_data_[35]->sibling_ = -1;
305  thormang3_link_data_[35]->child_ = -1;
306  thormang3_link_data_[35]->mass_ = 0.0;
307  thormang3_link_data_[35]->relative_position_ = robotis_framework::getTransitionXYZ( 0.145 , -0.045 , 0.0 );
308  thormang3_link_data_[35]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
309  thormang3_link_data_[35]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
310  thormang3_link_data_[35]->joint_limit_max_ = 100.0;
311  thormang3_link_data_[35]->joint_limit_min_ = -100.0;
312  thormang3_link_data_[35]->inertia_ = robotis_framework::getInertiaXYZ( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 );
313 
314  /*----- left arm -----*/
315 
316  // left arm shoulder pitch 1
317  thormang3_link_data_[2]->name_ = "l_arm_sh_p1";
318  thormang3_link_data_[2]->parent_ = 27;
319  thormang3_link_data_[2]->sibling_ = -1;
320  thormang3_link_data_[2]->child_ = 4;
321  thormang3_link_data_[2]->mass_ = 0.194;
322  thormang3_link_data_[2]->relative_position_ = robotis_framework::getTransitionXYZ( 0.000 , 0.152 , 0.160 );
323  thormang3_link_data_[2]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 1.0 , 0.0 );
324  thormang3_link_data_[2]->center_of_mass_ = robotis_framework::getTransitionXYZ( -0.003 , 0.020 , -0.005 );
325  thormang3_link_data_[2]->joint_limit_max_ = 0.65 * M_PI;
326  thormang3_link_data_[2]->joint_limit_min_ = -0.65 * M_PI;
327  thormang3_link_data_[2]->inertia_ = robotis_framework::getInertiaXYZ( 0.00018 , 0.00000 , 0.00000 , 0.00058 , 0.00004 , 0.00057 );
328 
329  // left arm shoulder roll
330  thormang3_link_data_[4]->name_ = "l_arm_sh_r";
331  thormang3_link_data_[4]->parent_ = 2;
332  thormang3_link_data_[4]->sibling_ = -1;
333  thormang3_link_data_[4]->child_ = 6;
334  thormang3_link_data_[4]->mass_ = 0.875;
335  thormang3_link_data_[4]->relative_position_ = robotis_framework::getTransitionXYZ( 0.057 , 0.060 , -0.039 );
336  thormang3_link_data_[4]->joint_axis_ = robotis_framework::getTransitionXYZ( -1.0 , 0.0 , 0.0 );
337  thormang3_link_data_[4]->center_of_mass_ = robotis_framework::getTransitionXYZ( -0.060 , 0.002 , 0.000 );
338  thormang3_link_data_[4]->joint_limit_max_ = 0.65 * M_PI;
339  thormang3_link_data_[4]->joint_limit_min_ = -0.65 * M_PI;
340  thormang3_link_data_[4]->inertia_ = robotis_framework::getInertiaXYZ( 0.00043 , 0.00000 , 0.00000 , 0.00112 , 0.00000 , 0.00113 );
341 
342  // left arm shoulder pitch 2
343  thormang3_link_data_[6]->name_ = "l_arm_sh_p2";
344  thormang3_link_data_[6]->parent_ = 4;
345  thormang3_link_data_[6]->sibling_ = -1;
346  thormang3_link_data_[6]->child_ = 8;
347  thormang3_link_data_[6]->mass_ = 1.122;
348  thormang3_link_data_[6]->relative_position_ = robotis_framework::getTransitionXYZ( -0.057 , 0.033 , 0.000 );
349  thormang3_link_data_[6]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 1.0 , 0.0 );
350  thormang3_link_data_[6]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.000 , 0.073 , 0.000 );
351  thormang3_link_data_[6]->joint_limit_max_ = 0.65 * M_PI;
352  thormang3_link_data_[6]->joint_limit_min_ = -0.65 * M_PI;
353  thormang3_link_data_[6]->inertia_ = robotis_framework::getInertiaXYZ( 0.00277 , -0.00002 , -0.00001 , 0.00090 , -0.00004 , 0.00255 );
354 
355  // left arm elbow yaw
356  thormang3_link_data_[8]->name_ = "l_arm_el_y";
357  thormang3_link_data_[8]->parent_ = 6;
358  thormang3_link_data_[8]->sibling_ = -1;
359  thormang3_link_data_[8]->child_ = 10;
360  thormang3_link_data_[8]->mass_ = 1.357;
361  thormang3_link_data_[8]->relative_position_ = robotis_framework::getTransitionXYZ( 0.030 , 0.187 , 0.057 );
362  thormang3_link_data_[8]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , -1.0 );
363  thormang3_link_data_[8]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.042 , 0.012 , -0.058 );
364  thormang3_link_data_[8]->joint_limit_max_ = 0.45 * M_PI;
365  thormang3_link_data_[8]->joint_limit_min_ = -0.45 * M_PI;
366  thormang3_link_data_[8]->inertia_ = robotis_framework::getInertiaXYZ( 0.00152 , -0.00100 , -0.00006 , 0.00560 , -0.00002 , 0.00528 );
367 
368  // left arm wrist roll
369  thormang3_link_data_[10]->name_ = "l_arm_wr_r";
370  thormang3_link_data_[10]->parent_ = 8;
371  thormang3_link_data_[10]->sibling_ = -1;
372  thormang3_link_data_[10]->child_ = 12;
373  thormang3_link_data_[10]->mass_ = 0.087;
374  thormang3_link_data_[10]->relative_position_ = robotis_framework::getTransitionXYZ( 0.171 , 0.030 , -0.057 );
375  thormang3_link_data_[10]->joint_axis_ = robotis_framework::getTransitionXYZ( 1.0 , 0.0 , 0.0 );
376  thormang3_link_data_[10]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.010 , 0.000 , 0.002 );
377  thormang3_link_data_[10]->joint_limit_max_ = 0.9 * M_PI;
378  thormang3_link_data_[10]->joint_limit_min_ = -0.9 * M_PI;
379  thormang3_link_data_[10]->inertia_ = robotis_framework::getInertiaXYZ( 0.00012 , 0.00000 , 0.00000 , 0.00011 , 0.00000 , 0.00003 );
380 
381  // left arm wrist yaw
382  thormang3_link_data_[12]->name_ = "l_arm_wr_y";
383  thormang3_link_data_[12]->parent_ = 10;
384  thormang3_link_data_[12]->sibling_ = -1;
385  thormang3_link_data_[12]->child_ = 14;
386  thormang3_link_data_[12]->mass_ = 0.768;
387  thormang3_link_data_[12]->relative_position_ = robotis_framework::getTransitionXYZ( 0.039 , 0.000 , 0.045 );
388  thormang3_link_data_[12]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , -1.0 );
389  thormang3_link_data_[12]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.023 , 0.001 , -0.046 );
390  thormang3_link_data_[12]->joint_limit_max_ = 0.45 * M_PI;
391  thormang3_link_data_[12]->joint_limit_min_ = -0.45 * M_PI;
392  thormang3_link_data_[12]->inertia_ = robotis_framework::getInertiaXYZ( 0.00059 , -0.00002 , -0.00002 , 0.00078 , 0.00000 , 0.00078 );
393 
394  // left arm wrist pitch
395  thormang3_link_data_[14]->name_ = "l_arm_wr_p";
396  thormang3_link_data_[14]->parent_ = 12;
397  thormang3_link_data_[14]->sibling_ = -1;
398  thormang3_link_data_[14]->child_ = 30;
399  thormang3_link_data_[14]->mass_ = 0.565;
400  thormang3_link_data_[14]->relative_position_ = robotis_framework::getTransitionXYZ( 0.045 , -0.045 , -0.045 );
401  thormang3_link_data_[14]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , -1.0 , 0.0 );
402  thormang3_link_data_[14]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.065 , 0.045 , 0.000 );
403  thormang3_link_data_[14]->joint_limit_max_ = 0.45 * M_PI;
404  thormang3_link_data_[14]->joint_limit_min_ = -0.45 * M_PI;
405  thormang3_link_data_[14]->inertia_ = robotis_framework::getInertiaXYZ( 0.00047 , -0.00001 , 0.00000 , 0.00042 , 0.00000 , 0.00058 );
406 
407  // left arm gripper
408  thormang3_link_data_[30]->name_ = "l_arm_grip";
409  thormang3_link_data_[30]->parent_ = 14;
410  thormang3_link_data_[30]->sibling_ = 32;
411  thormang3_link_data_[30]->child_ = -1;
412  thormang3_link_data_[30]->mass_ = 0.013;
413  thormang3_link_data_[30]->relative_position_ = robotis_framework::getTransitionXYZ( 0.088 , 0.058 , 0.000 );
414  thormang3_link_data_[30]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 1.0 );
415  thormang3_link_data_[30]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
416  thormang3_link_data_[30]->joint_limit_max_ = 0.5 * M_PI;
417  thormang3_link_data_[30]->joint_limit_min_ = -0.5 * M_PI;
418  thormang3_link_data_[30]->inertia_ = robotis_framework::getInertiaXYZ( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 );
419 
420  // left arm gripper_1
421  thormang3_link_data_[32]->name_ = "l_arm_grip_1";
422  thormang3_link_data_[32]->parent_ = 14;
423  thormang3_link_data_[32]->sibling_ = 34;
424  thormang3_link_data_[32]->child_ = -1;
425  thormang3_link_data_[32]->mass_ = 0.013;
426  thormang3_link_data_[32]->relative_position_ = robotis_framework::getTransitionXYZ( 0.088 , 0.032 , 0.000 );
427  thormang3_link_data_[32]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 1.0 );
428  thormang3_link_data_[32]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
429  thormang3_link_data_[32]->joint_limit_max_ = 0.5 * M_PI;
430  thormang3_link_data_[32]->joint_limit_min_ = -0.5 * M_PI;
431  thormang3_link_data_[32]->inertia_ = robotis_framework::getInertiaXYZ( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 );
432 
433  // left arm end effector
434  thormang3_link_data_[34]->name_ = "l_arm_end";
435  thormang3_link_data_[34]->parent_ = 14;
436  thormang3_link_data_[34]->sibling_ = -1;
437  thormang3_link_data_[34]->child_ = -1;
438  thormang3_link_data_[34]->mass_ = 0.0;
439  thormang3_link_data_[34]->relative_position_ = robotis_framework::getTransitionXYZ( 0.145 , 0.045 , 0.0 );
440  thormang3_link_data_[34]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
441  thormang3_link_data_[34]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
442  thormang3_link_data_[34]->joint_limit_max_ = 100.0;
443  thormang3_link_data_[34]->joint_limit_min_ = -100.0;
444  thormang3_link_data_[34]->inertia_ = robotis_framework::getInertiaXYZ( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 );
445 
446  /* ----- right leg -----*/
447 
448  // right leg hip yaw
449  thormang3_link_data_[15]->name_ = "r_leg_hip_y";
450  thormang3_link_data_[15]->parent_ = 44;
451  thormang3_link_data_[15]->sibling_ = 16;
452  thormang3_link_data_[15]->child_ = 17;
453  thormang3_link_data_[15]->mass_ = 0.243;
454  thormang3_link_data_[15]->relative_position_ = robotis_framework::getTransitionXYZ( 0.000 , -0.093 , -0.018 );
455  thormang3_link_data_[15]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , -1.0 );
456  thormang3_link_data_[15]->center_of_mass_ = robotis_framework::getTransitionXYZ( -0.012 , 0.000 , -0.025 );
457  thormang3_link_data_[15]->joint_limit_max_ = 2.0 * M_PI; //0.45 * M_PI;
458  thormang3_link_data_[15]->joint_limit_min_ = -2.0 * M_PI; //-0.45 * M_PI;
459  thormang3_link_data_[15]->inertia_ = robotis_framework::getInertiaXYZ( 0.00024 , 0.00000 , 0.00000 , 0.00101 , 0.00000 , 0.00092 );
460 
461  // right leg hip roll
462  thormang3_link_data_[17]->name_ = "r_leg_hip_r";
463  thormang3_link_data_[17]->parent_ = 15;
464  thormang3_link_data_[17]->sibling_ = -1;
465  thormang3_link_data_[17]->child_ = 19;
466  thormang3_link_data_[17]->mass_ = 1.045;
467  thormang3_link_data_[17]->relative_position_ = robotis_framework::getTransitionXYZ( 0.057 , 0.000 , -0.075 );
468  thormang3_link_data_[17]->joint_axis_ = robotis_framework::getTransitionXYZ( -1.0 , 0.0 , 0.0 );
469  thormang3_link_data_[17]->center_of_mass_ = robotis_framework::getTransitionXYZ( -0.068 , 0.000 , 0.000 );
470  thormang3_link_data_[17]->joint_limit_max_ = 2.0 * M_PI; //0.3 * M_PI;
471  thormang3_link_data_[17]->joint_limit_min_ = -2.0 * M_PI; //-0.3 * M_PI;
472  thormang3_link_data_[17]->inertia_ = robotis_framework::getInertiaXYZ( 0.00056 , 0.00000 , 0.00000 , 0.00168 , 0.00000 , 0.00171 );
473 
474  // right leg hip pitch
475  thormang3_link_data_[19]->name_ = "r_leg_hip_p";
476  thormang3_link_data_[19]->parent_ = 17;
477  thormang3_link_data_[19]->sibling_ = -1;
478  thormang3_link_data_[19]->child_ = 21;
479  thormang3_link_data_[19]->mass_ = 3.095;
480  thormang3_link_data_[19]->relative_position_ = robotis_framework::getTransitionXYZ( -0.057 , -0.033 , 0.000 );
481  thormang3_link_data_[19]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , -1.0 , 0.0 );
482  thormang3_link_data_[19]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.022 , 0.007 , -0.168 );
483  thormang3_link_data_[19]->joint_limit_max_ = 2.0 * M_PI; //0.4 * M_PI;
484  thormang3_link_data_[19]->joint_limit_min_ = -2.0 * M_PI; //-0.4 * M_PI;
485  thormang3_link_data_[19]->inertia_ = robotis_framework::getInertiaXYZ( 0.04329 , -0.00027 , 0.00286 , 0.04042 , 0.00203 , 0.00560 );
486 
487  // right leg knee pitch
488  thormang3_link_data_[21]->name_ = "r_leg_kn_p";
489  thormang3_link_data_[21]->parent_ = 19;
490  thormang3_link_data_[21]->sibling_ = -1;
491  thormang3_link_data_[21]->child_ = 23;
492  thormang3_link_data_[21]->mass_ = 2.401;
493  thormang3_link_data_[21]->relative_position_ = robotis_framework::getTransitionXYZ( 0.000 , -0.060 , -0.300 );
494  thormang3_link_data_[21]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , -1.0 , 0.0 );
495  thormang3_link_data_[21]->center_of_mass_ = robotis_framework::getTransitionXYZ( -0.002 , 0.066 , -0.183 );
496  thormang3_link_data_[21]->joint_limit_max_ = 2.0 * M_PI; //0.1 * M_PI;
497  thormang3_link_data_[21]->joint_limit_min_ = -2.0 * M_PI; //-0.7 * M_PI;
498  thormang3_link_data_[21]->inertia_ = robotis_framework::getInertiaXYZ( 0.01971 , -0.00031 , -0.00294 , 0.01687 , -0.00140 , 0.00574 );
499 
500  // right leg ankle pitch
501  thormang3_link_data_[23]->name_ = "r_leg_an_p";
502  thormang3_link_data_[23]->parent_ = 21;
503  thormang3_link_data_[23]->sibling_ = -1;
504  thormang3_link_data_[23]->child_ = 25;
505  thormang3_link_data_[23]->mass_ = 1.045;
506  thormang3_link_data_[23]->relative_position_ = robotis_framework::getTransitionXYZ( 0.000 , 0.060 , -0.300 );
507  thormang3_link_data_[23]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 1.0 , 0.0 );
508  thormang3_link_data_[23]->center_of_mass_ = robotis_framework::getTransitionXYZ( -0.011 , 0.033 , 0.000 );
509  thormang3_link_data_[23]->joint_limit_max_ = 2.0 * M_PI; //0.45 * M_PI;
510  thormang3_link_data_[23]->joint_limit_min_ = -2.0 * M_PI; //-0.45 * M_PI;
511  thormang3_link_data_[23]->inertia_ = robotis_framework::getInertiaXYZ( 0.00056 , 0.00000 , 0.00000 , 0.00168 , 0.00000 , 0.00171 );
512 
513  // right leg ankle roll
514  thormang3_link_data_[25]->name_ = "r_leg_an_r";
515  thormang3_link_data_[25]->parent_ = 23;
516  thormang3_link_data_[25]->sibling_ = -1;
517  thormang3_link_data_[25]->child_ = 37;
518  thormang3_link_data_[25]->mass_ = 0.223;
519  thormang3_link_data_[25]->relative_position_ = robotis_framework::getTransitionXYZ( 0.057 , 0.033 , 0.000 );
520  thormang3_link_data_[25]->joint_axis_ = robotis_framework::getTransitionXYZ( 1.0 , 0.0 , 0.0 );
521  thormang3_link_data_[25]->center_of_mass_ = robotis_framework::getTransitionXYZ( -0.070 , 0.000 , -0.048 );
522  thormang3_link_data_[25]->joint_limit_max_ = 2.0 * M_PI; //0.45 * M_PI;
523  thormang3_link_data_[25]->joint_limit_min_ = -2.0 * M_PI; //-0.45 * M_PI;
524  thormang3_link_data_[25]->inertia_ = robotis_framework::getInertiaXYZ( 0.00022 , 0.00000 , -0.00001 , 0.00099 , 0.00000 , 0.00091 );
525 
526  // right leg ft
527  thormang3_link_data_[37]->name_ = "r_leg_ft";
528  thormang3_link_data_[37]->parent_ = 25;
529  thormang3_link_data_[37]->sibling_ = -1;
530  thormang3_link_data_[37]->child_ = 45;
531  thormang3_link_data_[37]->mass_ = 1.689;
532  thormang3_link_data_[37]->relative_position_ = robotis_framework::getTransitionXYZ( -0.057 , 0.000 , -0.087 );
533  thormang3_link_data_[37]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
534  thormang3_link_data_[37]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.000 , -0.009 , -0.013 );
535  thormang3_link_data_[37]->joint_limit_max_ = 100.0;
536  thormang3_link_data_[37]->joint_limit_min_ = -100.0;
537  thormang3_link_data_[37]->inertia_ = robotis_framework::getInertiaXYZ( 0.00219 , 0.00000 , 0.00000 , 0.00433 , -0.00011 , 0.00609 );
538 
539  // right leg end
540  thormang3_link_data_[45]->name_ = "r_leg_end";
541  thormang3_link_data_[45]->parent_ = 37;
542  thormang3_link_data_[45]->sibling_ = -1;
543  thormang3_link_data_[45]->child_ = -1;
544  thormang3_link_data_[45]->mass_ = 0.0;
545  thormang3_link_data_[45]->relative_position_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , -0.0275 );
546  thormang3_link_data_[45]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
547  thormang3_link_data_[45]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
548  thormang3_link_data_[45]->joint_limit_max_ = 100.0;
549  thormang3_link_data_[45]->joint_limit_min_ = -100.0;
550  thormang3_link_data_[45]->inertia_ = robotis_framework::getInertiaXYZ( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 );
551 
552  /* ----- left leg -----*/
553 
554  // left leg hip yaw
555  thormang3_link_data_[16]->name_ = "l_leg_hip_y";
556  thormang3_link_data_[16]->parent_ = 44;
557  thormang3_link_data_[16]->sibling_ = -1;
558  thormang3_link_data_[16]->child_ = 18;
559  thormang3_link_data_[16]->mass_ = 0.243;
560  thormang3_link_data_[16]->relative_position_ = robotis_framework::getTransitionXYZ( 0.000 , 0.093 , -0.018 );
561  thormang3_link_data_[16]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , -1.0 );
562  thormang3_link_data_[16]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.012 , 0.000 , -0.025 );
563  thormang3_link_data_[16]->joint_limit_max_ = 2.0 * M_PI; //0.45 * M_PI;
564  thormang3_link_data_[16]->joint_limit_min_ = -2.0 * M_PI; //-0.45 * M_PI;
565  thormang3_link_data_[16]->inertia_ = robotis_framework::getInertiaXYZ( 0.00024 , 0.00000 , 0.00000 , 0.00101 , 0.00000 , 0.00092 );
566 
567  // left leg hip roll
568  thormang3_link_data_[18]->name_ = "l_leg_hip_r";
569  thormang3_link_data_[18]->parent_ = 16;
570  thormang3_link_data_[18]->sibling_ = -1;
571  thormang3_link_data_[18]->child_ = 20;
572  thormang3_link_data_[18]->mass_ = 1.045;
573  thormang3_link_data_[18]->relative_position_ = robotis_framework::getTransitionXYZ( 0.057 , 0.000 , -0.075 );
574  thormang3_link_data_[18]->joint_axis_ = robotis_framework::getTransitionXYZ( -1.0 , 0.0 , 0.0 );
575  thormang3_link_data_[18]->center_of_mass_ = robotis_framework::getTransitionXYZ( -0.068 , 0.000 , 0.000 );
576  thormang3_link_data_[18]->joint_limit_max_ = 2.0 * M_PI; //0.3 * M_PI;
577  thormang3_link_data_[18]->joint_limit_min_ = -2.0 * M_PI; //-0.3 * M_PI;
578  thormang3_link_data_[18]->inertia_ = robotis_framework::getInertiaXYZ( 0.00056 , 0.00000 , 0.00000 , 0.00168 , 0.00000 , 0.00171 );
579 
580  // left leg hip pitch
581  thormang3_link_data_[20]->name_ = "l_leg_hip_p";
582  thormang3_link_data_[20]->parent_ = 18;
583  thormang3_link_data_[20]->sibling_ = -1;
584  thormang3_link_data_[20]->child_ = 22;
585  thormang3_link_data_[20]->mass_ = 3.095;
586  thormang3_link_data_[20]->relative_position_ = robotis_framework::getTransitionXYZ( -0.057 , 0.033 , 0.000 );
587  thormang3_link_data_[20]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 1.0 , 0.0 );
588  thormang3_link_data_[20]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.022 , -0.007 , -0.168 );
589  thormang3_link_data_[20]->joint_limit_max_ = 2.0 * M_PI; //0.4 * M_PI;
590  thormang3_link_data_[20]->joint_limit_min_ = -2.0 * M_PI; //-0.4 * M_PI;
591  thormang3_link_data_[20]->inertia_ = robotis_framework::getInertiaXYZ( 0.04328 , 0.00028 , 0.00288 , 0.04042 , -0.00202 , 0.00560 );
592 
593  // left leg knee pitch
594  thormang3_link_data_[22]->name_ = "l_leg_kn_p";
595  thormang3_link_data_[22]->parent_ = 20;
596  thormang3_link_data_[22]->sibling_ = -1;
597  thormang3_link_data_[22]->child_ = 24;
598  thormang3_link_data_[22]->mass_ = 2.401;
599  thormang3_link_data_[22]->relative_position_ = robotis_framework::getTransitionXYZ( 0.000 , 0.060 , -0.300 );
600  thormang3_link_data_[22]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 1.0 , 0.0 );
601  thormang3_link_data_[22]->center_of_mass_ = robotis_framework::getTransitionXYZ( -0.002 , -0.066 , -0.183 );
602  thormang3_link_data_[22]->joint_limit_max_ = 2.0 * M_PI; //0.7 * M_PI;
603  thormang3_link_data_[22]->joint_limit_min_ = -2.0 * M_PI; //-0.1 * M_PI;
604  thormang3_link_data_[22]->inertia_ = robotis_framework::getInertiaXYZ( 0.01971 , 0.00031 , -0.00294 , 0.01687 , 0.00140 , 0.00574 );
605 
606  // left leg ankle pitch
607  thormang3_link_data_[24]->name_ = "l_leg_an_p";
608  thormang3_link_data_[24]->parent_ = 22;
609  thormang3_link_data_[24]->sibling_ = -1;
610  thormang3_link_data_[24]->child_ = 26;
611  thormang3_link_data_[24]->mass_ = 1.045;
612  thormang3_link_data_[24]->relative_position_ = robotis_framework::getTransitionXYZ( 0.000 , -0.060 , -0.300 );
613  thormang3_link_data_[24]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , -1.0 , 0.0 );
614  thormang3_link_data_[24]->center_of_mass_ = robotis_framework::getTransitionXYZ( -0.011 , -0.033 , 0.000 );
615  thormang3_link_data_[24]->joint_limit_max_ = 2.0 * M_PI; //0.45 * M_PI;
616  thormang3_link_data_[24]->joint_limit_min_ = -2.0 * M_PI; //-0.45 * M_PI;
617  thormang3_link_data_[24]->inertia_ = robotis_framework::getInertiaXYZ( 0.00056 , 0.00000 , 0.00000 , 0.00168 , 0.00000 , 0.00171 );
618 
619  // left leg ankle pitch
620  thormang3_link_data_[26]->name_ = "l_leg_an_r";
621  thormang3_link_data_[26]->parent_ = 24;
622  thormang3_link_data_[26]->sibling_ = -1;
623  thormang3_link_data_[26]->child_ = 36;
624  thormang3_link_data_[26]->mass_ = 0.223;
625  thormang3_link_data_[26]->relative_position_ = robotis_framework::getTransitionXYZ( 0.057 , -0.033 , 0.000 );
626  thormang3_link_data_[26]->joint_axis_ = robotis_framework::getTransitionXYZ( 1.0 , 0.0 , 0.0 );
627  thormang3_link_data_[26]->center_of_mass_ = robotis_framework::getTransitionXYZ( -0.070 , 0.000 , -0.048 );
628  thormang3_link_data_[26]->joint_limit_max_ = 2.0 * M_PI; //0.45 * M_PI;
629  thormang3_link_data_[26]->joint_limit_min_ = -2.0 * M_PI; //-0.45 * M_PI;
630  thormang3_link_data_[26]->inertia_ = robotis_framework::getInertiaXYZ( 0.00022 , 0.00000 , -0.00001 , 0.00099 , 0.00000 , 0.00091 );
631 
632  // left leg ft
633  thormang3_link_data_[36]->name_ = "l_leg_ft";
634  thormang3_link_data_[36]->parent_ = 26;
635  thormang3_link_data_[36]->sibling_ = -1;
636  thormang3_link_data_[36]->child_ = 46;
637  thormang3_link_data_[36]->mass_ = 1.689;
638  thormang3_link_data_[36]->relative_position_ = robotis_framework::getTransitionXYZ( -0.057 , 0.000 , -0.087 );
639  thormang3_link_data_[36]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
640  thormang3_link_data_[36]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.000 , 0.009 , -0.013 );
641  thormang3_link_data_[36]->joint_limit_max_ = 100.0;
642  thormang3_link_data_[36]->joint_limit_min_ = -100.0;
643  thormang3_link_data_[36]->inertia_ = robotis_framework::getInertiaXYZ( 0.00219 , 0.00000 , 0.00000 , 0.00433 , 0.00011 , 0.00609 );
644 
645  // left leg end
646  thormang3_link_data_[46]->name_ = "l_leg_end";
647  thormang3_link_data_[46]->parent_ = 36;
648  thormang3_link_data_[46]->sibling_ = -1;
649  thormang3_link_data_[46]->child_ = -1;
650  thormang3_link_data_[46]->mass_ = 0.0;
651  thormang3_link_data_[46]->relative_position_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , -0.0275 );
652  thormang3_link_data_[46]->joint_axis_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
653  thormang3_link_data_[46]->center_of_mass_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 );
654  thormang3_link_data_[46]->joint_limit_max_ = 100.0;
655  thormang3_link_data_[46]->joint_limit_min_ = -100.0;
656  thormang3_link_data_[46]->inertia_ = robotis_framework::getInertiaXYZ( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 );
657  }
658 
659  thigh_length_m_ = std::fabs(thormang3_link_data_[ID_R_LEG_START+2*3]->relative_position_.coeff(2,0));
660  calf_length_m_ = std::fabs(thormang3_link_data_[ID_R_LEG_START+2*4]->relative_position_.coeff(2,0));
661  ankle_length_m_ = std::fabs(thormang3_link_data_[ID_R_LEG_FT]->relative_position_.coeff(2,0)
662  + thormang3_link_data_[ID_R_LEG_END]->relative_position_.coeff(2,0));
663  leg_side_offset_m_ = 2.0*(std::fabs(thormang3_link_data_[ID_R_LEG_START]->relative_position_.coeff(1, 0)));
664 
665  for(int joint_idx = 0; joint_idx < ALL_JOINT_ID; joint_idx++)
666  {
667  thormang3_link_data_[joint_idx]->joint_center_of_mass_ = thormang3_link_data_[joint_idx]->center_of_mass_;
668  }
669 }
670 
671 std::vector<int> KinematicsDynamics::findRoute(int to)
672 {
673  int id = thormang3_link_data_[to]->parent_;
674 
675  std::vector<int> idx;
676 
677  if(id == 0)
678  {
679  idx.push_back(0);
680  idx.push_back(to);
681  }
682  else
683  {
684  idx = findRoute(id);
685  idx.push_back(to);
686  }
687 
688  return idx;
689 }
690 
691 std::vector<int> KinematicsDynamics::findRoute(int from, int to)
692 {
693  int id = thormang3_link_data_[to]->parent_;
694 
695  std::vector<int> idx;
696 
697  if(id == from)
698  {
699  idx.push_back(from);
700  idx.push_back(to);
701  }
702  else if (id != 0)
703  {
704  idx = findRoute(from, id);
705  idx.push_back(to);
706  }
707 
708  return idx;
709 }
710 
711 double KinematicsDynamics::calcTotalMass(int joint_id)
712 {
713  double mass;
714 
715  if (joint_id == -1)
716  mass = 0.0;
717  else
718  mass = thormang3_link_data_[joint_id]->mass_ + calcTotalMass(thormang3_link_data_[ joint_id ]->sibling_) + calcTotalMass(thormang3_link_data_[joint_id]->child_);
719 
720  return mass;
721 }
722 
723 Eigen::MatrixXd KinematicsDynamics::calcMassCenter(int joint_id)
724 {
725  Eigen::MatrixXd mc(3,1);
726 
727  if (joint_id == -1)
728  mc = Eigen::MatrixXd::Zero(3,1);
729  else
730  {
731  mc = thormang3_link_data_[ joint_id ]->mass_ * ( thormang3_link_data_[ joint_id ]->orientation_ * thormang3_link_data_[ joint_id ]->center_of_mass_ + thormang3_link_data_[ joint_id ]->position_ );
732  mc = mc + calcMassCenter( thormang3_link_data_[ joint_id ]->sibling_ ) + calcMassCenter( thormang3_link_data_[ joint_id ]->child_ );
733  }
734 
735  return mc;
736 }
737 
738 void KinematicsDynamics::calcJointsCenterOfMass(int joint_id)
739 {
740  if(joint_id != -1)
741  {
742  LinkData *temp_data = thormang3_link_data_[ joint_id ];
743  temp_data->joint_center_of_mass_
744  = ( temp_data->orientation_ * temp_data->center_of_mass_ + temp_data->position_ );
745 
746  calcJointsCenterOfMass(temp_data->sibling_);
747  calcJointsCenterOfMass(temp_data->child_);
748  }
749  else
750  return;
751 }
752 
753 Eigen::MatrixXd KinematicsDynamics::calcCenterOfMass(Eigen::MatrixXd mc)
754 {
755  double mass ;
756  Eigen::MatrixXd COM(3,1);
757 
758  mass = calcTotalMass(0);
759  COM = mc/mass;
760 
761  return COM;
762 }
763 
764 void KinematicsDynamics::calcForwardKinematics(int joint_id)
765 {
766  if (joint_id == -1)
767  return;
768 
769  if (joint_id == 0)
770  {
771  thormang3_link_data_[0]->position_ = Eigen::MatrixXd::Zero(3,1);
772  thormang3_link_data_[0]->orientation_ =
773  robotis_framework::calcRodrigues( robotis_framework::calcHatto( thormang3_link_data_[0]->joint_axis_ ), thormang3_link_data_[ 0 ]->joint_angle_ );
774  }
775 
776  if ( joint_id != 0 )
777  {
778  int parent = thormang3_link_data_[joint_id]->parent_;
779 
780  thormang3_link_data_[joint_id]->position_ =
781  thormang3_link_data_[parent]->orientation_ * thormang3_link_data_[joint_id]->relative_position_ + thormang3_link_data_[parent]->position_;
782  thormang3_link_data_[ joint_id ]->orientation_ =
783  thormang3_link_data_[ parent ]->orientation_ *
784  robotis_framework::calcRodrigues(robotis_framework::calcHatto(thormang3_link_data_[joint_id]->joint_axis_), thormang3_link_data_[joint_id]->joint_angle_);
785 
786  // thormang3_link_data_[joint_id]->transformation_.block<3,1>(0,3) = thormang3_link_data_[joint_id]->position_;
787  // thormang3_link_data_[joint_id]->transformation_.block<3,3>(0,0) = thormang3_link_data_[joint_id]->orientation_;
788  }
789 
790  calcForwardKinematics(thormang3_link_data_[joint_id]->sibling_);
791  calcForwardKinematics(thormang3_link_data_[joint_id]->child_);
792 }
793 
794 Eigen::MatrixXd KinematicsDynamics::calcJacobian(std::vector<int> idx)
795 {
796  int idx_size = idx.size();
797  int end = idx_size-1;
798 
799  Eigen::MatrixXd tar_position = thormang3_link_data_[idx[end]]->position_;
800  Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6,idx_size);
801 
802  for (int id=0; id<idx_size; id++)
803  {
804  int curr_id = idx[id];
805 
806  Eigen::MatrixXd tar_orientation = thormang3_link_data_[curr_id]->orientation_ * thormang3_link_data_[curr_id]->joint_axis_;
807 
808  jacobian.block(0,id,3,1) = robotis_framework::calcCross(tar_orientation,tar_position-thormang3_link_data_[curr_id]->position_);
809  jacobian.block(3,id,3,1) = tar_orientation;
810  }
811 
812  return jacobian;
813 }
814 
815 Eigen::MatrixXd KinematicsDynamics::calcJacobianCOM(std::vector<int> idx)
816 {
817  int idx_size = idx.size();
818  int end = idx_size-1;
819 
820  Eigen::MatrixXd tar_position = thormang3_link_data_[idx[end]]->position_;
821  Eigen::MatrixXd jacobian_com = Eigen::MatrixXd::Zero(6,idx_size);
822 
823  for (int id=0; id<idx_size; id++)
824  {
825  int curr_id = idx[id];
826  double mass = calcTotalMass(curr_id);
827 
828  Eigen::MatrixXd og = calcMassCenter(curr_id)/mass-thormang3_link_data_[curr_id]->position_;
829  Eigen::MatrixXd tar_orientation = thormang3_link_data_[curr_id]->orientation_*thormang3_link_data_[curr_id]->joint_axis_;
830 
831  jacobian_com.block(0,id,3,1) = robotis_framework::calcCross(tar_orientation,og);
832  jacobian_com.block(3,id,3,1) = tar_orientation;
833  }
834 
835  return jacobian_com;
836 }
837 
838 Eigen::MatrixXd KinematicsDynamics::calcVWerr(Eigen::MatrixXd tar_position, Eigen::MatrixXd curr_position, Eigen::MatrixXd tar_orientation, Eigen::MatrixXd curr_orientation)
839 {
840  Eigen::MatrixXd pos_err = tar_position - curr_position;
841  Eigen::MatrixXd ori_err = curr_orientation.transpose() * tar_orientation;
842  Eigen::MatrixXd ori_err_dash = curr_orientation * robotis_framework::convertRotToOmega(ori_err);
843 
844  Eigen::MatrixXd err = Eigen::MatrixXd::Zero(6,1);
845  err.block<3,1>(0,0) = pos_err;
846  err.block<3,1>(3,0) = ori_err_dash;
847 
848  return err;
849 }
850 
851 bool KinematicsDynamics::calcInverseKinematics(int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err)
852 {
853  bool ik_success = false;
854  bool limit_success = false;
855 
856  // calcForwardKinematics(0);
857 
858  std::vector<int> idx = findRoute(to);
859 
860  for (int iter=0; iter<max_iter; iter++)
861  {
862  Eigen::MatrixXd jacobian = calcJacobian(idx);
863 
864  Eigen::MatrixXd curr_position = thormang3_link_data_[to]->position_;
865  Eigen::MatrixXd curr_orientation = thormang3_link_data_[to]->orientation_;
866 
867  Eigen::MatrixXd err = calcVWerr(tar_position, curr_position, tar_orientation, curr_orientation);
868 
869  if (err.norm()<ik_err)
870  {
871  ik_success = true;
872  break;
873  }
874  else
875  ik_success = false;
876 
877  Eigen::MatrixXd jacobian_trans = jacobian * jacobian.transpose();
878  Eigen::MatrixXd jacobian_inverse = jacobian.transpose() * jacobian_trans.inverse();
879 
880  Eigen::MatrixXd delta_angle = jacobian_inverse * err ;
881 
882  for (int id=0; id<idx.size(); id++)
883  {
884  int joint_num = idx[id];
885  thormang3_link_data_[joint_num]->joint_angle_ += delta_angle.coeff(id);
886  }
887 
888  calcForwardKinematics(0);
889  }
890 
891  for ( int id = 0; id < idx.size(); id++ )
892  {
893  int joint_num = idx[ id ];
894 
895  if ( thormang3_link_data_[ joint_num ]->joint_angle_ >= thormang3_link_data_[ joint_num ]->joint_limit_max_ )
896  {
897  limit_success = false;
898  break;
899  }
900  else if ( thormang3_link_data_[ joint_num ]->joint_angle_ <= thormang3_link_data_[ joint_num ]->joint_limit_min_ )
901  {
902  limit_success = false;
903  break;
904  }
905  else
906  limit_success = true;
907  }
908 
909  if (ik_success == true && limit_success == true)
910  return true;
911  else
912  return false;
913 }
914 
915 bool KinematicsDynamics::calcInverseKinematics(int from, int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err)
916 {
917  bool ik_success = false;
918  bool limit_success = false;
919 
920  // calcForwardKinematics(0);
921 
922  std::vector<int> idx = findRoute(from, to);
923 
924  for (int iter=0; iter<max_iter; iter++)
925  {
926  Eigen::MatrixXd jacobian = calcJacobian(idx);
927 
928  Eigen::MatrixXd curr_position = thormang3_link_data_[to]->position_;
929  Eigen::MatrixXd curr_orientation = thormang3_link_data_[to]->orientation_;
930 
931  Eigen::MatrixXd err = calcVWerr(tar_position, curr_position, tar_orientation, curr_orientation);
932 
933  if (err.norm()<ik_err)
934  {
935  ik_success = true;
936  break;
937  }
938  else
939  ik_success = false;
940 
941  Eigen::MatrixXd jacobian_trans = jacobian * jacobian.transpose();
942  Eigen::MatrixXd jacobian_inv = jacobian.transpose() * jacobian_trans.inverse();
943 
944  Eigen::MatrixXd delta_angle = jacobian_inv * err ;
945 
946  for (int id=0; id<idx.size(); id++)
947  {
948  int joint_num = idx[id];
949  thormang3_link_data_[joint_num]->joint_angle_ +=delta_angle.coeff(id);
950  }
951 
952  calcForwardKinematics(0);
953  }
954 
955  for ( int id = 0; id < idx.size(); id++ )
956  {
957  int joint_num = idx[ id ];
958 
959  if ( thormang3_link_data_[ joint_num ]->joint_angle_ >= thormang3_link_data_[ joint_num ]->joint_limit_max_ )
960  {
961  limit_success = false;
962  break;
963  }
964  else if ( thormang3_link_data_[ joint_num ]->joint_angle_ <= thormang3_link_data_[ joint_num ]->joint_limit_min_ )
965  {
966  limit_success = false;
967  break;
968  }
969  else
970  limit_success = true;
971  }
972 
973  if (ik_success == true && limit_success == true)
974  return true;
975  else
976  return false;
977 }
978 
979 bool KinematicsDynamics::calcInverseKinematics(int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err , Eigen::MatrixXd weight)
980 {
981  bool ik_success = false;
982  bool limit_success = false;
983 
984  // calcForwardKinematics(0);
985 
986  std::vector<int> idx = findRoute(to);
987 
988  /* weight */
989  Eigen::MatrixXd weight_matrix = Eigen::MatrixXd::Identity(idx.size(), idx.size());
990 
991  for ( int ix = 0; ix < idx.size(); ix++ )
992  weight_matrix.coeffRef(ix,ix) = weight.coeff(idx[ix],0);
993 
994  /* damping */
995  Eigen::MatrixXd eval = Eigen::MatrixXd::Zero(6,6);
996 
997  double p_damping = 1e-5;
998  double R_damping = 1e-5;
999 
1000  for (int ix=0; ix<3; ix++)
1001  {
1002  eval.coeffRef(ix,ix) = p_damping;
1003  eval.coeffRef(ix+3,ix+3) = R_damping;
1004  }
1005 
1006  /* ik */
1007  for (int iter=0; iter<max_iter; iter++)
1008  {
1009  Eigen::MatrixXd jacobian = calcJacobian(idx);
1010 
1011  Eigen::MatrixXd curr_position = thormang3_link_data_[to]->position_;
1012  Eigen::MatrixXd curr_orientation = thormang3_link_data_[to]->orientation_;
1013 
1014  Eigen::MatrixXd err = calcVWerr(tar_position, curr_position, tar_orientation, curr_orientation);
1015 
1016  if (err.norm()<ik_err)
1017  {
1018  ik_success = true;
1019  break;
1020  }
1021  else
1022  ik_success = false;
1023 
1024  Eigen::MatrixXd jacobian_trans = (jacobian * weight_matrix * jacobian.transpose() + eval);
1025  Eigen::MatrixXd jacobian_inv = weight_matrix * jacobian.transpose() * jacobian_trans.inverse();
1026 
1027  Eigen::MatrixXd delta_angle = jacobian_inv * err ;
1028 
1029  for (int id=0; id<idx.size(); id++)
1030  {
1031  int joint_id = idx[id];
1032  thormang3_link_data_[joint_id]->joint_angle_ += delta_angle.coeff(id);
1033  }
1034 
1035  calcForwardKinematics(0);
1036  }
1037 
1038  /* check joint limit */
1039  for ( int id = 0; id < idx.size(); id++ )
1040  {
1041  int joint_num = idx[ id ];
1042 
1043  if ( thormang3_link_data_[ joint_num ]->joint_angle_ >= thormang3_link_data_[ joint_num ]->joint_limit_max_ )
1044  {
1045  limit_success = false;
1046  break;
1047  }
1048  else if ( thormang3_link_data_[ joint_num ]->joint_angle_ <= thormang3_link_data_[ joint_num ]->joint_limit_min_ )
1049  {
1050  limit_success = false;
1051  break;
1052  }
1053  else
1054  limit_success = true;
1055  }
1056 
1057  if (ik_success == true && limit_success == true)
1058  return true;
1059  else
1060  return false;
1061 }
1062 
1063 bool KinematicsDynamics::calcInverseKinematics(int from, int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err, Eigen::MatrixXd weight)
1064 {
1065  bool ik_success = false;
1066  bool limit_success = false;
1067 
1068  // calcForwardKinematics(0);
1069 
1070  std::vector<int> idx = findRoute(from, to);
1071 
1072  /* weight */
1073  Eigen::MatrixXd weight_matrix = Eigen::MatrixXd::Identity(idx.size(),idx.size());
1074 
1075  for (int ix = 0; ix < idx.size(); ix++)
1076  weight_matrix.coeffRef(ix, ix) = weight.coeff(idx[ix], 0);
1077 
1078  /* damping */
1079  Eigen::MatrixXd eval = Eigen::MatrixXd::Zero(6, 6);
1080 
1081  double p_damping = 1e-5;
1082  double R_damping = 1e-5;
1083 
1084  for (int ix = 0; ix < 3; ix++)
1085  {
1086  eval.coeffRef(ix, ix) = p_damping;
1087  eval.coeffRef(ix + 3, ix + 3) = R_damping;
1088  }
1089 
1090  /* ik */
1091  for (int iter = 0; iter < max_iter; iter++)
1092  {
1093  Eigen::MatrixXd jacobian = calcJacobian(idx);
1094  Eigen::MatrixXd curr_position = thormang3_link_data_[to]->position_;
1095  Eigen::MatrixXd curr_orientation = thormang3_link_data_[to]->orientation_;
1096  Eigen::MatrixXd err = calcVWerr(tar_position, curr_position, tar_orientation, curr_orientation);
1097 
1098  if (err.norm() < ik_err)
1099  {
1100  ik_success = true;
1101  break;
1102  }
1103  else
1104  ik_success = false;
1105 
1106  Eigen::MatrixXd jacobian_trans = (jacobian * weight_matrix * jacobian.transpose() + eval);
1107  Eigen::MatrixXd jacobian_inv = weight_matrix * jacobian.transpose() * jacobian_trans.inverse();
1108  Eigen::MatrixXd delta_angle = jacobian_inv * err;
1109 
1110  for (int id = 0; id < idx.size(); id++)
1111  {
1112  int joint_id = idx[id];
1113  thormang3_link_data_[joint_id]->joint_angle_ += delta_angle.coeff(id);
1114  }
1115  calcForwardKinematics(0);
1116  }
1117 
1118  /* check joint limit */
1119  for (int id = 0; id < idx.size(); id++)
1120  {
1121  int _joint_num = idx[id];
1122  if (thormang3_link_data_[_joint_num]->joint_angle_ >= thormang3_link_data_[_joint_num]->joint_limit_max_)
1123  {
1124  limit_success = false;
1125  break;
1126  }
1127  else if (thormang3_link_data_[_joint_num]->joint_angle_ <= thormang3_link_data_[_joint_num]->joint_limit_min_)
1128  {
1129  limit_success = false;
1130  break;
1131  }
1132  else
1133  limit_success = true;
1134  }
1135 
1136  if (ik_success == true && limit_success == true)
1137  return true;
1138  else
1139  return false;
1140 }
1141 
1142 bool KinematicsDynamics::calcInverseKinematicsForLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
1143 {
1144  //Eigen::MatrixXd target_transform;
1145  Eigen::Matrix4d trans_ad, trans_da, trans_cd, trans_dc, trans_ac;
1146  Eigen::Vector3d vec;
1147 
1148  bool invertible;
1149  double rac, arc_cos, arc_tan, k, l, m, n, s, c, theta;
1150  double thigh_length = thigh_length_m_;
1151  double calf_length = calf_length_m_;
1152  double ankle_length = ankle_length_m_;
1153 
1154  trans_ad = robotis_framework::getTransformationXYZRPY(x, y, z, roll, pitch, yaw);
1155 
1156  vec.coeffRef(0) = trans_ad.coeff(0,3) + trans_ad.coeff(0,2) * ankle_length;
1157  vec.coeffRef(1) = trans_ad.coeff(1,3) + trans_ad.coeff(1,2) * ankle_length;
1158  vec.coeffRef(2) = trans_ad.coeff(2,3) + trans_ad.coeff(2,2) * ankle_length;
1159 
1160  // Get Knee
1161  rac = vec.norm();
1162  arc_cos = acos((rac * rac - thigh_length * thigh_length - calf_length * calf_length) / (2.0 * thigh_length * calf_length));
1163  if(std::isnan(arc_cos) == 1)
1164  return false;
1165  *(out + 3) = arc_cos;
1166 
1167  // Get Ankle Roll
1168  trans_ad.computeInverseWithCheck(trans_da, invertible);
1169  if(invertible == false)
1170  return false;
1171 
1172  k = sqrt(trans_da.coeff(1,3) * trans_da.coeff(1,3) + trans_da.coeff(2,3) * trans_da.coeff(2,3));
1173  l = sqrt(trans_da.coeff(1,3) * trans_da.coeff(1,3) + (trans_da.coeff(2,3) - ankle_length)*(trans_da.coeff(2,3) - ankle_length));
1174  m = (k * k - l * l - ankle_length * ankle_length) / (2.0 * l * ankle_length);
1175 
1176  if(m > 1.0)
1177  m = 1.0;
1178  else if(m < -1.0)
1179  m = -1.0;
1180  arc_cos = acos(m);
1181 
1182  if(std::isnan(arc_cos) == 1)
1183  return false;
1184 
1185  if(trans_da.coeff(1,3) < 0.0)
1186  *(out + 5) = -arc_cos;
1187  else
1188  *(out + 5) = arc_cos;
1189 
1190  // Get Hip Yaw
1191  trans_cd = robotis_framework::getTransformationXYZRPY(0, 0, -ankle_length, *(out + 5), 0, 0);
1192  trans_cd.computeInverseWithCheck(trans_dc, invertible);
1193  if(invertible == false)
1194  return false;
1195 
1196  trans_ac = trans_ad * trans_dc;
1197  arc_tan = atan2(-trans_ac.coeff(0,1) , trans_ac.coeff(1,1));
1198  if(std::isinf(arc_tan) != 0)
1199  return false;
1200  *(out) = arc_tan;
1201 
1202  // Get Hip Roll
1203  arc_tan = atan2(trans_ac.coeff(2,1), -trans_ac.coeff(0,1) * sin(*(out)) + trans_ac.coeff(1,1) * cos(*(out)));
1204  if(std::isinf(arc_tan) != 0)
1205  return false;
1206  *(out + 1) = arc_tan;
1207 
1208  // Get Hip Pitch and Ankle Pitch
1209  arc_tan = atan2(trans_ac.coeff(0,2) * cos(*(out)) + trans_ac.coeff(1,2) * sin(*(out)), trans_ac.coeff(0,0) * cos(*(out)) + trans_ac.coeff(1,0) * sin(*(out)));
1210  if(std::isinf(arc_tan) == 1)
1211  return false;
1212  theta = arc_tan;
1213  k = sin(*(out + 3)) * calf_length;
1214  l = -thigh_length - cos(*(out + 3)) * calf_length;
1215  m = cos(*(out)) * vec.coeff(0) + sin(*(out)) * vec.coeff(1);
1216  n = cos(*(out + 1)) * vec.coeff(2) + sin(*(out)) * sin(*(out + 1)) * vec.coeff(0) - cos(*(out)) * sin(*(out + 1)) * vec.coeff(1);
1217  s = (k * n + l * m) / (k * k + l * l);
1218  c = (n - k * s) / l;
1219  arc_tan = atan2(s, c);
1220  if(std::isinf(arc_tan) == 1)
1221  return false;
1222  *(out + 2) = arc_tan;
1223  *(out + 4) = theta - *(out + 3) - *(out + 2);
1224 
1225  return true;
1226 }
1227 
1228 bool KinematicsDynamics::calcInverseKinematicsForRightLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
1229 {
1230  if(calcInverseKinematicsForLeg(out, x, y, z, roll, pitch, yaw) == true) {
1231 
1232  *(out + 0) = out[0] * (thormang3_link_data_[ID_R_LEG_START + 2*0]->joint_axis_.coeff(2,0));
1233  *(out + 1) = out[1] * (thormang3_link_data_[ID_R_LEG_START + 2*1]->joint_axis_.coeff(0,0));
1234  *(out + 2) = out[2] * (thormang3_link_data_[ID_R_LEG_START + 2*2]->joint_axis_.coeff(1,0));
1235  *(out + 3) = out[3] * (thormang3_link_data_[ID_R_LEG_START + 2*3]->joint_axis_.coeff(1,0));
1236  *(out + 4) = out[4] * (thormang3_link_data_[ID_R_LEG_START + 2*4]->joint_axis_.coeff(1,0));
1237  *(out + 5) = out[5] * (thormang3_link_data_[ID_R_LEG_START + 2*5]->joint_axis_.coeff(0,0));
1238  return true;
1239  }
1240  else
1241  return false;
1242 }
1243 
1244 bool KinematicsDynamics::calcInverseKinematicsForLeftLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
1245 {
1246  if(calcInverseKinematicsForLeg(out, x, y, z, roll, pitch, yaw) == true) {
1247 
1248  out[0] = out[0] * (thormang3_link_data_[ID_L_LEG_START + 2*0]->joint_axis_.coeff(2,0));
1249  out[1] = out[1] * (thormang3_link_data_[ID_L_LEG_START + 2*1]->joint_axis_.coeff(0,0));
1250  out[2] = out[2] * (thormang3_link_data_[ID_L_LEG_START + 2*2]->joint_axis_.coeff(1,0));
1251  out[3] = out[3] * (thormang3_link_data_[ID_L_LEG_START + 2*3]->joint_axis_.coeff(1,0));
1252  out[4] = out[4] * (thormang3_link_data_[ID_L_LEG_START + 2*4]->joint_axis_.coeff(1,0));
1253  out[5] = out[5] * (thormang3_link_data_[ID_L_LEG_START + 2*5]->joint_axis_.coeff(0,0));
1254  return true;
1255  }
1256  else
1257  return false;
1258 }
1259 
1260 }
thormang3::LinkData::joint_center_of_mass_
Eigen::MatrixXd joint_center_of_mass_
Definition: link_data.h:55
thormang3::KinematicsDynamics::findRoute
std::vector< int > findRoute(int to)
Definition: kinematics_dynamics.cpp:671
thormang3::LinkData::orientation_
Eigen::MatrixXd orientation_
Definition: link_data.h:65
ID_R_LEG_START
#define ID_R_LEG_START
Definition: kinematics_dynamics_define.h:46
ID_L_LEG_START
#define ID_L_LEG_START
Definition: kinematics_dynamics_define.h:47
kinematics_dynamics.h
thormang3::KinematicsDynamics::calcJacobian
Eigen::MatrixXd calcJacobian(std::vector< int > idx)
Definition: kinematics_dynamics.cpp:794
s
XmlRpcServer s
robotis_framework::calcHatto
Eigen::Matrix3d calcHatto(const Eigen::Vector3d &matrix3d)
robotis_framework::calcRodrigues
Eigen::Matrix3d calcRodrigues(const Eigen::Matrix3d &hat_matrix, double angle)
robotis_framework::convertRotToOmega
Eigen::Vector3d convertRotToOmega(const Eigen::Matrix3d &rotation)
thormang3::KinematicsDynamics::calcMassCenter
Eigen::MatrixXd calcMassCenter(int joint_id)
Definition: kinematics_dynamics.cpp:723
thormang3::KinematicsDynamics::calcJacobianCOM
Eigen::MatrixXd calcJacobianCOM(std::vector< int > idx)
Definition: kinematics_dynamics.cpp:815
robotis_framework::getTransformationXYZRPY
Eigen::Matrix4d getTransformationXYZRPY(double position_x, double position_y, double position_z, double roll, double pitch, double yaw)
robotis_framework::getTransitionXYZ
Eigen::Vector3d getTransitionXYZ(double position_x, double position_y, double position_z)
robotis_framework::calcCross
Eigen::Vector3d calcCross(const Eigen::Vector3d &vector3d_a, const Eigen::Vector3d &vector3d_b)
thormang3::KinematicsDynamics::calcInverseKinematicsForLeg
bool calcInverseKinematicsForLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
Definition: kinematics_dynamics.cpp:1142
thormang3::LinkData::joint_limit_max_
double joint_limit_max_
Definition: link_data.h:57
thormang3::LinkData::position_
Eigen::MatrixXd position_
Definition: link_data.h:64
thormang3::LinkData::joint_limit_min_
double joint_limit_min_
Definition: link_data.h:58
thormang3::KinematicsDynamics::thormang3_link_data_
LinkData * thormang3_link_data_[ALL_JOINT_ID+1]
Definition: kinematics_dynamics.h:79
thormang3::LinkData::joint_axis_
Eigen::MatrixXd joint_axis_
Definition: link_data.h:52
ID_R_LEG_FT
#define ID_R_LEG_FT
Definition: kinematics_dynamics_define.h:51
thormang3::LinkData::center_of_mass_
Eigen::MatrixXd center_of_mass_
Definition: link_data.h:53
ID_R_LEG_END
#define ID_R_LEG_END
Definition: kinematics_dynamics_define.h:48
thormang3::KinematicsDynamics::calcInverseKinematicsForRightLeg
bool calcInverseKinematicsForRightLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
Definition: kinematics_dynamics.cpp:1228
thormang3::KinematicsDynamics::calcVWerr
Eigen::MatrixXd calcVWerr(Eigen::MatrixXd tar_position, Eigen::MatrixXd curr_position, Eigen::MatrixXd tar_orientation, Eigen::MatrixXd curr_orientation)
Definition: kinematics_dynamics.cpp:838
thormang3::KinematicsDynamics::calcCenterOfMass
Eigen::MatrixXd calcCenterOfMass(Eigen::MatrixXd mc)
Definition: kinematics_dynamics.cpp:753
ALL_JOINT_ID
#define ALL_JOINT_ID
Definition: kinematics_dynamics_define.h:31
robotis_framework::getInertiaXYZ
Eigen::Matrix3d getInertiaXYZ(double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
thormang3::KinematicsDynamics::calcInverseKinematicsForLeftLeg
bool calcInverseKinematicsForLeftLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
Definition: kinematics_dynamics.cpp:1244
thormang3::LinkData::relative_position_
Eigen::MatrixXd relative_position_
Definition: link_data.h:51
thormang3::LinkData::inertia_
Eigen::MatrixXd inertia_
Definition: link_data.h:54
thormang3::LinkData::name_
std::string name_
Definition: link_data.h:43
thormang3::KinematicsDynamics::calcInverseKinematics
bool calcInverseKinematics(int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err)
Definition: kinematics_dynamics.cpp:851

thormang3_kinematics_dynamics
Author(s): SCH
autogenerated on Mon Jun 10 2019 15:37:49