Choreonoid  1.8
Link.h
Go to the documentation of this file.
1 
6 #ifndef CNOID_BODY_LINK_H
7 #define CNOID_BODY_LINK_H
8 
9 #include <cnoid/ClonableReferenced>
10 #include <cnoid/EigenTypes>
11 #include <cnoid/SceneUpdate>
12 #include <vector>
13 #include "exportdecl.h"
14 
15 namespace cnoid {
16 
17 class Body;
18 class SgNode;
19 class SgGroup;
20 class SgPosTransform;
21 class Mapping;
22 
23 class Link;
24 typedef ref_ptr<Link> LinkPtr;
25 
26 class CNOID_EXPORT Link : public ClonableReferenced
27 {
28 public:
29  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
30 
31  Link();
32 
37  Link(const Link& link);
38 
39  Link* clone() const {
40  return static_cast<Link*>(doClone(nullptr));
41  }
42 
43  virtual ~Link();
44 
45  virtual void initializeState();
46 
47  const std::string& name() const { return name_; }
48  int index() const { return index_; }
49  bool isValid() const { return (index_ >= 0); }
50  bool isRoot() const { return !parent_; }
51  bool isStatic() const;
52  bool isFixedToRoot() const;
53  bool isOwnerOf(const Link* link) const;
54  bool isEndLink() const { return !child_; }
55 
56  Body* body() { return body_; }
57  const Body* body() const { return body_; }
58  Link* parent() const { return parent_; }
59  Link* sibling() const { return sibling_; }
60  Link* child() const { return child_; }
61 
62  Isometry3& T() { return T_; }
63  const Isometry3& T() const { return T_; }
64 
65  Isometry3& position() { return T_; }
66  const Isometry3& position() const { return T_; }
67 
68  template<class Scalar, int Mode, int Options>
69  void setPosition(const Eigen::Transform<Scalar, 3, Mode, Options>& T){
70  T_ = T.template cast<Isometry3::Scalar>();
71  }
72  template<class Derived>
73  void setPosition(const Eigen::MatrixBase<Derived>& T){
74  T_ = T.template cast<Isometry3::Scalar>();
75  }
76  template<typename Derived1, typename Derived2>
77  void setPosition(const Eigen::MatrixBase<Derived1>& rotation, const Eigen::MatrixBase<Derived2>& translation){
78  T_.linear() = rotation;
79  T_.translation() = translation;
80  }
81 
82  Isometry3::TranslationPart p() { return T_.translation(); }
83  Isometry3::ConstTranslationPart p() const { return T_.translation(); }
84  Isometry3::TranslationPart translation() { return T_.translation(); }
85  Isometry3::ConstTranslationPart translation() const { return T_.translation(); }
86 
87  template<typename Derived>
88  void setTranslation(const Eigen::MatrixBase<Derived>& p) {
89  T_.translation() = p.template cast<Isometry3::Scalar>();
90  }
91 
92  Isometry3::LinearPart R() { return T_.linear(); }
93  Isometry3::ConstLinearPart R() const { return T_.linear(); }
94  Isometry3::LinearPart rotation() { return T_.linear(); }
95  Isometry3::ConstLinearPart rotation() const { return T_.linear(); }
96 
97  template<typename Derived>
98  void setRotation(const Eigen::MatrixBase<Derived>& R) {
99  T_.linear() = R.template cast<Isometry3::Scalar>();
100  }
101  template<typename T>
102  void setRotation(const Eigen::AngleAxis<T>& a) {
103  T_.linear() = a.template cast<Isometry3::Scalar>().toRotationMatrix();
104  }
105  template<typename Derived>
106  void setRotation(const Eigen::QuaternionBase<Derived>& q) {
107  T_.linear() = q.template cast<Isometry3::Scalar>().toRotationMatrix();
108  }
109 
110  // To, Ro?
111  const Isometry3& Tb() const { return Tb_; }
112  const Isometry3& offsetPosition() const { return Tb_; }
113 
114  Isometry3::ConstTranslationPart b() const { return Tb_.translation(); }
115  Isometry3::ConstTranslationPart offsetTranslation() const { return Tb_.translation(); }
116 
117  Isometry3::ConstLinearPart Rb() const { return Tb_.linear(); }
118  Isometry3::ConstLinearPart offsetRotation() const { return Tb_.linear(); }
119 
120  [[deprecated("This func. always returns the identity matrix")]]
121  Matrix3 Rs() const { return Matrix3::Identity(); }
122 
123  enum JointType {
124  RevoluteJoint = 0,
125  PrismaticJoint = 1,
127  FreeJoint = 2,
128  /*
129  Joint types below here are treated as a fixed joint
130  when a code for processing a joint type is not given
131  */
132  FixedJoint = 3,
133 
137  PseudoContinuousTrackJoint = 4,
138 
139  // Deprecated
140  REVOLUTE_JOINT = RevoluteJoint,
141  ROTATIONAL_JOINT = RevoluteJoint,
142  PRISMATIC_JOINT = PrismaticJoint,
143  SLIDE_JOINT = PrismaticJoint,
144  FREE_JOINT = FreeJoint,
145  FIXED_JOINT = FixedJoint,
146  PSEUDO_CONTINUOUS_TRACK = PseudoContinuousTrackJoint
147  };
148 
149 #if !defined(__GNUC__) || __GNUC__ > 5
150  [[deprecated("Use Link::PseudoContinuousTrack.")]]
151 #endif
152  static constexpr int PseudoContinousTrack = PseudoContinuousTrackJoint;
153 
154  int jointId() const { return jointId_; }
155  const std::string& jointName() const;
156  const std::string& jointSpecificName() const { return jointSpecificName_; }
157 
158  JointType jointType() const { return static_cast<JointType>(jointType_); }
159  const char* jointTypeLabel() const;
160  const char* jointTypeSymbol() const;
161  [[deprecated("Use jointTypeLabel or jointTypeSymbol")]]
162  const char* jointTypeString(bool useUnderscore = false) const;
163  bool isFixedJoint() const { return (jointType_ >= FixedJoint); }
164  bool isFreeJoint() const { return jointType_ == FreeJoint; }
165  bool isRevoluteJoint() const { return jointType_ == RevoluteJoint; }
166  bool isPrismaticJoint() const { return jointType_ == PrismaticJoint; }
167  bool hasJoint() const { return jointType_ <= 1; }
168 
170  bool isRotationalJoint() const { return jointType_ == RevoluteJoint; }
172  bool isSlideJoint() const { return jointType_ == PrismaticJoint; }
173 
174  const Vector3& a() const { return a_; }
175  const Vector3& jointAxis() const { return a_; }
176  const Vector3& d() const { return a_; } // joint axis alias for a slide joint
177 
179  double Jm2() const { return Jm2_; }
180 
181  enum StateFlag {
182 
183  // States
184  StateNone = 0,
185  JointDisplacement = 1 << 0,
186  JointAngle = JointDisplacement,
187  JointVelocity = 1 << 1,
188  JointAcceleration = 1 << 2,
189  JointEffort = 1 << 3,
190  JointForce = JointEffort,
191  JointTorque = JointEffort,
192  LinkPosition = 1 << 4,
193  LinkTwist = 1 << 5,
194  LinkExtWrench = 1 << 6,
195  LinkContactState = 1 << 7,
196 
197  // Options
198  HighGainActuation = 1 << 8,
199 
200  // Don't use this
201  DeprecatedJointSurfaceVelocity = 1 << 9,
202 
203  MaxStateTypeBit = 9,
204  NumStateTypes = 10,
205 
206  // Deprecated
207  NO_ACTUATION = StateNone,
208  JOINT_TORQUE = JointTorque,
209  JOINT_FORCE = JointForce,
210  JOINT_EFFORT = JointEffort,
211  JOINT_ANGLE = JointAngle,
212  JOINT_DISPLACEMENT = JointDisplacement,
213  JOINT_VELOCITY = JointVelocity,
214  LINK_POSITION = LinkPosition
215  };
216 
217 #if !defined(__GNUC__) || __GNUC__ > 5
218  [[deprecated("Use Link::JointVelocity as the actuation mode and Link::PseudoContinuousTrackJoint as the joint type.")]]
219 #endif
220  static constexpr int JOINT_SURFACE_VELOCITY = DeprecatedJointSurfaceVelocity;
221 
222  // \ret Logical sum of the correpsonding StateFlag bits
223  short actuationMode() const { return actuationMode_; }
224  // \param mode Logical sum of the correpsonding StateFlag bits
225  void setActuationMode(short mode) { actuationMode_ = mode; }
226 
227  [[deprecated("Just use the link name as a prefix or use the int type as a variable.")]]
228  typedef StateFlag ActuationMode;
229 
235  static constexpr short AllStateHighGainActuationMode =
236  LinkPosition | LinkTwist | LinkExtWrench | JointDisplacement | JointVelocity | JointEffort | HighGainActuation;
237 
238  // \ret Logical sum of the correpsonding StateFlag bits
239  short sensingMode() const { return sensingMode_; }
240  // \param mode Logical sum of the correpsonding StateFlag bits
241  void setSensingMode(short mode) { sensingMode_ = mode; }
242  void mergeSensingMode(short mode) { sensingMode_ |= mode; }
243 
244  static std::string getStateModeString(short mode);
245 
246  double q() const { return q_; }
247  double& q() { return q_; }
248  double dq() const { return dq_; }
249  double& dq() { return dq_; }
250  double ddq() const { return ddq_; }
251  double& ddq() { return ddq_; }
252  double u() const { return u_; }
253  double& u() { return u_; }
254 
255  double q_target() const { return q_target_; }
256  double& q_target() { return q_target_; }
257  double dq_target() const { return dq_target_; }
258  double& dq_target() { return dq_target_; }
259 
260  double q_initial() const { return q_initial_; }
261  double q_upper() const { return q_upper_; }
262  double q_lower() const { return q_lower_; }
263  double dq_upper() const { return dq_upper_; }
264  double dq_lower() const { return dq_lower_; }
265 
266  const Vector3& v() const { return v_; }
267  Vector3& v() { return v_; }
268  const Vector3& w() const { return w_; }
269  Vector3& w() { return w_; }
270  const Vector3& dv() const { return dv_; }
271  Vector3& dv() { return dv_; }
272  const Vector3& dw() const { return dw_; }
273  Vector3& dw() { return dw_; }
274 
276  const Vector3& c() const { return c_; }
277  const Vector3& centerOfMass() const { return c_; }
278 
280  const Vector3& wc() const { return wc_; }
281  const Vector3& centerOfMassGlobal() const { return wc_; }
282  Vector3& wc() { return wc_; }
283 
285  double m() const { return m_; }
286  double mass() const { return m_; }
287 
289  const Matrix3& I() const { return I_; }
290 
291  const Vector6& externalWrench() const { return F_ext_; }
292  Vector6& externalWrench() { return F_ext_; }
293  Vector6::ConstFixedSegmentReturnType<3>::Type externalForce() const { return F_ext_.head<3>(); }
294  Vector6::FixedSegmentReturnType<3>::Type externalForce() { return F_ext_.head<3>(); }
295  Vector6::ConstFixedSegmentReturnType<3>::Type externalTorque() const { return F_ext_.tail<3>(); }
296  Vector6::FixedSegmentReturnType<3>::Type externalTorque() { return F_ext_.tail<3>(); }
297 
298  const Vector6& F_ext() const { return F_ext_; }
299  Vector6& F_ext() { return F_ext_; }
300  Vector6::ConstFixedSegmentReturnType<3>::Type f_ext() const { return F_ext_.head<3>(); }
301  Vector6::FixedSegmentReturnType<3>::Type f_ext() { return F_ext_.head<3>(); }
302  Vector6::ConstFixedSegmentReturnType<3>::Type tau_ext() const { return F_ext_.tail<3>(); }
303  Vector6::FixedSegmentReturnType<3>::Type tau_ext() { return F_ext_.tail<3>(); }
304 
305  void addExternalForceAtLocalPosition(const Vector3& f_global, const Vector3& p_local){
306  f_ext() += f_global;
307  tau_ext() += (T_ * p_local).cross(f_global);
308  }
309 
310  void addExternalForceAtGlobalPosition(const Vector3& f_global, const Vector3& p_global){
311  f_ext() += f_global;
312  tau_ext() += p_global.cross(f_global);
313  }
314 
315  [[deprecated("Use addExternalForceAtLocalPosition.")]]
316  void addExternalForce(const Vector3& f_global, const Vector3& p_local){
317  addExternalForceAtLocalPosition(f_global, p_local);
318  }
319 
320  int materialId() const { return materialId_; }
321  std::string materialName() const;
322 
324  class ContactPoint
325  {
326  public:
327  ContactPoint(const Vector3& position, const Vector3& normal, const Vector3& force, const Vector3& velocity, double depth)
328  : position_(position), normal_(normal), force_(force), velocity_(velocity), depth_(depth) { }
329 
330  const Vector3& position() const { return position_; }
332  const Vector3& normal() const { return normal_; }
334  const Vector3& force() const { return force_; }
336  const Vector3& velocity() const { return velocity_; }
337  double depth() { return depth_; }
338 
339  private:
340  Vector3 position_;
341  Vector3 normal_;
342  Vector3 force_;
343  Vector3 velocity_;
344  double depth_;
345 
355  //int objectId_;
356  };
357 
362  std::vector<ContactPoint>& contactPoints() { return contactPoints_; }
363  const std::vector<ContactPoint>& contactPoints() const { return contactPoints_; }
364 
365  SgGroup* shape() const { return visualShape_; }
366  SgGroup* visualShape() const { return visualShape_; }
367  SgGroup* collisionShape() const { return collisionShape_; }
368  bool hasDedicatedCollisionShape() const;
369 
370  // functions for constructing a link
371  void setBodyToSubTree(Body* newBody);
372  void setParent(Link* parent){ parent_ = parent; }
373  void setIndex(int index) { index_ = index; }
374  void setName(const std::string& name);
375  virtual void prependChild(Link* link);
376  virtual void appendChild(Link* link);
377  bool removeChild(Link* link);
378 
379  void setOffsetPosition(const Isometry3& T){
380  Tb_ = T;
381  }
382 
383  template<typename Derived>
384  void setOffsetTranslation(const Eigen::MatrixBase<Derived>& offset) {
385  Tb_.translation() = offset;
386  }
387  template<typename Derived>
388  void setOffsetRotation(const Eigen::MatrixBase<Derived>& offset) {
389  Tb_.linear() = offset;
390  }
391  template<typename T>
392  void setOffsetRotation(const Eigen::AngleAxis<T>& a) {
393  Tb_.linear() = a.template cast<Isometry3::Scalar>().toRotationMatrix();
394  }
395 
396  template<typename Derived>
397  [[deprecated("No need to use this function.")]]
398  void setAccumulatedSegmentRotation(const Eigen::MatrixBase<Derived>& /* Rs */) {
399  }
400 
401  void setJointType(JointType type) { jointType_ = type; }
402  void setJointId(int id) { jointId_ = id; }
403  void setJointName(const std::string& name);
404  void resetJointSpecificName();
405  void setJointAxis(const Vector3& axis) { a_ = axis; }
406 
407  void setInitialJointDisplacement(double q) { q_initial_ = q; }
408  void setInitialJointAngle(double q) { q_initial_ = q; }
409  void setJointRange(double lower, double upper) { q_lower_ = lower; q_upper_ = upper; }
410  void setJointVelocityRange(double lower, double upper) { dq_lower_ = lower; dq_upper_ = upper; }
411 
412  void setCenterOfMass(const Vector3& c) { c_ = c; }
413  void setMass(double m) { m_ = m; }
414  void setInertia(const Matrix3& I) { I_ = I; }
415  void setEquivalentRotorInertia(double Jm2) { Jm2_ = Jm2; }
416 
417  void setMaterial(int id) { materialId_ = id; }
418  void setMaterial(const std::string& name);
419 
420  void addShapeNode(SgNode* shape, SgUpdateRef update = nullptr);
421  void addVisualShapeNode(SgNode* shape, SgUpdateRef update = nullptr);
422  void addCollisionShapeNode(SgNode* shape, SgUpdateRef update = nullptr);
423  void removeShapeNode(SgNode* shape, SgUpdateRef update = nullptr);
424  void clearShapeNodes(SgUpdateRef update = nullptr);
425 
426  [[deprecated("You don't have to use this function.")]]
427  void updateShapeRs() {}
428 
429  // The following two methods should be deprecated after introducing Tb
430  [[deprecated("Use T() instead.")]]
431  Isometry3 Ta() const { return T(); }
432  [[deprecated("Uuse R() instead.")]]
433  Matrix3 attitude() const { return R(); }
434  [[deprecated("Use setRotation(.) instead.")]]
435  void setAttitude(const Matrix3& Ra) { R() = Ra; }
436  [[deprecated]]
437  Matrix3 calcRfromAttitude(const Matrix3& Ra) { return Ra; }
438  [[deprecated("Use T() instead.")]]
439  void getAttitudeAndTranslation(Isometry3& out_T) { out_T = T(); };
440 
441  const Mapping* info() const { return info_; }
442  Mapping* info() { return info_; }
443  template<typename T> T info(const std::string& key) const;
444  template<typename T> T info(const std::string& key, const T& defaultValue) const;
445  template<typename T> void setInfo(const std::string& key, const T& value);
446  std::string info(const std::string& key, const char* defaultValue) const;
447  void resetInfo(Mapping* info);
448 
449 #ifdef CNOID_BACKWARD_COMPATIBILITY
450  // fext, tauext
451  const double& ulimit() const { return q_upper_; }
452  const double& llimit() const { return q_lower_; }
453  const double& uvlimit() const { return dq_upper_; }
454  const double& lvlimit() const { return dq_lower_; }
455 
456  Matrix3 segmentAttitude() const { return R(); }
457  void setSegmentAttitude(const Matrix3& Ra) { R() = Ra; }
458 #endif
459 
460 protected:
461  Referenced* doClone(CloneMap* cloneMap) const override;
462 
463 private:
464  int index_;
465  Link* parent_;
466  LinkPtr sibling_;
467  LinkPtr child_;
468  Body* body_;
469 
470  Isometry3 T_;
471  Isometry3 Tb_;
472 
473  short jointType_;
474  short jointId_;
475  short actuationMode_;
476  short sensingMode_;
477 
478  Vector3 a_;
479  double q_;
480  double dq_;
481  double ddq_;
482  double q_target_;
483  double dq_target_;
484  double u_;
485 
486  Vector3 v_;
487  Vector3 w_;
488  Vector3 dv_;
489  Vector3 dw_;
490  Vector6 F_ext_; // should be Vector3 x 2?
491 
492  Vector3 c_;
493  Vector3 wc_;
494  double m_;
495  Matrix3 I_;
496 
497  double Jm2_;
498  double q_initial_;
499  double q_upper_;
500  double q_lower_;
501  double dq_upper_;
502  double dq_lower_;
503 
504  int materialId_;
505 
506  std::vector<ContactPoint> contactPoints_;
507 
508  std::string name_;
509  std::string jointSpecificName_;
510 
511  ref_ptr<SgGroup> visualShape_;
512  ref_ptr<SgGroup> collisionShape_;
513 
514  ref_ptr<Mapping> info_;
515 
516  void setBodyToSubTreeIter(Body* newBody);
517 };
518 
519 template<> CNOID_EXPORT double Link::info(const std::string& key) const;
520 template<> CNOID_EXPORT double Link::info(const std::string& key, const double& defaultValue) const;
521 template<> CNOID_EXPORT bool Link::info(const std::string& key, const bool& defaultValue) const;
522 template<> CNOID_EXPORT void Link::setInfo(const std::string& key, const double& value);
523 template<> CNOID_EXPORT void Link::setInfo(const std::string& key, const bool& value);
524 
525 }
526 
527 #endif
cnoid::Mapping
Definition: ValueTree.h:253
cnoid::Vector3
Eigen::Vector3d Vector3
Definition: EigenTypes.h:57
cnoid::SgGroup
Definition: SceneGraph.h:220
cnoid::Isometry3
Eigen::Isometry3d Isometry3
Definition: EigenTypes.h:73
cnoid::Matrix3
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:56
cnoid::SgUpdateRef
Definition: SceneUpdate.h:73
cnoid::ref_ptr< Link >
cnoid::LinkPtr
ref_ptr< Link > LinkPtr
Definition: Link.h:23
cnoid
Definition: AbstractSceneLoader.h:11
cnoid::ClonableReferenced
Definition: ClonableReferenced.h:10
cnoid::Vector6
Eigen::Matrix< double, 6, 1 > Vector6
Definition: EigenTypes.h:61
cnoid::Body
Definition: Body.h:28
cnoid::SgNode
Definition: SceneGraph.h:157
Generated by   doxygen 1.8.17