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

op3_kinematics_dynamics
Author(s): SCH , Kayman , Jay Song
autogenerated on Mon Jun 10 2019 14:41:13