Choreonoid  1.5
DyBody.h
Go to the documentation of this file.
1 
6 #ifndef CNOID_BODY_DYBODY_H
7 #define CNOID_BODY_DYBODY_H
8 
9 #include "Body.h"
10 #include "Link.h"
11 #include "exportdecl.h"
12 
13 namespace cnoid {
14 
18 class CNOID_EXPORT DyLink : public Link
19 {
20 public:
22 
23  DyLink();
24  DyLink(const Link& link);
25 
26  DyLink* parent() const { return static_cast<DyLink*>(Link::parent()); }
27  DyLink* sibling() const { return static_cast<DyLink*>(Link::sibling()); }
28  DyLink* child() const { return static_cast<DyLink*>(Link::child()); }
29 
30  const Vector3& vo() const { return vo_; }
31  Vector3& vo() { return vo_; }
32  const Vector3& dvo() const { return dvo_; }
33  Vector3& dvo() { return dvo_; }
34  const Vector3& sw() const { return sw_; }
35  Vector3& sw() { return sw_; }
36  const Vector3& sv() const { return sv_; }
37  Vector3& sv() { return sv_; }
38  const Vector3& cv() const { return cv_; }
39  Vector3& cv() { return cv_; }
40  const Vector3& cw() const { return cw_; }
41  Vector3& cw() { return cw_; }
42  const Matrix3& Iww() const { return Iww_; }
43  Matrix3& Iww() { return Iww_; }
44  const Matrix3& Iwv() const { return Iwv_; }
45  Matrix3& Iwv() { return Iwv_; }
46  const Matrix3& Ivv() const { return Ivv_; }
47  Matrix3& Ivv() { return Ivv_; }
48  const Vector3& pf() const { return pf_; }
49  Vector3& pf() { return pf_; }
50  const Vector3& ptau() const { return ptau_; }
51  Vector3& ptau() { return ptau_; }
52  const Vector3& hhv() const { return hhv_; }
53  Vector3& hhv() { return hhv_; }
54  const Vector3& hhw() const { return hhw_; }
55  Vector3& hhw() { return hhw_; }
56  double uu() const { return uu_; }
57  double& uu() { return uu_; }
58  double dd() const { return dd_; }
59  double& dd() { return dd_; }
60 
65  struct ConstraintForce {
66  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
67  ConstraintForce(const Vector3& point, const Vector3& force) : point(point), force(force) { }
70  };
71  typedef std::vector<ConstraintForce> ConstraintForceArray;
72 
73  ConstraintForceArray& constraintForces() { return constraintForces_; }
74  const ConstraintForceArray& constraintForces() const { return constraintForces_; }
75 
76  virtual void prependChild(Link* link);
77  virtual void appendChild(Link* link);
78 
79 private:
80  Vector3 vo_;
81  Vector3 dvo_;
82 
87  Vector3 sw_;
88 
93  Vector3 sv_;
94 
95  Vector3 cv_;
96  Vector3 cw_;
97 
98  Matrix3 Iww_;
99  Matrix3 Iwv_;
100  Matrix3 Ivv_;
101  Vector3 pf_;
102  Vector3 ptau_;
103  Vector3 hhv_;
104  Vector3 hhw_;
105  double uu_;
106  double dd_;
107 
108  std::vector<ConstraintForce> constraintForces_;
109 };
110 
111 
115 class CNOID_EXPORT DyBody : public Body
116 {
117 public:
118  DyBody();
119  DyBody(const Body& org);
120 
121  virtual Body* clone() const;
122  virtual Link* createLink(const Link* org = 0) const;
123 
124  DyLink* joint(int id) const {
125  return static_cast<DyLink*>(Body::joint(id));
126  }
127 
128  DyLink* link(int index) const {
129  return static_cast<DyLink*>(Body::link(index));
130  }
131 
132  DyLink* link(const std::string& name) const {
133  return static_cast<DyLink*>(Body::link(name));
134  }
135 
136  DyLink* rootLink() const {
137  return static_cast<DyLink*>(Body::rootLink());
138  }
139 
140  void calcSpatialForwardKinematics();
141 };
142 
144 
145 };
146 
147 #endif
Link * rootLink() const
Definition: Body.h:127
Definition: Body.h:28
Definition: DyBody.h:115
Link * link(int index) const
Definition: Body.h:107
DyLink * joint(int id) const
Definition: DyBody.h:124
DyLink * rootLink() const
Definition: DyBody.h:136
DyLink * link(const std::string &name) const
Definition: DyBody.h:132
DyLink * link(int index) const
Definition: DyBody.h:128
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
ref_ptr< DyBody > DyBodyPtr
Definition: DyBody.h:143
Link * joint(int id) const
Definition: Body.h:90
Eigen::Vector3d Vector3
Definition: EigenTypes.h:58
#define CNOID_EXPORT
Definition: Util/exportdecl.h:37
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:57