Body.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  */
13 #ifndef HRPMODEL_BODY_H_INCLUDED
14 #define HRPMODEL_BODY_H_INCLUDED
15 
16 #include <map>
17 #include <vector>
18 #include <ostream>
19 #include <boost/shared_ptr.hpp>
20 #include <hrpUtil/Referenced.h>
21 #include <hrpUtil/Eigen3d.h>
22 #include "LinkTraverse.h"
23 #include "Config.h"
24 
25 namespace hrp {
26  class Sensor;
27  class Light;
28  class Body;
29  class JointPath;
30  typedef boost::shared_ptr<JointPath> JointPathPtr;
31 }
32 
33 namespace hrp {
34 
37  };
38 
39  struct BodyInterface;
41  typedef void* BodyHandle;
42  typedef void* BodyCustomizerHandle;
43 
44  class HRPMODEL_API Body : public Referenced
45  {
46 
47  public:
48 
49  static BodyInterface* bodyInterface();
50 
51  Body();
52  Body(const Body& org);
53 
54  virtual ~Body();
55 
56  const std::string& name() {
57  return name_;
58  }
59 
60  void setName(const std::string& name) {
61  name_ = name;
62  }
63 
64  const std::string& modelName() {
65  return modelName_;
66  }
67 
68  void setModelName(const std::string& name) {
69  modelName_ = name;
70  }
71 
72  void setRootLink(Link* link);
73 
77  void updateLinkTree();
78 
87  inline unsigned int numJoints() const {
88  return jointIdToLinkArray.size();
89  }
90 
97  inline Link* joint(int id) const {
98  return jointIdToLinkArray[id];
99  }
100 
104  inline const std::vector<Link*>& joints() const {
105  return jointIdToLinkArray;
106  }
107 
112  inline unsigned int numLinks() const {
113  return linkTraverse_.numLinks();
114  }
115 
121  inline Link* link(int index) const {
122  return linkTraverse_.link(index);
123  }
124 
125  inline const LinkTraverse& links() const {
126  return linkTraverse_;
127  }
128 
132  inline const LinkTraverse& linkTraverse() const {
133  return linkTraverse_;
134  }
135 
139  Link* link(const std::string& name) const;
140 
144  inline Link* rootLink() const {
145  return rootLink_;
146  }
147 
148  Light *createLight(Link *link, int lightType, const std::string& name);
149  inline Light* light(const std::string& name) {
150  return nameToLightMap[name];
151  }
152 
153  // sensor access methods
154  Sensor* createSensor(Link* link, int sensorType, int id, const std::string& name);
155 
156  void addSensor(Sensor* sensor, int sensorType, int id );
157 
158  inline Sensor* sensor(int sensorType, int sensorId) const {
159  return allSensors[sensorType][sensorId];
160  }
161 
162  inline unsigned int numSensors(int sensorType) const {
163  return allSensors[sensorType].size();
164  }
165 
166  inline unsigned int numSensorTypes() const {
167  return allSensors.size();
168  }
169 
170  void clearSensorValues();
171 
172  template <class TSensor> inline TSensor* sensor(int id) const {
173  return static_cast<TSensor*>(allSensors[TSensor::TYPE][id]);
174  }
175 
176  template <class TSensor> inline TSensor* sensor(const std::string& name) const {
177  TSensor* sensor = 0;
178  NameToSensorMap::const_iterator p = nameToSensorMap.find(name);
179  if(p != nameToSensorMap.end()){
180  sensor = dynamic_cast<TSensor*>(p->second);
181  }
182  return sensor;
183  }
184 
188  inline bool isStaticModel() {
189  return isStaticModel_;
190  }
191 
192  double calcTotalMass();
193 
194  inline double totalMass() const {
195  return totalMass_;
196  }
197 
198  Vector3 calcCM();
199 
200  /*
201  The motion equation for calcMassMatrix()
202  | | | dv | | | | fext |
203  | out_M | * | dw | + | b1 | = | tauext |
204  | | |ddq | | | | u |
205  */
206  void calcMassMatrix(dmatrix& out_M);
207 
208  void setColumnOfMassMatrix(dmatrix& M, int column);
209 
210  void calcInverseDynamics(Link* link, Vector3& out_f, Vector3& out_tau);
211 
212  void calcTotalMomentum(Vector3& out_P, Vector3& out_L);
213 
214  void calcTotalMomentumFromJacobian(Vector3& out_P, Vector3& out_L);
215 
216  void setDefaultRootPosition(const Vector3& p, const Matrix33& R);
217 
218  void getDefaultRootPosition(Vector3& out_p, Matrix33& out_R);
219 
220  void initializeConfiguration();
221 
222  void calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false);
223 
224  void clearExternalForces();
225 
226  JointPathPtr getJointPath(Link* baseLink, Link* targetLink);
227 
228  inline void setVirtualJointForces(){
229  if(customizerInterface){
230  setVirtualJointForcesSub();
231  }
232  }
233 
239  void updateLinkColdetModelPositions();
240 
241  void putInformation(std::ostream &out);
242 
243  bool installCustomizer();
244  bool installCustomizer(BodyCustomizerInterface* customizerInterface);
245 
246  enum ExtraJointType { EJ_XYZ, EJ_XY, EJ_Z };
247 
248  struct ExtraJoint {
249  std::string name;
252  Link* link[2];
253  Vector3 point[2];
254  };
255 
256  std::vector<ExtraJoint> extraJoints;
257 
264  void calcCMJacobian(Link *base, dmatrix &J);
271  void calcAngularMomentumJacobian(Link *base, dmatrix &H);
272  private:
273 
276 
277  std::string name_;
278  std::string modelName_;
279 
280  typedef std::vector<Link*> LinkArray;
282 
284 
285  typedef std::map<std::string, Link*> NameToLinkMap;
286  NameToLinkMap nameToLinkMap;
287 
288  // sensor = sensors[type][sensorId]
289  typedef std::vector<Sensor*> SensorArray;
290  std::vector<SensorArray> allSensors;
291 
292  typedef std::map<std::string, Sensor*> NameToSensorMap;
293  NameToSensorMap nameToSensorMap;
294  typedef std::map<std::string, Light*> NameToLightMap;
295  NameToLightMap nameToLightMap;
296 
297  double totalMass_;
298 
301 
302  // Members for customizer
303  BodyCustomizerHandle customizerHandle;
306  BodyHandle bodyHandle;
307 
308  void initialize();
309  Link* createEmptyJoint(int jointId);
310  void setVirtualJointForcesSub();
311 
312  friend class CustomizedJointPath;
313  };
314 
315  typedef boost::shared_ptr<Body> BodyPtr;
316 
317 };
318 
319 
320 HRPMODEL_API std::ostream &operator<< (std::ostream& out, hrp::Body& body);
321 
322 
323 #endif
The header file of the LinkTraverse class.
std::vector< SensorArray > allSensors
Definition: Body.h:290
std::map< std::string, Light * > NameToLightMap
Definition: Body.h:294
Eigen::MatrixXd dmatrix
Definition: EigenTypes.h:13
TSensor * sensor(const std::string &name) const
Definition: Body.h:176
bool isStaticModel()
Definition: Body.h:188
ExtraJointType type
Definition: Body.h:250
void setName(const std::string &name)
Definition: Body.h:60
void setVirtualJointForces()
Definition: Body.h:228
NameToLinkMap nameToLinkMap
Definition: Body.h:286
png_infop png_charpp name
Definition: png.h:2382
Link * rootLink() const
Definition: Body.h:144
Definition: inflate.h:32
std::map< std::string, Sensor * > NameToSensorMap
Definition: Body.h:292
void * BodyHandle
Definition: Body.h:40
const LinkTraverse & links() const
Definition: Body.h:125
unsigned int numSensors(int sensorType) const
Definition: Body.h:162
static BodyInterface * bodyInterface
TSensor * sensor(int id) const
Definition: Body.h:172
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
double totalMass() const
Definition: Body.h:194
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
Vector3 defaultRootPosition
Definition: Body.h:299
std::vector< Sensor * > SensorArray
Definition: Body.h:289
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
const std::string & modelName()
Definition: Body.h:64
std::vector< Link * > LinkArray
Definition: Body.h:280
std::string name_
Definition: Body.h:277
double totalMass_
Definition: Body.h:297
const LinkTraverse & linkTraverse() const
Definition: Body.h:132
NameToLightMap nameToLightMap
Definition: Body.h:295
BodyCustomizerInterface * customizerInterface
Definition: Body.h:304
void * BodyCustomizerHandle
Definition: Body.h:42
const std::string & name()
Definition: Body.h:56
NameToSensorMap nameToSensorMap
Definition: Body.h:293
std::string modelName_
Definition: Body.h:278
std::vector< ExtraJoint > extraJoints
Definition: Body.h:256
unsigned int numSensorTypes() const
Definition: Body.h:166
Link * joint(int id) const
Definition: Body.h:97
Light * light(const std::string &name)
Definition: Body.h:149
Link * link(int index) const
Definition: Body.h:121
void setModelName(const std::string &name)
Definition: Body.h:68
unsigned int numJoints() const
Definition: Body.h:87
std::map< std::string, Link * > NameToLinkMap
Definition: Body.h:285
Matrix33 defaultRootAttitude
Definition: Body.h:300
const std::vector< Link * > & joints() const
Definition: Body.h:104
Definition: Body.h:44
BodyHandle bodyHandle
Definition: Body.h:306
ExtraJointType
Definition: Body.h:246
boost::shared_ptr< JointPath > JointPathPtr
Definition: Body.h:29
org
HRPMODEL_API std::ostream & operator<<(std::ostream &out, hrp::Body &body)
Definition: Body.cpp:721
BodyHandleEntity bodyHandleEntity
Definition: Body.h:305
unsigned int numLinks() const
Definition: Body.h:112
bool isStaticModel_
Definition: Body.h:274
std::string name
Definition: Body.h:249
Link * rootLink_
Definition: Body.h:275
Sensor * sensor(int sensorType, int sensorId) const
Definition: Body.h:158
BodyCustomizerHandle customizerHandle
Definition: Body.h:303
LinkTraverse linkTraverse_
Definition: Body.h:283
LinkArray jointIdToLinkArray
Definition: Body.h:281


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:19