Choreonoid  1.5
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/Referenced>
10 #include <cnoid/EigenTypes>
11 #ifdef WIN32
12 #include "Link.h"
13 #include <cnoid/SceneGraph>
14 #include <cnoid/ValueTree>
15 #endif
16 #include "exportdecl.h"
17 
18 namespace cnoid {
19 
20 class Body;
21 
22 class Link;
23 typedef ref_ptr<Link> LinkPtr;
24 
25 class SgNode;
27 
28 class Mapping;
30 
32 {
33 public:
35 
36  Link();
37 
42  Link(const Link& link);
43 
44  virtual ~Link();
45 
46  int index() const { return index_; }
47  bool isValid() const { return (index_ >= 0); }
48 
49  Link* parent() const { return parent_; }
50  Link* sibling() const { return sibling_; }
51  Link* child() const { return child_; }
52 
53  bool isRoot() const { return !parent_; }
54 
55  Body* body() { return body_; }
56  const Body* body() const { return body_; }
57 
58  Position& T() { return T_; }
59  const Position& T() const { return T_; }
60 
61  Position& position() { return T_; }
62  const Position& position() const { return T_; }
63 
64  template<class Scalar, int Mode, int Options>
65  void setPosition(const Eigen::Transform<Scalar, 3, Mode, Options>& T) {
66  T_ = T.template cast<Position::Scalar>();
67  }
68 
69  template<typename Derived1, typename Derived2>
70  void setPosition(const Eigen::MatrixBase<Derived1>& rotation, const Eigen::MatrixBase<Derived2>& translation) {
71  T_.linear() = rotation;
72  T_.translation() = translation;
73  }
74 
75  Position::TranslationPart p() { return T_.translation(); }
76  Position::ConstTranslationPart p() const { return T_.translation(); }
77  Position::TranslationPart translation() { return T_.translation(); }
78  Position::ConstTranslationPart translation() const { return T_.translation(); }
79 
80  template<typename Derived>
81  void setTranslation(const Eigen::MatrixBase<Derived>& p) {
82  T_.translation() = p.template cast<Affine3::Scalar>();
83  }
84 
85  Position::LinearPart R() { return T_.linear(); }
86  Position::ConstLinearPart R() const { return T_.linear(); }
87  Position::LinearPart rotation() { return T_.linear(); }
88  Position::ConstLinearPart rotation() const { return T_.linear(); }
89 
90  template<typename Derived>
91  void setRotation(const Eigen::MatrixBase<Derived>& R) {
92  T_.linear() = R.template cast<Affine3::Scalar>();
93  }
94 
95  template<typename T>
96  void setRotation(const Eigen::AngleAxis<T>& a) {
97  T_.linear() = a.template cast<Affine3::Scalar>().toRotationMatrix();
98  }
99 
100  // To, Ro?
101  Position& Tb() { return Tb_; }
102  const Position& Tb() const { return Tb_; }
103 
104  Position::ConstTranslationPart b() const { return Tb_.translation(); }
105  Position::ConstTranslationPart offsetTranslation() const { return Tb_.translation(); }
106 
107  Position::ConstLinearPart Rb() const { return Tb_.linear(); }
108  Position::ConstLinearPart offsetRotation() const { return Tb_.linear(); }
109 
110  Matrix3& Rs() { return Rs_; }
111  const Matrix3& Rs() const { return Rs_; }
112 
113  enum JointType {
115  REVOLUTE_JOINT = 0,
116  ROTATIONAL_JOINT = REVOLUTE_JOINT,
118  SLIDE_JOINT = 1,
120  FREE_JOINT = 2,
121  /*
122  Joint types below here are treated as a fixed joint
123  when a code for processing a joint type is not given
124  */
126  FIXED_JOINT = 3,
128  PSEUDO_CONTINUOUS_TRACK = 4,
129  // deprecated
130  CRAWLER_JOINT = 5,
131 
132  AGX_CRAWLER_JOINT = 6
133  };
134 
135  int jointId() const { return jointId_; }
136 
137  JointType jointType() const { return jointType_; }
138  bool isFixedJoint() const { return (jointType_ >= FIXED_JOINT); }
139  bool isFreeJoint() const { return jointType_ == FREE_JOINT; }
140  bool isRotationalJoint() const { return jointType_ == ROTATIONAL_JOINT; }
141  bool isSlideJoint() const { return jointType_ == SLIDE_JOINT; }
142 
143  std::string jointTypeString() const;
144 
145  const Vector3& a() const { return a_; }
146  const Vector3& jointAxis() const { return a_; }
147  const Vector3& d() const { return a_; } // joint axis alias for a slide joint
148 
149  double q() const { return q_; }
150  double& q() { return q_; }
151  double dq() const { return dq_; }
152  double& dq() { return dq_; }
153  double ddq() const { return ddq_; }
154  double& ddq() { return ddq_; }
155  double u() const { return u_; }
156  double& u() { return u_; }
157 
158  double q_upper() const { return q_upper_; }
159  double q_lower() const { return q_lower_; }
160  double dq_upper() const { return dq_upper_; }
161  double dq_lower() const { return dq_lower_; }
162 
163  const Vector3& v() const { return v_; }
164  Vector3& v() { return v_; }
165  const Vector3& w() const { return w_; }
166  Vector3& w() { return w_; }
167  const Vector3& dv() const { return dv_; }
168  Vector3& dv() { return dv_; }
169  const Vector3& dw() const { return dw_; }
170  Vector3& dw() { return dw_; }
171 
173  const Vector3& c() const { return c_; }
174  const Vector3& centerOfMass() const { return c_; }
175 
177  const Vector3& wc() const { return wc_; }
178  const Vector3& centerOfMassGlobal() const { return wc_; }
179  Vector3& wc() { return wc_; }
180 
182  double m() const { return m_; }
183  double mass() const { return m_; }
184 
186  const Matrix3& I() const { return I_; }
187 
189  double Jm2() const { return Jm2_; }
190 
191  const Vector6& F_ext() const { return F_ext_; }
192  Vector6& F_ext() { return F_ext_; }
193  Vector6::ConstFixedSegmentReturnType<3>::Type f_ext() const { return F_ext_.head<3>(); }
194  Vector6::FixedSegmentReturnType<3>::Type f_ext() { return F_ext_.head<3>(); }
195  Vector6::ConstFixedSegmentReturnType<3>::Type tau_ext() const { return F_ext_.tail<3>(); }
196  Vector6::FixedSegmentReturnType<3>::Type tau_ext() { return F_ext_.tail<3>(); }
197 
198  const std::string& name() const { return name_; }
199 
200  SgNode* shape() const { return visualShape_; }
201  SgNode* visualShape() const { return visualShape_; }
202  SgNode* collisionShape() const { return collisionShape_; }
203 
204  // functions for constructing a link
205  void setIndex(int index) { index_ = index; }
206 
207  virtual void prependChild(Link* link);
208  virtual void appendChild(Link* link);
209  bool removeChild(Link* link);
210 
211  void setOffsetPosition(const Position& T){
212  Tb_ = T;
213  }
214 
215  template<typename Derived>
216  void setOffsetTranslation(const Eigen::MatrixBase<Derived>& offset) {
217  Tb_.translation() = offset;
218  }
219  template<typename Derived>
220  void setOffsetRotation(const Eigen::MatrixBase<Derived>& offset) {
221  Tb_.linear() = offset;
222  }
223  template<typename T>
224  void setOffsetRotation(const Eigen::AngleAxis<T>& a) {
225  Tb_.linear() = a.template cast<Affine3::Scalar>().toRotationMatrix();
226  }
227 
228  template<typename Derived>
229  void setAccumulatedSegmentRotation(const Eigen::MatrixBase<Derived>& Rs) {
230  Rs_ = Rs;
231  }
232 
233  void setJointType(JointType type) { jointType_ = type; }
234  void setJointId(int id) { jointId_ = id; }
235  void setJointAxis(const Vector3& axis) { a_ = axis; }
236 
237  void setJointRange(double lower, double upper) { q_lower_ = lower; q_upper_ = upper; }
238  void setJointVelocityRange(double lower, double upper) { dq_lower_ = lower; dq_upper_ = upper; }
239 
240  void setCenterOfMass(const Vector3& c) { c_ = c; }
241  void setMass(double m) { m_ = m; }
242  void setInertia(const Matrix3& I) { I_ = I; }
243  void setEquivalentRotorInertia(double Jm2) { Jm2_ = Jm2; }
244 
245  void setName(const std::string& name);
246 
247  void setShape(SgNode* shape);
248  void setVisualShape(SgNode* shape);
249  void setCollisionShape(SgNode* shape);
250 
251  // The following two methods should be deprecated after introducing Tb
252  Matrix3 attitude() const { return R() * Rs_; }
253  void setAttitude(const Matrix3& Ra) { R() = Ra * Rs_.transpose(); }
254  Matrix3 calcRfromAttitude(const Matrix3& Ra) { return Ra * Rs_.transpose(); }
255 
256  const Mapping* info() const { return info_; }
257  Mapping* info() { return info_; }
258 
259  template<typename T> T info(const std::string& key) const;
260  template<typename T> T info(const std::string& key, const T& defaultValue) const;
261  template<typename T> void setInfo(const std::string& key, const T& value);
262 
263  void resetInfo(Mapping* info);
264 
265  double initialJointDisplacement() const { return initd_; }
266  double& initialJointDisplacement() { return initd_; }
267 
268 #ifdef CNOID_BACKWARD_COMPATIBILITY
269  // fext, tauext
270  const double& ulimit() const { return q_upper_; }
271  const double& llimit() const { return q_lower_; }
272  const double& uvlimit() const { return dq_upper_; }
273  const double& lvlimit() const { return dq_lower_; }
274 
275  Matrix3 segmentAttitude() const { return R() * Rs_; }
276  void setSegmentAttitude(const Matrix3& Ra) { R() = Ra * Rs_.transpose(); }
277 #endif
278 
279 private:
280  int index_;
281  int jointId_;
282  Link* parent_;
283  LinkPtr sibling_;
284  LinkPtr child_;
285  Body* body_;
286  Position T_;
287  Position Tb_;
288  Matrix3 Rs_; // temporary variable for porting. This should be removed later.
289  Vector3 a_;
290  JointType jointType_;
291  double q_;
292  double dq_;
293  double ddq_;
294  double u_;
295  Vector3 v_;
296  Vector3 w_;
297  Vector3 dv_;
298  Vector3 dw_;
299  Vector3 c_;
300  Vector3 wc_;
301  double m_;
302  Matrix3 I_;
303  double Jm2_;
304  Vector6 F_ext_; // should be Vector3 x 2?
305  double q_upper_;
306  double q_lower_;
307  double dq_upper_;
308  double dq_lower_;
309  std::string name_;
310  double initd_;
311  SgNodePtr visualShape_;
312  SgNodePtr collisionShape_;
313  MappingPtr info_;
314 
315  friend class Body;
316 
317  void setBody(Body* newBody);
318  void setBodySub(Body* newBody);
319 };
320 
321 template<> CNOID_EXPORT double Link::info(const std::string& key) const;
322 template<> CNOID_EXPORT double Link::info(const std::string& key, const double& defaultValue) const;
323 template<> CNOID_EXPORT void Link::setInfo(const std::string& key, const double& value);
324 
325 }
326 
327 #endif
Definition: Body.h:28
Definition: SceneGraph.h:142
Definition: ValueTree.h:224
Definition: Referenced.h:67
ref_ptr< SgNode > SgNodePtr
Definition: SceneGraph.h:138
Eigen::Transform< double, 3, Eigen::AffineCompact > Position
Definition: EigenTypes.h:73
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
ref_ptr< Link > LinkPtr
Definition: Link.h:22
Eigen::Vector3d Vector3
Definition: EigenTypes.h:58
ref_ptr< Mapping > MappingPtr
Definition: ValueTree.h:416
#define CNOID_EXPORT
Definition: Util/exportdecl.h:37
Eigen::Matrix< double, 6, 1 > Vector6
Definition: EigenTypes.h:63
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:57