humoto
model_description.h
Go to the documentation of this file.
1 /**
2  @file
3  @author Alexander Sherikov
4  @copyright 2014-2017 INRIA. Licensed under the Apache License, Version 2.0.
5  (see @ref LICENSE or http://www.apache.org/licenses/LICENSE-2.0)
6 
7  @brief
8 
9  @todo This code is overcomplicated, find a better approach.
10 */
11 
12 #pragma once
13 
14 namespace humoto
15 {
16  namespace pepper_ik
17  {
18  /**
19  * @brief Model features are used to define different versions of the model.
20  */
21  class HUMOTO_LOCAL ModelFeatures
22  {
23  public:
24  /**
25  * @brief Binary map
26  *
27  * 000 000
28  * root wheels
29  */
30  enum Features
31  {
32  UNDEFINED = 0,
33 
34  FIXED_WHEELS = 1,
35 
36  ROOT_DEFAULT = 8,
37  ROOT_TORSO = 8,
38  ROOT_TIBIA = 16,
39  ROOT_PLANAR = 32
40  };
41  };
42 
43 
44 
45  /**
46  * @brief Base model description class
47  *
48  * @tparam t_features Model features.
49  */
50  template <int t_features>
51  class HUMOTO_LOCAL ModelDescriptionBase;
52 
53 
54  /**
55  * @brief Specific model description classes
56  *
57  * @tparam t_features Model features.
58  */
59  template <int t_features> class HUMOTO_LOCAL ModelDescription;
60 
61 
62 
63  /**
64  * @brief Base description class of models with fixed wheels
65  */
66  template <>
67  class HUMOTO_LOCAL ModelDescriptionBase<ModelFeatures::FIXED_WHEELS>
68  {
69  public:
70  enum WheelIds
71  {
72  WHEEL_FRONT_LEFT = 0,
73  WHEEL_FRONT_RIGHT = 1,
74  WHEEL_BACK = 2
75  };
76 
77 
78  /**
79  * @brief Get string ids of the bodies comprising the base.
80  *
81  * @param[out] ids ids
82  */
83  static void getBaseLinksStringIds(std::vector<std::string> &ids)
84  {
85  ids.clear();
86  /*
87  ids.push_back("WheelB_link");
88  ids.push_back("WheelFL_link");
89  ids.push_back("WheelFR_link");
90  */
91  ids.push_back("Tibia");
92  }
93 
94 
95  /**
96  * @brief Get string ids of the links comprising the upper body.
97  *
98  * @param[out] ids ids
99  */
100  static void getBodyLinksStringIds(std::vector<std::string> &ids)
101  {
102  ids.clear();
103 
104  ids.push_back("Head");
105  ids.push_back("Hip");
106  ids.push_back("LBicep");
107  ids.push_back("LElbow");
108  /*
109  ids.push_back("LFinger11_link");
110  ids.push_back("LFinger12_link");
111  ids.push_back("LFinger13_link");
112  ids.push_back("LFinger21_link");
113  ids.push_back("LFinger22_link");
114  ids.push_back("LFinger23_link");
115  ids.push_back("LFinger31_link");
116  ids.push_back("LFinger32_link");
117  ids.push_back("LFinger33_link");
118  ids.push_back("LFinger41_link");
119  ids.push_back("LFinger42_link");
120  ids.push_back("LFinger43_link");
121  */
122  ids.push_back("LForeArm");
123  ids.push_back("LShoulder");
124  /*
125  ids.push_back("LThumb1_link");
126  ids.push_back("LThumb2_link");
127  */
128  ids.push_back("Neck");
129  ids.push_back("Pelvis");
130  ids.push_back("RBicep");
131  ids.push_back("RElbow");
132  /*
133  ids.push_back("RFinger11_link");
134  ids.push_back("RFinger12_link");
135  ids.push_back("RFinger13_link");
136  ids.push_back("RFinger21_link");
137  ids.push_back("RFinger22_link");
138  ids.push_back("RFinger23_link");
139  ids.push_back("RFinger31_link");
140  ids.push_back("RFinger32_link");
141  ids.push_back("RFinger33_link");
142  ids.push_back("RFinger41_link");
143  ids.push_back("RFinger42_link");
144  ids.push_back("RFinger43_link");
145  */
146  ids.push_back("RForeArm");
147  ids.push_back("RShoulder");
148  /*
149  ids.push_back("RThumb1_link");
150  ids.push_back("RThumb2_link");
151  ids.push_back("l_gripper");
152  */
153  ids.push_back("l_wrist");
154  //ids.push_back("r_gripper");
155  ids.push_back("r_wrist");
156  ids.push_back("torso"); // torso
157  }
158 
159 
160  /**
161  * @brief Get id of the body which is used to control orientation of the base.
162  *
163  * @param[out] id id
164  */
165  static void getBaseOrientationLinkStringId(std::string &id)
166  {
167  id = "base_footprint";
168  }
169 
170 
171  /**
172  * @brief Get id of the body which is used to control position of the base.
173  *
174  * @param[out] id id
175  */
176  static void getBasePositionLinkStringId(std::string &id)
177  {
178  id = "base_footprint";
179  }
180 
181 
182  /**
183  * @brief Returns matrix which maps velocities of the wheels to
184  * translational and rotational motion of the base.
185  *
186  * @return 3x3 matrix
187  */
188  static etools::Matrix3 getRootMotionToWheelsTransform()
189  {
190  // ------------------------------------------------
191  /// @todo This code is based on the URDF file -- may be
192  /// there is a way to avoid hardcoding it.
193  etools::Vector3 front_left_wheel_rpy(0, 0, 1.07519348146676);
194  etools::Vector3 front_right_wheel_rpy(0, 0, -1.07519348146676);
195  etools::Vector3 back_wheel_rpy(0, 0, -humoto::g_pi);
196 
197 
198  etools::Vector3 front_left_wheel_position(0.09, 0.155, 0);
199  etools::Vector3 front_right_wheel_position(0.09, -0.155, 0);
200  etools::Vector3 back_wheel_position(-0.17, 0, 0);
201 
202  etools::Vector3 z_axis(0, 0, 1);
203 
204  double wheel_radius = 0.07;
205 
206 
207  etools::Vector3 front_left_wheel_tangent = -convertEulerAnglesToMatrix( front_left_wheel_rpy,
208  rigidbody::EulerAngles::RPY).col(1);
209  etools::Vector3 front_right_wheel_tangent = -convertEulerAnglesToMatrix(front_right_wheel_rpy,
210  rigidbody::EulerAngles::RPY).col(1);
211  etools::Vector3 back_wheel_tangent = -convertEulerAnglesToMatrix(back_wheel_rpy,
212  rigidbody::EulerAngles::RPY).col(1);
213  // ------------------------------------------------
214 
215 
216  // ------------------------------------------------
217  etools::Matrix1x3 front_left_wheel_transform;
218  etools::Matrix1x3 front_right_wheel_transform;
219  etools::Matrix1x3 back_wheel_transform;
220 
221 
222  front_left_wheel_transform << front_left_wheel_tangent.segment(0, 2).transpose(),
223  -front_left_wheel_tangent.transpose()*(front_left_wheel_position.cross(z_axis));
224 
225  front_right_wheel_transform << front_right_wheel_tangent.segment(0, 2).transpose(),
226  -front_right_wheel_tangent.transpose()*(front_right_wheel_position.cross(z_axis));
227 
228  back_wheel_transform << back_wheel_tangent.segment(0, 2).transpose(),
229  -back_wheel_tangent.transpose()*(back_wheel_position.cross(z_axis));
230  // ------------------------------------------------
231 
232 
233  // ------------------------------------------------
234  etools::Matrix3 result;
235 
236  result << front_left_wheel_transform / wheel_radius,
237  front_right_wheel_transform / wheel_radius,
238  back_wheel_transform / wheel_radius;
239 
240  return (result);
241  }
242 
243 
244  /**
245  * @brief Use joint order to reorder columns of the given matrix
246  *
247  * @tparam t_features_out features of the model corresponding to output
248  * @tparam t_features_in features of the model corresponding to input
249  *
250  * @param[in,out] matrix
251  */
252  template <int t_features_out, int t_features_in>
253  static void reorderJointsColumns(Eigen::MatrixXd & matrix)
254  {
255  const int out_index_offset = ModelDescription<t_features_out>::ROOT_DOF_NUMBER;
256  const int in_index_offset = ModelDescription<t_features_in>::ROOT_DOF_NUMBER;
257 
258  Eigen::MatrixXd reordered_matrix;
259 
260  reordered_matrix.resize(matrix.rows(), ModelDescription<t_features_out>::DOF_NUMBER);
261  etools::unsetMatrix(reordered_matrix);
262 
263  reordered_matrix.col(out_index_offset + ModelDescription<t_features_out>::KneePitch ) = matrix.col(in_index_offset + ModelDescription<t_features_in>::KneePitch );
264  reordered_matrix.col(out_index_offset + ModelDescription<t_features_out>::HipPitch ) = matrix.col(in_index_offset + ModelDescription<t_features_in>::HipPitch );
265  reordered_matrix.col(out_index_offset + ModelDescription<t_features_out>::HipRoll ) = matrix.col(in_index_offset + ModelDescription<t_features_in>::HipRoll );
266  reordered_matrix.col(out_index_offset + ModelDescription<t_features_out>::HeadYaw ) = matrix.col(in_index_offset + ModelDescription<t_features_in>::HeadYaw );
267  reordered_matrix.col(out_index_offset + ModelDescription<t_features_out>::HeadPitch ) = matrix.col(in_index_offset + ModelDescription<t_features_in>::HeadPitch );
268  reordered_matrix.col(out_index_offset + ModelDescription<t_features_out>::LShoulderPitch) = matrix.col(in_index_offset + ModelDescription<t_features_in>::LShoulderPitch);
269  reordered_matrix.col(out_index_offset + ModelDescription<t_features_out>::LShoulderRoll ) = matrix.col(in_index_offset + ModelDescription<t_features_in>::LShoulderRoll );
270  reordered_matrix.col(out_index_offset + ModelDescription<t_features_out>::LElbowYaw ) = matrix.col(in_index_offset + ModelDescription<t_features_in>::LElbowYaw );
271  reordered_matrix.col(out_index_offset + ModelDescription<t_features_out>::LElbowRoll ) = matrix.col(in_index_offset + ModelDescription<t_features_in>::LElbowRoll );
272  reordered_matrix.col(out_index_offset + ModelDescription<t_features_out>::LWristYaw ) = matrix.col(in_index_offset + ModelDescription<t_features_in>::LWristYaw );
273  reordered_matrix.col(out_index_offset + ModelDescription<t_features_out>::RShoulderPitch) = matrix.col(in_index_offset + ModelDescription<t_features_in>::RShoulderPitch);
274  reordered_matrix.col(out_index_offset + ModelDescription<t_features_out>::RShoulderRoll ) = matrix.col(in_index_offset + ModelDescription<t_features_in>::RShoulderRoll );
275  reordered_matrix.col(out_index_offset + ModelDescription<t_features_out>::RElbowYaw ) = matrix.col(in_index_offset + ModelDescription<t_features_in>::RElbowYaw );
276  reordered_matrix.col(out_index_offset + ModelDescription<t_features_out>::RElbowRoll ) = matrix.col(in_index_offset + ModelDescription<t_features_in>::RElbowRoll );
277  reordered_matrix.col(out_index_offset + ModelDescription<t_features_out>::RWristYaw ) = matrix.col(in_index_offset + ModelDescription<t_features_in>::RWristYaw );
278 
279  matrix = reordered_matrix;
280  }
281 
282 
283  /**
284  * @brief Returns maximal velocites, which can be executed by the wheels
285  *
286  * @param[out] max_velocities
287  *
288  * @todo RBDL must load this from an URDF file!
289  */
290  static void getMaxWheelVelocities(etools::Vector3 & max_velocities)
291  {
292  max_velocities << 6.28319, 6.28319, 6.28319;
293  }
294 
295 
296 
297  protected:
298  /**
299  * @brief Returns joint position bounds.
300  *
301  * @tparam t_features these features identify model description
302  *
303  * @param[out] min_bounds
304  * @param[out] max_bounds
305  *
306  * @todo RBDL must load this from an URDF file!
307  */
308  template <int t_features>
309  static void getJointPositionBounds(EIGENTOOLS_CONSTANT_SIZE_VECTOR(ModelDescription<t_features>::JOINTS_DOF_NUMBER) & min_bounds,
310  EIGENTOOLS_CONSTANT_SIZE_VECTOR(ModelDescription<t_features>::JOINTS_DOF_NUMBER) & max_bounds)
311  {
312  min_bounds(ModelDescription<t_features>::KneePitch ) = -0.51487200000000000;
313  min_bounds(ModelDescription<t_features>::HipPitch ) = -1.03847000000000000;
314  min_bounds(ModelDescription<t_features>::HipRoll ) = -0.51487200000000000;
315  min_bounds(ModelDescription<t_features>::HeadYaw ) = -2.08566999999999991;
316  min_bounds(ModelDescription<t_features>::HeadPitch ) = -0.70685799999999999;
317  min_bounds(ModelDescription<t_features>::LShoulderPitch) = -2.08566999999999991;
318  min_bounds(ModelDescription<t_features>::LShoulderRoll ) = 0.00872665000000000;
319  min_bounds(ModelDescription<t_features>::LElbowYaw ) = -2.08566999999999991;
320  min_bounds(ModelDescription<t_features>::LElbowRoll ) = -1.56207000000000007;
321  min_bounds(ModelDescription<t_features>::LWristYaw ) = -1.82387000000000010;
322  min_bounds(ModelDescription<t_features>::RShoulderPitch) = -2.08566999999999991;
323  min_bounds(ModelDescription<t_features>::RShoulderRoll ) = -1.56207000000000007;
324  min_bounds(ModelDescription<t_features>::RElbowYaw ) = -2.08566999999999991;
325  min_bounds(ModelDescription<t_features>::RElbowRoll ) = 0.00872665000000000;
326  min_bounds(ModelDescription<t_features>::RWristYaw ) = -1.82387000000000010;
327 
328  max_bounds(ModelDescription<t_features>::KneePitch ) = 0.51487200000000000;
329  max_bounds(ModelDescription<t_features>::HipPitch ) = 1.03847000000000000;
330  max_bounds(ModelDescription<t_features>::HipRoll ) = 0.51487200000000000;
331  max_bounds(ModelDescription<t_features>::HeadYaw ) = 2.08566999999999991;
332  max_bounds(ModelDescription<t_features>::HeadPitch ) = 0.63704499999999997;
333  max_bounds(ModelDescription<t_features>::LShoulderPitch) = 2.08566999999999991;
334  max_bounds(ModelDescription<t_features>::LShoulderRoll ) = 1.56207000000000007;
335  max_bounds(ModelDescription<t_features>::LElbowYaw ) = 2.08566999999999991;
336  max_bounds(ModelDescription<t_features>::LElbowRoll ) = -0.00872665000000000;
337  max_bounds(ModelDescription<t_features>::LWristYaw ) = 1.82387000000000010;
338  max_bounds(ModelDescription<t_features>::RShoulderPitch) = 2.08566999999999991;
339  max_bounds(ModelDescription<t_features>::RShoulderRoll ) = -0.00872665000000000;
340  max_bounds(ModelDescription<t_features>::RElbowYaw ) = 2.08566999999999991;
341  max_bounds(ModelDescription<t_features>::RElbowRoll ) = 1.56207000000000007;
342  max_bounds(ModelDescription<t_features>::RWristYaw ) = 1.82387000000000010;
343  }
344 
345 
346  /**
347  * @brief Returns bounds on joint velocities.
348  *
349  * @tparam t_features these features identify model description
350  *
351  * @param[out] joint_vel_bounds
352  *
353  * @todo RBDL must load this from an URDF file!
354  */
355  template <int t_features>
356  static void getJointVelocityBounds(EIGENTOOLS_CONSTANT_SIZE_VECTOR(ModelDescription<t_features>::JOINTS_DOF_NUMBER) & joint_vel_bounds)
357  {
358  joint_vel_bounds(ModelDescription<t_features>::KneePitch ) = 2.93276;
359  joint_vel_bounds(ModelDescription<t_features>::HipPitch ) = 2.93276;
360  joint_vel_bounds(ModelDescription<t_features>::HipRoll ) = 2.27032;
361  joint_vel_bounds(ModelDescription<t_features>::HeadYaw ) = 7.33998;
362  joint_vel_bounds(ModelDescription<t_features>::HeadPitch ) = 9.22756;
363  joint_vel_bounds(ModelDescription<t_features>::LShoulderPitch) = 7.33998;
364  joint_vel_bounds(ModelDescription<t_features>::LShoulderRoll ) = 9.22756;
365  joint_vel_bounds(ModelDescription<t_features>::LElbowYaw ) = 7.33998;
366  joint_vel_bounds(ModelDescription<t_features>::LElbowRoll ) = 9.22756;
367  joint_vel_bounds(ModelDescription<t_features>::LWristYaw ) = 17.3835;
368  joint_vel_bounds(ModelDescription<t_features>::RShoulderPitch) = 7.33998;
369  joint_vel_bounds(ModelDescription<t_features>::RShoulderRoll ) = 9.22756;
370  joint_vel_bounds(ModelDescription<t_features>::RElbowYaw ) = 7.33998;
371  joint_vel_bounds(ModelDescription<t_features>::RElbowRoll ) = 9.22756;
372  joint_vel_bounds(ModelDescription<t_features>::RWristYaw ) = 17.3835;
373  }
374 
375 
376  /**
377  * @brief Sets joint angles in the given vector to their default values.
378  *
379  * @tparam t_features these features identify model description
380  *
381  * @param[out] joint_angles vector, where the joint angles are set
382  */
383  template <int t_features>
384  static void getDefaultJointAngles(EIGENTOOLS_CONSTANT_SIZE_VECTOR(ModelDescription<t_features>::JOINTS_DOF_NUMBER) & joint_angles)
385  {
386  /*
387  joint_angles(ModelDescription<t_features>::HeadYaw ) = 0.0;
388  joint_angles(ModelDescription<t_features>::HeadPitch ) = -0.2;
389 
390  joint_angles(ModelDescription<t_features>::HipRoll ) = 0.0;
391  joint_angles(ModelDescription<t_features>::HipPitch ) = -0.04;
392  joint_angles(ModelDescription<t_features>::KneePitch ) = -0.01;
393 
394  joint_angles(ModelDescription<t_features>::LShoulderPitch) = 1.57;
395  joint_angles(ModelDescription<t_features>::LShoulderRoll ) = 0.12;
396  joint_angles(ModelDescription<t_features>::LElbowYaw ) = -1.2217007;
397  joint_angles(ModelDescription<t_features>::LElbowRoll ) = -0.52345699;
398  joint_angles(ModelDescription<t_features>::LWristYaw ) = 0.0;
399 
400  joint_angles(ModelDescription<t_features>::RShoulderPitch) = 1.57;
401  joint_angles(ModelDescription<t_features>::RShoulderRoll ) = -0.12;
402  joint_angles(ModelDescription<t_features>::RElbowYaw ) = 1.2217007;
403  joint_angles(ModelDescription<t_features>::RElbowRoll ) = 0.52345699;
404  joint_angles(ModelDescription<t_features>::RWristYaw ) = 0.0;
405  */
406 
407  joint_angles(ModelDescription<t_features>::KneePitch ) = -0.1975011835036236;
408  joint_angles(ModelDescription<t_features>::HipPitch ) = 0.4160023626190753;
409  joint_angles(ModelDescription<t_features>::HipRoll ) = 0.0;
410 
411  joint_angles(ModelDescription<t_features>::HeadYaw ) = 0.0;
412  joint_angles(ModelDescription<t_features>::HeadPitch ) = 0.06850114982656237;
413 
414  joint_angles(ModelDescription<t_features>::LShoulderPitch) = 1.639454123287812;
415  joint_angles(ModelDescription<t_features>::LShoulderRoll ) = 0.12;
416  joint_angles(ModelDescription<t_features>::LElbowYaw ) = -1.2217007;
417  joint_angles(ModelDescription<t_features>::LElbowRoll ) = -0.52345699;
418  joint_angles(ModelDescription<t_features>::LWristYaw ) = 0.0;
419 
420  joint_angles(ModelDescription<t_features>::RShoulderPitch) = 1.639454123287804;
421  joint_angles(ModelDescription<t_features>::RShoulderRoll ) = -0.12;
422  joint_angles(ModelDescription<t_features>::RElbowYaw ) = 1.2217007;
423  joint_angles(ModelDescription<t_features>::RElbowRoll ) = 0.52345699;
424  joint_angles(ModelDescription<t_features>::RWristYaw ) = 0.0;
425  }
426  };
427 
428 
429 #define HUMOTO_PARENT_CLASS_SHORTHAND ModelDescriptionBase<ModelFeatures::FIXED_WHEELS>
430  /**
431  * @brief Specific model description (default model)
432  */
433  template <>
434  class HUMOTO_LOCAL ModelDescription<ModelFeatures::FIXED_WHEELS | ModelFeatures::ROOT_TORSO> :
435  public HUMOTO_PARENT_CLASS_SHORTHAND
436  {
437  private:
438  static const int features_ = ModelFeatures::FIXED_WHEELS | ModelFeatures::ROOT_TORSO;
439 
440 
441  public:
442  /**
443  * @brief Ids of root DoF.
444  */
445  enum RootDoFIds
446  {
447  ROOT_TRANSLATION_X = 0,
448  ROOT_TRANSLATION_Y = 1,
449  ROOT_TRANSLATION_Z = 2,
450 
451  ROOT_ORIENTATION_ROLL = 3,
452  ROOT_ORIENTATION_PITCH = 4,
453  ROOT_ORIENTATION_YAW = 5,
454 
455  ROOT_DOF_NUMBER = 6
456  };
457 
458 
459  /**
460  * @brief Ids of joints DoF.
461  */
462  enum JointsDoFIds
463  {
464  HeadYaw = 0,
465  HeadPitch = 1,
466 
467  HipRoll = 2,
468  HipPitch = 3,
469  KneePitch = 4,
470 
471  LShoulderPitch = 5,
472  LShoulderRoll = 6,
473  LElbowYaw = 7,
474  LElbowRoll = 8,
475  LWristYaw = 9,
476 
477  RShoulderPitch = 10,
478  RShoulderRoll = 11,
479  RElbowYaw = 12,
480  RElbowRoll = 13,
481  RWristYaw = 14,
482 
483  JOINTS_DOF_NUMBER = 15,
484 
485  DOF_NUMBER = ROOT_DOF_NUMBER + JOINTS_DOF_NUMBER
486  };
487 
488  typedef EIGENTOOLS_CONSTANT_SIZE_VECTOR(JOINTS_DOF_NUMBER) JointAnglesVector;
489 
490  public:
491  /**
492  * @brief Get default generalized coordinates
493  *
494  * @param[out] joint_angles
495  * @param[out] root_pose
496  */
497  static void getDefaultGeneralizedCoordinates(JointAnglesVector & joint_angles,
498  etools::Vector6 & root_pose)
499  {
500  root_pose.segment(ROOT_TRANSLATION_X, 3) << 0.0135554392293566, 0.0, 0.819715156461154;
501  root_pose.segment(ROOT_ORIENTATION_ROLL, 3) << 0.0, 0.05, 0.0;
502 
503  HUMOTO_PARENT_CLASS_SHORTHAND::getDefaultJointAngles<features_>(joint_angles);
504  }
505 
506 
507  /**
508  * @brief Returns joint position bounds.
509  *
510  * @param[out] min_bounds
511  * @param[out] max_bounds
512  */
513  static void getJointPositionBounds(JointAnglesVector & min_bounds,
514  JointAnglesVector & max_bounds)
515  {
516  HUMOTO_PARENT_CLASS_SHORTHAND::getJointPositionBounds<features_>(min_bounds, max_bounds);
517  }
518 
519 
520  /**
521  * @brief Returns bounds on joint velocities.
522  *
523  * @param[out] joint_vel_bounds
524  */
525  static void getJointVelocityBounds(JointAnglesVector & joint_vel_bounds)
526  {
527  HUMOTO_PARENT_CLASS_SHORTHAND::getJointVelocityBounds<features_>(joint_vel_bounds);
528  }
529 
530 
531  /**
532  * @brief Get default joint angles (with zero offset)
533  *
534  * @param[out] joint_angles
535  */
536  static void getDefaultJointAngles (JointAnglesVector & joint_angles)
537  {
538  HUMOTO_PARENT_CLASS_SHORTHAND::getDefaultJointAngles<features_>(joint_angles);
539  }
540 
541 
542 
543  /**
544  * @brief Set root position to the position given vector
545  *
546  * @param[out] root_pose
547  * @param[in] position
548  */
549  static void setRootPosition(etools::Vector6 & root_pose,
550  const etools::Vector3 & position)
551  {
552  root_pose.segment(ROOT_TRANSLATION_X, 3) = position;
553  }
554 
555 
556  /**
557  * @brief Set root orientation given RPY Euler angles.
558  *
559  * @param[out] root_pose
560  * @param[in] rpy
561  */
562  static void setRootOrientation( etools::Vector6 & root_pose,
563  const etools::Vector3 & rpy)
564  {
565  root_pose.segment(ROOT_ORIENTATION_ROLL, 3) =
566  rigidbody::convertEulerAngles( rpy,
567  rigidbody::EulerAngles::RPY,
568  rigidbody::EulerAngles::YPR);
569  }
570 
571 
572  /**
573  * @brief Get root position
574  *
575  * @param[out] position
576  * @param[in] root_pose
577  */
578  static void getRootPosition(etools::Vector3 & position,
579  const etools::Vector6 & root_pose)
580  {
581  position = root_pose.segment(ROOT_TRANSLATION_X, 3);
582  }
583 
584 
585  /**
586  * @brief Get root orientation given RPY Euler angles.
587  *
588  * @param[out] rpy
589  * @param[in] root_pose
590  */
591  static void getRootOrientation( etools::Vector3 & rpy,
592  const etools::Vector6 & root_pose)
593  {
594  rpy = rigidbody::convertEulerAngles(root_pose.segment(ROOT_ORIENTATION_ROLL, 3),
595  rigidbody::EulerAngles::YPR,
596  rigidbody::EulerAngles::RPY);
597  }
598 
599 
600  /**
601  * @brief Location of the torso root
602  *
603  * @param[in] parent_link_string_id
604  * @param[in] position
605  */
606  static void getTorsoRootLocation( std::string &parent_link_string_id,
607  etools::Vector3 & position)
608  {
609  parent_link_string_id = "torso";
610  position << 0.0, 0.0, 0.0;
611  }
612  };
613 
614 
615 
616  /**
617  * @brief Specific model description (relocated root)
618  */
619  template <>
620  class HUMOTO_LOCAL ModelDescription<ModelFeatures::FIXED_WHEELS | ModelFeatures::ROOT_TIBIA> :
621  public HUMOTO_PARENT_CLASS_SHORTHAND
622  {
623  private:
624  static const int features_ = ModelFeatures::FIXED_WHEELS | ModelFeatures::ROOT_TIBIA;
625 
626  public:
627  /**
628  * @brief Ids of root DoF.
629  */
630  enum RootDoFIds
631  {
632  ROOT_TRANSLATION_X = 0,
633  ROOT_TRANSLATION_Y = 1,
634  ROOT_TRANSLATION_Z = 2,
635 
636  ROOT_ORIENTATION_ROLL = 3,
637  ROOT_ORIENTATION_PITCH = 4,
638  ROOT_ORIENTATION_YAW = 5,
639 
640  ROOT_DOF_NUMBER = 6
641  };
642 
643 
644  /**
645  * @brief Ids of joints DoF.
646  */
647  enum JointsDoFIds
648  {
649  KneePitch = 0,
650  HipPitch = 1,
651  HipRoll = 2,
652 
653  HeadYaw = 3,
654  HeadPitch = 4,
655 
656  LShoulderPitch = 5,
657  LShoulderRoll = 6,
658  LElbowYaw = 7,
659  LElbowRoll = 8,
660  LWristYaw = 9,
661 
662  RShoulderPitch = 10,
663  RShoulderRoll = 11,
664  RElbowYaw = 12,
665  RElbowRoll = 13,
666  RWristYaw = 14,
667 
668  JOINTS_DOF_NUMBER = 15,
669 
670  DOF_NUMBER = ROOT_DOF_NUMBER + JOINTS_DOF_NUMBER
671  };
672 
673  typedef EIGENTOOLS_CONSTANT_SIZE_VECTOR(JOINTS_DOF_NUMBER) JointAnglesVector;
674 
675 
676  public:
677  /**
678  * @brief Get default generalized coordinates
679  *
680  * @param[out] joint_angles
681  * @param[out] root_pose
682  */
683  static void getDefaultGeneralizedCoordinates(JointAnglesVector & joint_angles,
684  etools::Vector6 & root_pose)
685  {
686  root_pose.setZero();
687  HUMOTO_PARENT_CLASS_SHORTHAND::getDefaultJointAngles<features_>(joint_angles);
688  }
689 
690 
691  /**
692  * @brief Returns joint position bounds.
693  *
694  * @param[out] min_bounds
695  * @param[out] max_bounds
696  */
697  static void getJointPositionBounds(JointAnglesVector & min_bounds,
698  JointAnglesVector & max_bounds)
699  {
700  HUMOTO_PARENT_CLASS_SHORTHAND::getJointPositionBounds<features_>(min_bounds, max_bounds);
701  }
702 
703 
704  /**
705  * @brief Returns bounds on joint velocities.
706  *
707  * @param[out] joint_vel_bounds
708  */
709  static void getJointVelocityBounds(JointAnglesVector & joint_vel_bounds)
710  {
711  HUMOTO_PARENT_CLASS_SHORTHAND::getJointVelocityBounds<features_>(joint_vel_bounds);
712  }
713 
714 
715  /**
716  * @brief Get default joint angles (with zero offset)
717  *
718  * @param[out] joint_angles
719  */
720  static void getDefaultJointAngles (JointAnglesVector & joint_angles)
721  {
722  HUMOTO_PARENT_CLASS_SHORTHAND::getDefaultJointAngles<features_>(joint_angles);
723  }
724 
725 
726  /**
727  * @brief Set root position to the position given vector
728  *
729  * @param[out] root_pose
730  * @param[in] position
731  */
732  static void setRootPosition(etools::Vector6 & root_pose,
733  const etools::Vector3 & position)
734  {
735  root_pose.segment(ROOT_TRANSLATION_X, 3) = position;
736  }
737 
738 
739  /**
740  * @brief Set root orientation given RPY Euler angles.
741  *
742  * @param[out] root_pose
743  * @param[in] rpy
744  */
745  static void setRootOrientation( etools::Vector6 & root_pose,
746  const etools::Vector3 & rpy)
747  {
748  root_pose.segment(ROOT_ORIENTATION_ROLL, 3) =
749  rigidbody::convertEulerAngles( rpy,
750  rigidbody::EulerAngles::RPY,
751  rigidbody::EulerAngles::YPR);
752  }
753 
754 
755  /**
756  * @brief Get root position
757  *
758  * @param[out] position
759  * @param[in] root_pose
760  */
761  static void getRootPosition(etools::Vector3 & position,
762  const etools::Vector6 & root_pose)
763  {
764  position = root_pose.segment(ROOT_TRANSLATION_X, 3);
765  }
766 
767 
768  /**
769  * @brief Location of the torso root
770  *
771  * @param[in] parent_link_string_id
772  * @param[in] position
773  */
774  static void getTorsoRootLocation( std::string &parent_link_string_id,
775  etools::Vector3 & position)
776  {
777  parent_link_string_id = "torso";
778  position << -2e-05, 0, 0.139;
779  }
780 
781 
782  /**
783  * @brief Get root orientation given RPY Euler angles.
784  *
785  * @param[out] rpy
786  * @param[in] root_pose
787  */
788  static void getRootOrientation( etools::Vector3 & rpy,
789  const etools::Vector6 & root_pose)
790  {
791  rpy = rigidbody::convertEulerAngles(root_pose.segment(ROOT_ORIENTATION_ROLL, 3),
792  rigidbody::EulerAngles::YPR,
793  rigidbody::EulerAngles::RPY);
794  }
795  };
796 
797 
798 
799  /**
800  * @brief Specific model description (planar root joint)
801  */
802  template <>
803  class HUMOTO_LOCAL ModelDescription<ModelFeatures::FIXED_WHEELS | ModelFeatures::ROOT_PLANAR> :
804  public HUMOTO_PARENT_CLASS_SHORTHAND
805  {
806  private:
807  static const int features_ = ModelFeatures::FIXED_WHEELS | ModelFeatures::ROOT_PLANAR;
808 
809  public:
810  /**
811  * @brief Ids of root DoF.
812  */
813  enum RootDoFIds
814  {
815  ROOT_TRANSLATION_X = 0,
816  ROOT_TRANSLATION_Y = 1,
817  ROOT_ORIENTATION_YAW = 2,
818 
819  ROOT_DOF_NUMBER = 3
820  };
821 
822 
823  /**
824  * @brief Ids of joints DoF.
825  */
826  enum JointsDoFIds
827  {
828  KneePitch = 0,
829  HipPitch = 1,
830  HipRoll = 2,
831 
832  HeadYaw = 3,
833  HeadPitch = 4,
834 
835  LShoulderPitch = 5,
836  LShoulderRoll = 6,
837  LElbowYaw = 7,
838  LElbowRoll = 8,
839  LWristYaw = 9,
840 
841  RShoulderPitch = 10,
842  RShoulderRoll = 11,
843  RElbowYaw = 12,
844  RElbowRoll = 13,
845  RWristYaw = 14,
846 
847  JOINTS_DOF_NUMBER = 15,
848 
849  DOF_NUMBER = ROOT_DOF_NUMBER + JOINTS_DOF_NUMBER
850  };
851 
852  typedef EIGENTOOLS_CONSTANT_SIZE_VECTOR(JOINTS_DOF_NUMBER) JointAnglesVector;
853 
854 
855  public:
856  /**
857  * @brief Get default generalized coordinates
858  *
859  * @param[out] joint_angles
860  * @param[out] root_pose
861  */
862  static void getDefaultGeneralizedCoordinates(JointAnglesVector & joint_angles,
863  etools::Vector3 & root_pose)
864  {
865  root_pose.segment(ROOT_TRANSLATION_X, 2) << 0.0, 0.0;
866  root_pose(ROOT_ORIENTATION_YAW) = 0.0;
867 
868  HUMOTO_PARENT_CLASS_SHORTHAND::getDefaultJointAngles<features_>(joint_angles);
869  }
870 
871 
872  /**
873  * @brief Returns joint position bounds.
874  *
875  * @param[out] min_bounds
876  * @param[out] max_bounds
877  */
878  static void getJointPositionBounds( JointAnglesVector & min_bounds,
879  JointAnglesVector & max_bounds)
880  {
881  HUMOTO_PARENT_CLASS_SHORTHAND::getJointPositionBounds<features_>(min_bounds, max_bounds);
882  }
883 
884 
885  /**
886  * @brief Returns bounds on joint velocities.
887  *
888  * @param[out] joint_vel_bounds
889  */
890  static void getJointVelocityBounds(JointAnglesVector & joint_vel_bounds)
891  {
892  HUMOTO_PARENT_CLASS_SHORTHAND::getJointVelocityBounds<features_>(joint_vel_bounds);
893  }
894 
895 
896  /**
897  * @brief Get default joint angles (with zero offset)
898  *
899  * @param[out] joint_angles
900  */
901  static void getDefaultJointAngles (JointAnglesVector & joint_angles)
902  {
903  HUMOTO_PARENT_CLASS_SHORTHAND::getDefaultJointAngles<features_>(joint_angles);
904  }
905 
906 
907  /**
908  * @brief Set root position to the position given vector
909  *
910  * @param[out] root_pose
911  * @param[in] position
912  */
913  static void setRootPosition(etools::Vector3 & root_pose,
914  const etools::Vector3 & position)
915  {
916  HUMOTO_ASSERT( 0.0 == position.z(),
917  "Translation along the Z axis is not supported.");
918  root_pose(ROOT_TRANSLATION_X) = position.x();
919  root_pose(ROOT_TRANSLATION_Y) = position.y();
920  }
921 
922 
923  /**
924  * @brief Set root orientation given RPY Euler angles.
925  *
926  * @param[out] root_pose
927  * @param[in] rpy
928  */
929  static void setRootOrientation( etools::Vector3 & root_pose,
930  const etools::Vector3 & rpy)
931  {
932  HUMOTO_ASSERT( (0.0 == rpy.x()) && (0.0 == rpy.y()),
933  "Only rotation about the Z axis is supported.");
934  root_pose(ROOT_ORIENTATION_YAW) = rpy.z();
935  }
936 
937 
938  /**
939  * @brief Get root position
940  *
941  * @param[out] position
942  * @param[in] root_pose
943  */
944  static void getRootPosition(etools::Vector3 & position,
945  const etools::Vector3 & root_pose)
946  {
947  position.x() = root_pose(ROOT_TRANSLATION_X);
948  position.y() = root_pose(ROOT_TRANSLATION_Y);
949  position.z() = 0.0;
950  }
951 
952 
953  /**
954  * @brief Get root orientation given RPY Euler angles.
955  *
956  * @param[out] rpy
957  * @param[in] root_pose
958  */
959  static void getRootOrientation( etools::Vector3 & rpy,
960  const etools::Vector3 & root_pose)
961  {
962  rpy << 0.0, 0.0, root_pose(ROOT_ORIENTATION_YAW);
963  }
964 
965 
966  /**
967  * @brief Location of the torso root
968  *
969  * @param[in] parent_link_string_id
970  * @param[in] position
971  */
972  static void getTorsoRootLocation( std::string &parent_link_string_id,
973  etools::Vector3 & position)
974  {
975  parent_link_string_id = "torso";
976  position << -2e-05, 0, 0.139;
977  }
978  };
979 #undef HUMOTO_PARENT_CLASS_SHORTHAND
980  }
981 }
humoto::pepper_ik::ModelDescriptionBase< ModelFeatures::FIXED_WHEELS >::getBodyLinksStringIds
static void getBodyLinksStringIds(std::vector< std::string > &ids)
Get string ids of the links comprising the upper body.
Definition: model_description.h:100
humoto::pepper_ik::ModelDescriptionBase< ModelFeatures::FIXED_WHEELS >::reorderJointsColumns
static void reorderJointsColumns(Eigen::MatrixXd &matrix)
Use joint order to reorder columns of the given matrix.
Definition: model_description.h:253
HUMOTO_LOCAL
#define HUMOTO_LOCAL
Definition: export_import.h:26
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_PLANAR >::JointsDoFIds
JointsDoFIds
Ids of joints DoF.
Definition: model_description.h:826
humoto::pepper_ik::ModelFeatures::ROOT_PLANAR
Definition: model_description.h:39
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TORSO >::getDefaultGeneralizedCoordinates
static void getDefaultGeneralizedCoordinates(JointAnglesVector &joint_angles, etools::Vector6 &root_pose)
Get default generalized coordinates.
Definition: model_description.h:497
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TIBIA >::getRootOrientation
static void getRootOrientation(etools::Vector3 &rpy, const etools::Vector6 &root_pose)
Get root orientation given RPY Euler angles.
Definition: model_description.h:788
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TORSO >::JointsDoFIds
JointsDoFIds
Ids of joints DoF.
Definition: model_description.h:462
humoto::pepper_ik::ModelDescriptionBase< ModelFeatures::FIXED_WHEELS >::getJointVelocityBounds
static void getJointVelocityBounds(EIGENTOOLS_CONSTANT_SIZE_VECTOR(ModelDescription< t_features >::JOINTS_DOF_NUMBER) &joint_vel_bounds)
Returns bounds on joint velocities.
Definition: model_description.h:356
HUMOTO_ASSERT
#define HUMOTO_ASSERT(condition, message)
Definition: humoto_helpers.h:86
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_PLANAR >::getRootOrientation
static void getRootOrientation(etools::Vector3 &rpy, const etools::Vector3 &root_pose)
Get root orientation given RPY Euler angles.
Definition: model_description.h:959
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_PLANAR >::getDefaultGeneralizedCoordinates
static void getDefaultGeneralizedCoordinates(JointAnglesVector &joint_angles, etools::Vector3 &root_pose)
Get default generalized coordinates.
Definition: model_description.h:862
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_PLANAR >::getRootPosition
static void getRootPosition(etools::Vector3 &position, const etools::Vector3 &root_pose)
Get root position.
Definition: model_description.h:944
HUMOTO_PARENT_CLASS_SHORTHAND
#define HUMOTO_PARENT_CLASS_SHORTHAND
Definition: model_description.h:429
humoto::rigidbody::convertEulerAngles
etools::Vector3 convertEulerAngles(const etools::Vector3 &euler_angles, const EulerAngles::Type euler_angles_type_from, const EulerAngles::Type euler_angles_type_to)
Convert Euler angles to a different set ofEuler angles.
Definition: rotary_state.h:107
humoto::pepper_ik::ModelDescriptionBase< ModelFeatures::FIXED_WHEELS >::getBaseLinksStringIds
static void getBaseLinksStringIds(std::vector< std::string > &ids)
Get string ids of the bodies comprising the base.
Definition: model_description.h:83
humoto::pepper_ik::ModelDescriptionBase< ModelFeatures::FIXED_WHEELS >::getRootMotionToWheelsTransform
static etools::Matrix3 getRootMotionToWheelsTransform()
Returns matrix which maps velocities of the wheels to translational and rotational motion of the base...
Definition: model_description.h:188
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TIBIA >::getTorsoRootLocation
static void getTorsoRootLocation(std::string &parent_link_string_id, etools::Vector3 &position)
Location of the torso root.
Definition: model_description.h:774
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_PLANAR >::RootDoFIds
RootDoFIds
Ids of root DoF.
Definition: model_description.h:813
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TIBIA >::setRootPosition
static void setRootPosition(etools::Vector6 &root_pose, const etools::Vector3 &position)
Set root position to the position given vector.
Definition: model_description.h:732
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_PLANAR >::setRootPosition
static void setRootPosition(etools::Vector3 &root_pose, const etools::Vector3 &position)
Set root position to the position given vector.
Definition: model_description.h:913
humoto::pepper_ik::ModelFeatures::ROOT_TORSO
Definition: model_description.h:37
etools::unsetMatrix
void unsetMatrix(Eigen::DenseBase< t_Derived > &matrix)
Unset matrix (initialize to NaN)
Definition: eigentools.h:178
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TORSO >::getJointVelocityBounds
static void getJointVelocityBounds(JointAnglesVector &joint_vel_bounds)
Returns bounds on joint velocities.
Definition: model_description.h:525
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TIBIA >::getDefaultJointAngles
static void getDefaultJointAngles(JointAnglesVector &joint_angles)
Get default joint angles (with zero offset)
Definition: model_description.h:720
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_PLANAR >::getJointVelocityBounds
static void getJointVelocityBounds(JointAnglesVector &joint_vel_bounds)
Returns bounds on joint velocities.
Definition: model_description.h:890
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_PLANAR >::getTorsoRootLocation
static void getTorsoRootLocation(std::string &parent_link_string_id, etools::Vector3 &position)
Location of the torso root.
Definition: model_description.h:972
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TORSO >::getJointPositionBounds
static void getJointPositionBounds(JointAnglesVector &min_bounds, JointAnglesVector &max_bounds)
Returns joint position bounds.
Definition: model_description.h:513
humoto::pepper_ik::ModelFeatures::FIXED_WHEELS
Definition: model_description.h:34
humoto::pepper_ik::ModelDescriptionBase< ModelFeatures::FIXED_WHEELS >::getBasePositionLinkStringId
static void getBasePositionLinkStringId(std::string &id)
Get id of the body which is used to control position of the base.
Definition: model_description.h:176
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TORSO >::setRootOrientation
static void setRootOrientation(etools::Vector6 &root_pose, const etools::Vector3 &rpy)
Set root orientation given RPY Euler angles.
Definition: model_description.h:562
humoto::pepper_ik::ModelDescription
class HUMOTO_LOCAL ModelDescription
Specific model description classes.
Definition: model_description.h:59
humoto
The root namespace of HuMoTo.
Definition: config.h:12
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TIBIA >::getJointPositionBounds
static void getJointPositionBounds(JointAnglesVector &min_bounds, JointAnglesVector &max_bounds)
Returns joint position bounds.
Definition: model_description.h:697
humoto::pepper_ik::ModelDescriptionBase< ModelFeatures::FIXED_WHEELS >::getDefaultJointAngles
static void getDefaultJointAngles(EIGENTOOLS_CONSTANT_SIZE_VECTOR(ModelDescription< t_features >::JOINTS_DOF_NUMBER) &joint_angles)
Sets joint angles in the given vector to their default values.
Definition: model_description.h:384
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TORSO >::setRootPosition
static void setRootPosition(etools::Vector6 &root_pose, const etools::Vector3 &position)
Set root position to the position given vector.
Definition: model_description.h:549
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_PLANAR >::getJointPositionBounds
static void getJointPositionBounds(JointAnglesVector &min_bounds, JointAnglesVector &max_bounds)
Returns joint position bounds.
Definition: model_description.h:878
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TORSO >::RootDoFIds
RootDoFIds
Ids of root DoF.
Definition: model_description.h:445
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_PLANAR >::setRootOrientation
static void setRootOrientation(etools::Vector3 &root_pose, const etools::Vector3 &rpy)
Set root orientation given RPY Euler angles.
Definition: model_description.h:929
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TORSO >::getDefaultJointAngles
static void getDefaultJointAngles(JointAnglesVector &joint_angles)
Get default joint angles (with zero offset)
Definition: model_description.h:536
etools::Matrix1x3
EIGENTOOLS_CONSTANT_SIZE_MATRIX Matrix1x3
Definition: eigentools.h:87
humoto::pepper_ik::ModelDescriptionBase< ModelFeatures::FIXED_WHEELS >::WheelIds
WheelIds
Definition: model_description.h:70
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TIBIA >::setRootOrientation
static void setRootOrientation(etools::Vector6 &root_pose, const etools::Vector3 &rpy)
Set root orientation given RPY Euler angles.
Definition: model_description.h:745
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_PLANAR >::getDefaultJointAngles
static void getDefaultJointAngles(JointAnglesVector &joint_angles)
Get default joint angles (with zero offset)
Definition: model_description.h:901
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TIBIA >::getDefaultGeneralizedCoordinates
static void getDefaultGeneralizedCoordinates(JointAnglesVector &joint_angles, etools::Vector6 &root_pose)
Get default generalized coordinates.
Definition: model_description.h:683
humoto::pepper_ik::ModelFeatures::Features
Features
Binary map.
Definition: model_description.h:30
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TORSO >::getRootPosition
static void getRootPosition(etools::Vector3 &position, const etools::Vector6 &root_pose)
Get root position.
Definition: model_description.h:578
humoto::pepper_ik::ModelDescriptionBase< ModelFeatures::FIXED_WHEELS >::getBaseOrientationLinkStringId
static void getBaseOrientationLinkStringId(std::string &id)
Get id of the body which is used to control orientation of the base.
Definition: model_description.h:165
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TORSO >::getTorsoRootLocation
static void getTorsoRootLocation(std::string &parent_link_string_id, etools::Vector3 &position)
Location of the torso root.
Definition: model_description.h:606
humoto::pepper_ik::ModelDescriptionBase
class HUMOTO_LOCAL ModelDescriptionBase
Base model description class.
Definition: model_description.h:51
humoto::pepper_ik::ModelDescriptionBase< ModelFeatures::FIXED_WHEELS >::getMaxWheelVelocities
static void getMaxWheelVelocities(etools::Vector3 &max_velocities)
Returns maximal velocites, which can be executed by the wheels.
Definition: model_description.h:290
humoto::g_pi
const double g_pi
PI constant.
Definition: constants.h:18
EIGENTOOLS_CONSTANT_SIZE_VECTOR
#define EIGENTOOLS_CONSTANT_SIZE_VECTOR(rows)
Definition: eigentools.h:58
humoto::pepper_ik::ModelFeatures::ROOT_TIBIA
Definition: model_description.h:38
humoto::pepper_ik::ModelFeatures
Model features are used to define different versions of the model.
Definition: model_description.h:21
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TIBIA >::getRootPosition
static void getRootPosition(etools::Vector3 &position, const etools::Vector6 &root_pose)
Get root position.
Definition: model_description.h:761
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TIBIA >::getJointVelocityBounds
static void getJointVelocityBounds(JointAnglesVector &joint_vel_bounds)
Returns bounds on joint velocities.
Definition: model_description.h:709
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TIBIA >::JointsDoFIds
JointsDoFIds
Ids of joints DoF.
Definition: model_description.h:647
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TIBIA >::RootDoFIds
RootDoFIds
Ids of root DoF.
Definition: model_description.h:630
etools::Matrix3
EIGENTOOLS_CONSTANT_SIZE_MATRIX Matrix3
Definition: eigentools.h:77
humoto::pepper_ik::ModelDescription< ModelFeatures::FIXED_WHEELS|ModelFeatures::ROOT_TORSO >::getRootOrientation
static void getRootOrientation(etools::Vector3 &rpy, const etools::Vector6 &root_pose)
Get root orientation given RPY Euler angles.
Definition: model_description.h:591
humoto::pepper_ik::ModelDescriptionBase< ModelFeatures::FIXED_WHEELS >::getJointPositionBounds
static void getJointPositionBounds(EIGENTOOLS_CONSTANT_SIZE_VECTOR(ModelDescription< t_features >::JOINTS_DOF_NUMBER) &min_bounds, EIGENTOOLS_CONSTANT_SIZE_VECTOR(ModelDescription< t_features >::JOINTS_DOF_NUMBER) &max_bounds)
Returns joint position bounds.
Definition: model_description.h:309
humoto::rigidbody::convertEulerAnglesToMatrix
etools::Matrix3 convertEulerAnglesToMatrix(const etools::Vector3 &euler_angles, const EulerAngles::Type euler_angles_type)
Convert rotation matrix to Euler angles.
Definition: rotary_state.h:71
Generated on Fri Sep 29 2017 16:44:40 for humoto by   doxygen 1.8.13