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 
28  class InverseKinematics;
29  typedef boost::shared_ptr<InverseKinematics> InverseKinematicsPtr;
30 
31  class LinkGroup;
32 
33  class YamlMapping;
34  typedef boost::intrusive_ptr<YamlMapping> YamlMappingPtr;
35 
36  struct BodyHandleEntity {
37  Body* body;
38  };
39 
40  struct BodyInterface;
41  struct BodyCustomizerInterface;
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];
221  int numConstraintAxes;
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
cnoid::Body::rootLink
Link * rootLink() const
Definition: Body.h:143
LinkTraverse.h
The header file of the LinkTraverse class.
cnoid::Body::joints
const std::vector< Link * > & joints() const
Definition: Body.h:103
cnoid::BodyPtr
boost::intrusive_ptr< Body > BodyPtr
Definition: Body.h:22
cnoid::JointPathPtr
boost::shared_ptr< JointPath > JointPathPtr
Definition: Body.h:25
cnoid::InverseKinematicsPtr
boost::shared_ptr< InverseKinematics > InverseKinematicsPtr
Definition: Body.h:28
cnoid::LinkGroup
Definition: LinkGroup.h:23
cnoid::Body
Definition: Body.h:45
cnoid::Body::info
YamlMapping * info()
Definition: Body.h:228
cnoid::InverseKinematics
Definition: InverseKinematics.h:13
cnoid::Body::numSensorTypes
int numSensorTypes() const
Definition: Body.h:160
cnoid::Body::link
Link * link(int index) const
Definition: Body.h:120
cnoid::Body::linkTraverse
const LinkTraverse & linkTraverse() const
Definition: Body.h:131
cnoid::Body::linkGroup
LinkGroup * linkGroup()
Definition: Body.h:231
cnoid::Body::isStaticModel
bool isStaticModel()
Definition: Body.h:182
cnoid::Body::numJoints
int numJoints() const
Definition: Body.h:86
cnoid::Referenced
Definition: Referenced.h:21
cnoid::Body::links
const LinkTraverse & links() const
Definition: Body.h:124
cnoid::BodyHandleEntity::body
Body * body
Definition: Body.h:37
cnoid::Body::numLinks
int numLinks() const
Definition: Body.h:111
cnoid::LinkGroupPtr
boost::intrusive_ptr< LinkGroup > LinkGroupPtr
Definition: LinkGroup.h:20
cnoid::Sensor
Definition: Sensor.h:19
cnoid::Body::numSensors
int numSensors(int sensorType) const
Definition: Body.h:156
cnoid::Body::sensor
TSensor * sensor(int id) const
Definition: Body.h:166
cnoid::BodyCustomizerHandle
void * BodyCustomizerHandle
Definition: Body.h:43
cnoid::BodyHandleEntity
Definition: Body.h:36
cnoid::JointPath
Definition: JointPath.h:18
cnoid::Body::setName
void setName(const std::string &name)
Definition: Body.h:61
cnoid::Body::lastCM
const Vector3 & lastCM()
Definition: Body.h:193
cnoid::BodyCustomizerInterface
Definition: BodyCustomizerInterface.h:53
cnoid::Body::LinkConnection
Definition: Body.h:218
cnoid::Body::linkConnections
LinkConnectionArray linkConnections
Definition: Body.h:226
cnoid::Body::LinkConnection::numConstraintAxes
int numConstraintAxes
Definition: Body.h:221
cnoid::LinkTraverse
Definition: LinkTraverse.h:18
cnoid::YamlMapping::get
YamlNode & get(const std::string &key) const
Definition: YamlNodes.cpp:398
cnoid::Body::sensor
Sensor * sensor(int sensorType, int sensorId) const
Definition: Body.h:152
cnoid::Body::joint
Link * joint(int id) const
Definition: Body.h:96
cnoid::BodyHandle
void * BodyHandle
Definition: Body.h:41
cnoid::YamlMapping
Definition: YamlNodes.h:212
cnoid::BodyInterface
Definition: BodyCustomizerInterface.h:26
cnoid::YamlMappingPtr
boost::intrusive_ptr< YamlMapping > YamlMappingPtr
Definition: YamlNodes.h:380
cnoid::Vector3
Eigen::Vector3d Vector3
Definition: EigenTypes.h:26
CNOID_EXPORT
#define CNOID_EXPORT
Definition: Util/exportdecl.h:13
cnoid::Body::LinkConnectionArray
std::vector< LinkConnection > LinkConnectionArray
Definition: Body.h:224
cnoid::Body::name
const std::string & name()
Definition: Body.h:58
cnoid::Body::modelName
const std::string & modelName()
Definition: Body.h:64
cnoid::Body::totalMass
double totalMass() const
Definition: Body.h:188
exportdecl.h
cnoid::Body::setModelName
void setModelName(const std::string &name)
Definition: Body.h:67
cnoid::Body::sensor
TSensor * sensor(const std::string &name) const
Definition: Body.h:170
initialize
bool initialize()
operator<<
CNOID_EXPORT std::ostream & operator<<(std::ostream &out, cnoid::Body &body)
Definition: Body.cpp:579
cnoid::Matrix3
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:25
2014年02月04日(火) 01時54分17秒作成 - Choreonoid / 構成:   doxygen 1.8.6