Choreonoid  1.1
Body.h
説明を見る。
1 
6 #ifndef CNOID_BODY_BODY_H_INCLUDED
7 #define CNOID_BODY_BODY_H_INCLUDED
8 
9 #include "LinkTraverse.h"
10 #include "LinkGroup.h"
11 #include <cnoid/YamlNodes>
12 #include <cnoid/Referenced>
13 #include <cnoid/EigenTypes>
14 #include <boost/shared_ptr.hpp>
15 #include <map>
16 #include "exportdecl.h"
17 
18 namespace cnoid {
19 
20  class Sensor;
21 
22  class Body;
23  typedef boost::intrusive_ptr<Body> BodyPtr;
24 
25  class JointPath;
26  typedef boost::shared_ptr<JointPath> JointPathPtr;
27 
29  typedef boost::shared_ptr<InverseKinematics> InverseKinematicsPtr;
30 
31  class LinkGroup;
32 
33  class YamlMapping;
34  typedef boost::intrusive_ptr<YamlMapping> YamlMappingPtr;
35 
38  };
39 
40  struct BodyInterface;
42  typedef void* BodyHandle;
43  typedef void* BodyCustomizerHandle;
44 
45  class CNOID_EXPORT Body : public Referenced
46  {
47  public:
48 
49  static void addCustomizerDirectory(const std::string& path);
50 
51  static BodyInterface* bodyInterface();
52 
53  Body();
54  virtual ~Body();
55 
56  virtual BodyPtr duplicate() const;
57 
58  inline const std::string& name() {
59  return name_;
60  }
61  inline void setName(const std::string& name) {
62  name_ = name;
63  }
64  inline const std::string& modelName() {
65  return modelName_;
66  }
67  inline void setModelName(const std::string& name) {
68  modelName_ = name;
69  }
70 
71  void setRootLink(Link* link);
72 
76  void updateLinkTree();
77 
86  inline int numJoints() const {
87  return jointIdToLinkArray.size();
88  }
89 
96  inline Link* joint(int id) const {
97  return jointIdToLinkArray[id];
98  }
99 
103  inline const std::vector<Link*>& joints() const {
104  return jointIdToLinkArray;
105  }
106 
111  inline int numLinks() const {
112  return linkTraverse_.numLinks();
113  }
114 
120  inline Link* link(int index) const {
121  return linkTraverse_.link(index);
122  }
123 
124  inline const LinkTraverse& links() const {
125  return linkTraverse_;
126  }
127 
131  inline const LinkTraverse& linkTraverse() const {
132  return linkTraverse_;
133  }
134 
138  Link* link(const std::string& name) const;
139 
143  inline Link* rootLink() const {
144  return rootLink_;
145  }
146 
147  // sensor access methods
148  Sensor* createSensor(Link* link, int sensorType, int id, const std::string& name);
149 
150  void addSensor(Sensor* sensor, int sensorType, int id );
151 
152  inline Sensor* sensor(int sensorType, int sensorId) const {
153  return allSensors[sensorType][sensorId];
154  }
155 
156  inline int numSensors(int sensorType) const {
157  return allSensors[sensorType].size();
158  }
159 
160  inline int numSensorTypes() const {
161  return allSensors.size();
162  }
163 
164  void clearSensorValues();
165 
166  template <class TSensor> inline TSensor* sensor(int id) const {
167  return static_cast<TSensor*>(allSensors[TSensor::TYPE][id]);
168  }
169 
170  template <class TSensor> inline TSensor* sensor(const std::string& name) const {
171  TSensor* sensor = 0;
172  NameToSensorMap::const_iterator p = nameToSensorMap.find(name);
173  if(p != nameToSensorMap.end()){
174  sensor = dynamic_cast<TSensor*>(p->second);
175  }
176  return sensor;
177  }
178 
182  inline bool isStaticModel() {
183  return isStaticModel_;
184  }
185 
186  double calcTotalMass();
187 
188  inline double totalMass() const {
189  return totalMass_;
190  }
191 
192  Vector3 calcCM();
193  const Vector3& lastCM() { return lastCM_; }
194 
195  void calcTotalMomentum(Vector3& out_P, Vector3& out_L);
196  void setDefaultRootPosition(const Vector3& p, const Matrix3& R);
197  void getDefaultRootPosition(Vector3& out_p, Matrix3& out_R);
198  void initializeConfiguration();
199  void calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false);
200  void clearExternalForces();
201  JointPathPtr getJointPath(Link* baseLink, Link* targetLink);
202  void setVirtualJointForces();
203 
204  virtual InverseKinematicsPtr getDefaultIK(Link* targetLink);
205 
211  void updateLinkColdetModelPositions();
212 
213  void putInformation(std::ostream &out);
214 
215  bool installCustomizer();
216  bool installCustomizer(BodyCustomizerInterface* customizerInterface);
217 
218  struct LinkConnection {
219  Link* link[2];
220  Vector3 point[2];
222  Vector3 constraintAxes[3];
223  };
224  typedef std::vector<LinkConnection> LinkConnectionArray;
225 
227 
228  YamlMapping* info() { return info_.get(); }
229  void resetInfo(YamlMappingPtr info);
230 
231  LinkGroup* linkGroup() { return linkGroup_.get(); }
232 
233  protected:
234 
235  Body(const Body& org);
236 
237  virtual void doResetInfo(const YamlMapping& info);
238 
239  private:
240 
241  bool isStaticModel_;
242  Link* rootLink_;
243 
244  std::string name_;
245  std::string modelName_;
246 
247  typedef std::vector<Link*> LinkArray;
248  LinkArray jointIdToLinkArray;
249 
250  LinkTraverse linkTraverse_;
251 
252  typedef std::map<std::string, Link*> NameToLinkMap;
253  NameToLinkMap nameToLinkMap;
254 
255  // sensor = sensors[type][sensorId]
256  typedef std::vector<Sensor*> SensorArray;
257  std::vector<SensorArray> allSensors;
258 
259  typedef std::map<std::string, Sensor*> NameToSensorMap;
260  NameToSensorMap nameToSensorMap;
261 
262  double totalMass_;
263  Vector3 lastCM_;
264 
265  Vector3 defaultRootPosition;
266  Matrix3 defaultRootAttitude;
267 
268  LinkGroupPtr linkGroup_;
269  YamlMappingPtr info_;
270 
271  // Members for customizer
272  BodyCustomizerHandle customizerHandle;
273  BodyCustomizerInterface* customizerInterface;
274  BodyHandleEntity bodyHandleEntity;
275  BodyHandle bodyHandle;
276 
277  void initialize();
278  Link* createEmptyJoint(int jointId);
279  void setVirtualJointForcesSub();
280 
281  friend class CustomizedJointPath;
282  };
283 
284 };
285 
286 
287 CNOID_EXPORT std::ostream &operator<< (std::ostream& out, cnoid::Body& body);
288 
289 #endif