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 
226  LinkConnectionArray linkConnections;
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
Link * rootLink() const
Definition: Body.h:143
The header file of the LinkTraverse class.
const std::vector< Link * > & joints() const
Definition: Body.h:103
boost::intrusive_ptr< Body > BodyPtr
Definition: Body.h:22
boost::shared_ptr< JointPath > JointPathPtr
Definition: Body.h:25
boost::shared_ptr< InverseKinematics > InverseKinematicsPtr
Definition: Body.h:28
Definition: LinkGroup.h:23
Definition: Body.h:45
YamlMapping * info()
Definition: Body.h:228
Definition: InverseKinematics.h:13
int numSensorTypes() const
Definition: Body.h:160
Link * link(int index) const
Definition: Body.h:120
const LinkTraverse & linkTraverse() const
Definition: Body.h:131
LinkGroup * linkGroup()
Definition: Body.h:231
bool isStaticModel()
Definition: Body.h:182
int numJoints() const
Definition: Body.h:86
Definition: Referenced.h:21
const LinkTraverse & links() const
Definition: Body.h:124
Body * body
Definition: Body.h:37
int numLinks() const
Definition: Body.h:111
boost::intrusive_ptr< LinkGroup > LinkGroupPtr
Definition: LinkGroup.h:20
Definition: Sensor.h:19
int numSensors(int sensorType) const
Definition: Body.h:156
TSensor * sensor(int id) const
Definition: Body.h:166
void * BodyCustomizerHandle
Definition: Body.h:43
Definition: Body.h:36
Definition: JointPath.h:18
void setName(const std::string &name)
Definition: Body.h:61
const Vector3 & lastCM()
Definition: Body.h:193
Definition: BodyCustomizerInterface.h:53
Definition: Body.h:218
LinkConnectionArray linkConnections
Definition: Body.h:226
int numConstraintAxes
Definition: Body.h:221
Definition: EasyScanner.h:16
Definition: LinkTraverse.h:18
YamlNode & get(const std::string &key) const
Definition: YamlNodes.cpp:398
Sensor * sensor(int sensorType, int sensorId) const
Definition: Body.h:152
Link * joint(int id) const
Definition: Body.h:96
void * BodyHandle
Definition: Body.h:41
Definition: YamlNodes.h:212
Definition: BodyCustomizerInterface.h:26
boost::intrusive_ptr< YamlMapping > YamlMappingPtr
Definition: YamlNodes.h:380
Eigen::Vector3d Vector3
Definition: EigenTypes.h:26
#define CNOID_EXPORT
Definition: Util/exportdecl.h:13
std::vector< LinkConnection > LinkConnectionArray
Definition: Body.h:224
const std::string & name()
Definition: Body.h:58
const std::string & modelName()
Definition: Body.h:64
double totalMass() const
Definition: Body.h:188
void setModelName(const std::string &name)
Definition: Body.h:67
TSensor * sensor(const std::string &name) const
Definition: Body.h:170
bool initialize()
CNOID_EXPORT std::ostream & operator<<(std::ostream &out, cnoid::Body &body)
Definition: Body.cpp:579
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:25