Choreonoid  1.5
Body.h
Go to the documentation of this file.
1 
6 #ifndef CNOID_BODY_BODY_H
7 #define CNOID_BODY_BODY_H
8 
9 #include "LinkTraverse.h"
10 #include "Link.h"
11 #include "DeviceList.h"
12 #include "exportdecl.h"
13 
14 namespace cnoid {
15 
16 class Body;
17 class BodyImpl;
18 class Mapping;
19 class SgCloneMap;
20 
21 struct BodyInterface;
23 typedef void* BodyCustomizerHandle;
24 
26 
27 
29 {
30 public:
31  Body();
32  Body(const Body& org);
33 
34  virtual Body* clone() const;
35 
36  virtual Link* createLink(const Link* org = 0) const;
37 
38  virtual ~Body();
39 
40  const std::string& name() const;
41  void setName(const std::string& name);
42  const std::string& modelName() const;
43  void setModelName(const std::string& name);
44 
45  void setRootLink(Link* link);
46 
50  void updateLinkTree();
51 
52  void initializeState();
53 
63  int numJoints() const {
64  return numActualJoints;
65  }
66 
72  int numVirtualJoints() const {
73  return jointIdToLinkArray.size() - numActualJoints;
74  }
75 
79  int numAllJoints() const {
80  return jointIdToLinkArray.size();
81  }
82 
90  Link* joint(int id) const {
91  return jointIdToLinkArray[id];
92  }
93 
98  int numLinks() const {
99  return linkTraverse_.numLinks();
100  }
101 
107  Link* link(int index) const {
108  return linkTraverse_.link(index);
109  }
110 
114  const LinkTraverse& linkTraverse() const {
115  return linkTraverse_;
116  }
117 
122  Link* link(const std::string& name) const;
123 
127  Link* rootLink() const {
128  return rootLink_;
129  }
130 
131  int numDevices() const {
132  return devices_.size();
133  }
134 
135  Device* device(int index) const { return devices_[index]; }
136 
143  const DeviceList<>& devices() const {
144  return devices_;
145  }
146 
147  template<class DeviceType> DeviceList<DeviceType> devices() const {
148  return devices_;
149  }
150 
151  template<class DeviceType> DeviceType* findDevice(const std::string& name) const {
152  return dynamic_cast<DeviceType*>(findDeviceSub(name));
153  }
154 
155  Device* findDevice(const std::string& name) const {
156  return findDeviceSub(name);
157  }
158 
159  void addDevice(Device* device);
160  void initializeDeviceStates();
161  void clearDevices();
162 
166  bool isStaticModel() const {
167  return isStaticModel_;
168  }
169  bool isFixedRootModel() const {
170  return rootLink_->isFixedJoint();
171  }
172 
173  void resetDefaultPosition(const Position& T);
174  const Position& defaultPosition() const { return rootLink_->Tb(); }
175 
176  double mass() const;
177 
178  const Vector3& calcCenterOfMass();
179  const Vector3& centerOfMass() const;
180 
181  void calcTotalMomentum(Vector3& out_P, Vector3& out_L);
182 
183  void calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false) {
184  linkTraverse_.calcForwardKinematics(calcVelocity, calcAcceleration);
185  }
186 
187  void clearExternalForces();
188 
189  enum ExtraJointType { EJ_PISTON, EJ_BALL };
190 
191  struct ExtraJoint {
194  Link* link[2];
195  Vector3 point[2];
196  };
197 
198  int numExtraJoints() const { return extraJoints_.size(); }
199  ExtraJoint& extraJoint(int index) { return extraJoints_[index]; }
200  const ExtraJoint& extraJoint(int index) const { return extraJoints_[index]; }
201  void addExtraJoint(const ExtraJoint& extraJoint) { extraJoints_.push_back(extraJoint); }
202  void clearExtraJoints() { extraJoints_.clear(); }
203 
204  const Mapping* info() const;
205  Mapping* info();
206  void resetInfo(Mapping* info);
207 
208  void cloneShapes(SgCloneMap& cloneMap);
209 
210  template<class T> T* findCache(const std::string& name) {
211  return dynamic_cast<T*>(findCacheSub(name));
212  }
213 
214  template<class T> const T* findCache(const std::string& name) const {
215  return dynamic_cast<const T*>(findCacheSub(name));
216  }
217 
218  template<class T> T* getOrCreateCache(const std::string& name) {
219  T* cache = findCache<T>(name);
220  if(!cache){
221  cache = new T();
222  insertCache(name, cache);
223  }
224  return cache;
225  }
226 
227  bool getCaches(PolymorphicReferencedArrayBase<>& out_caches, std::vector<std::string>& out_names) const;
228 
229  void removeCache(const std::string& name);
230 
231  BodyCustomizerHandle customizerHandle() const;
232  BodyCustomizerInterface* customizerInterface() const;
233 
234  bool installCustomizer();
235  bool installCustomizer(BodyCustomizerInterface* customizerInterface);
236 
237  bool hasVirtualJointForces() const;
238  void setVirtualJointForces();
239 
240  static void addCustomizerDirectory(const std::string& path);
241  static BodyInterface* bodyInterface();
242 
243 protected:
244  void copy(const Body& org);
245 
246 private:
247  LinkTraverse linkTraverse_;
248  LinkPtr rootLink_;
249  bool isStaticModel_;
250  std::vector<LinkPtr> jointIdToLinkArray;
251  int numActualJoints;
252  DeviceList<> devices_;
253  std::vector<ExtraJoint> extraJoints_;
254  BodyImpl* impl;
255 
256  void initialize();
257  Link* cloneLinkTree(const Link* orgLink);
258  Link* createEmptyJoint(int jointId);
259  Device* findDeviceSub(const std::string& name) const;
260  Referenced* findCacheSub(const std::string& name);
261  const Referenced* findCacheSub(const std::string& name) const;
262  void insertCache(const std::string& name, Referenced* cache);
263  void setVirtualJointForcesSub();
264 };
265 
266 }
267 
268 #endif
bool isStaticModel() const
Definition: Body.h:166
Link * rootLink() const
Definition: Body.h:127
int numExtraJoints() const
Definition: Body.h:198
The header file of the LinkTraverse class.
Device * findDevice(const std::string &name) const
Definition: Body.h:155
ExtraJoint & extraJoint(int index)
Definition: Body.h:199
Definition: Body.h:28
DeviceList< DeviceType > devices() const
Definition: Body.h:147
ExtraJointType
Definition: Body.h:189
bool isFixedRootModel() const
Definition: Body.h:169
Definition: ValueTree.h:224
void calcForwardKinematics(bool calcVelocity=false, bool calcAcceleration=false)
Definition: Body.h:183
Link * link(int index) const
Definition: Body.h:107
const LinkTraverse & linkTraverse() const
Definition: Body.h:114
int numJoints() const
Definition: Body.h:63
Definition: Referenced.h:67
T * getOrCreateCache(const std::string &name)
Definition: Body.h:218
void addExtraJoint(const ExtraJoint &extraJoint)
Definition: Body.h:201
int numDevices() const
Definition: Body.h:131
int numLinks() const
Definition: Body.h:98
const Position & defaultPosition() const
Definition: Body.h:174
void * BodyCustomizerHandle
Definition: Body.h:22
Definition: SceneGraph.h:56
DeviceType * findDevice(const std::string &name) const
Definition: Body.h:151
const ExtraJoint & extraJoint(int index) const
Definition: Body.h:200
Definition: BodyCustomizerInterface.h:53
Eigen::Transform< double, 3, Eigen::AffineCompact > Position
Definition: EigenTypes.h:73
void clearExtraJoints()
Definition: Body.h:202
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
Definition: LinkTraverse.h:17
Definition: PolymorphicReferencedArray.h:14
ref_ptr< Body > BodyPtr
Definition: Body.h:25
ExtraJointType type
Definition: Body.h:192
Link * joint(int id) const
Definition: Body.h:90
const DeviceList & devices() const
Definition: Body.h:143
Definition: BodyCustomizerInterface.h:26
const T * findCache(const std::string &name) const
Definition: Body.h:214
Definition: Body.h:191
Eigen::Vector3d Vector3
Definition: EigenTypes.h:58
#define CNOID_EXPORT
Definition: Util/exportdecl.h:37
T * findCache(const std::string &name)
Definition: Body.h:210
int numVirtualJoints() const
Definition: Body.h:72
Definition: DeviceList.h:16
Definition: Device.h:52
Device * device(int index) const
Definition: Body.h:135
int numAllJoints() const
Definition: Body.h:79
Vector3 axis
Definition: Body.h:193