Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00013 #ifndef HRPMODEL_BODY_H_INCLUDED
00014 #define HRPMODEL_BODY_H_INCLUDED
00015
00016 #include <map>
00017 #include <vector>
00018 #include <ostream>
00019 #include <boost/shared_ptr.hpp>
00020 #include <hrpUtil/Referenced.h>
00021 #include <hrpUtil/Eigen3d.h>
00022 #include "LinkTraverse.h"
00023 #include "Config.h"
00024
00025 namespace hrp {
00026 class Sensor;
00027 class Light;
00028 class Body;
00029 class JointPath;
00030 typedef boost::shared_ptr<JointPath> JointPathPtr;
00031 }
00032
00033 namespace hrp {
00034
00035 struct BodyHandleEntity {
00036 Body* body;
00037 };
00038
00039 struct BodyInterface;
00040 struct BodyCustomizerInterface;
00041 typedef void* BodyHandle;
00042 typedef void* BodyCustomizerHandle;
00043
00044 class HRPMODEL_API Body : public Referenced
00045 {
00046
00047 public:
00048
00049 static BodyInterface* bodyInterface();
00050
00051 Body();
00052 Body(const Body& org);
00053
00054 virtual ~Body();
00055
00056 const std::string& name() {
00057 return name_;
00058 }
00059
00060 void setName(const std::string& name) {
00061 name_ = name;
00062 }
00063
00064 const std::string& modelName() {
00065 return modelName_;
00066 }
00067
00068 void setModelName(const std::string& name) {
00069 modelName_ = name;
00070 }
00071
00072 void setRootLink(Link* link);
00073
00077 void updateLinkTree();
00078
00087 inline unsigned int numJoints() const {
00088 return jointIdToLinkArray.size();
00089 }
00090
00097 inline Link* joint(int id) const {
00098 return jointIdToLinkArray[id];
00099 }
00100
00104 inline const std::vector<Link*>& joints() const {
00105 return jointIdToLinkArray;
00106 }
00107
00112 inline unsigned int numLinks() const {
00113 return linkTraverse_.numLinks();
00114 }
00115
00121 inline Link* link(int index) const {
00122 return linkTraverse_.link(index);
00123 }
00124
00125 inline const LinkTraverse& links() const {
00126 return linkTraverse_;
00127 }
00128
00132 inline const LinkTraverse& linkTraverse() const {
00133 return linkTraverse_;
00134 }
00135
00139 Link* link(const std::string& name) const;
00140
00144 inline Link* rootLink() const {
00145 return rootLink_;
00146 }
00147
00148 Light *createLight(Link *link, int lightType, const std::string& name);
00149 inline Light* light(const std::string& name) {
00150 return nameToLightMap[name];
00151 }
00152
00153
00154 Sensor* createSensor(Link* link, int sensorType, int id, const std::string& name);
00155
00156 void addSensor(Sensor* sensor, int sensorType, int id );
00157
00158 inline Sensor* sensor(int sensorType, int sensorId) const {
00159 return allSensors[sensorType][sensorId];
00160 }
00161
00162 inline unsigned int numSensors(int sensorType) const {
00163 return allSensors[sensorType].size();
00164 }
00165
00166 inline unsigned int numSensorTypes() const {
00167 return allSensors.size();
00168 }
00169
00170 void clearSensorValues();
00171
00172 template <class TSensor> inline TSensor* sensor(int id) const {
00173 return static_cast<TSensor*>(allSensors[TSensor::TYPE][id]);
00174 }
00175
00176 template <class TSensor> inline TSensor* sensor(const std::string& name) const {
00177 TSensor* sensor = 0;
00178 NameToSensorMap::const_iterator p = nameToSensorMap.find(name);
00179 if(p != nameToSensorMap.end()){
00180 sensor = dynamic_cast<TSensor*>(p->second);
00181 }
00182 return sensor;
00183 }
00184
00188 inline bool isStaticModel() {
00189 return isStaticModel_;
00190 }
00191
00192 double calcTotalMass();
00193
00194 inline double totalMass() const {
00195 return totalMass_;
00196 }
00197
00198 Vector3 calcCM();
00199
00200
00201
00202
00203
00204
00205
00206 void calcMassMatrix(dmatrix& out_M);
00207
00208 void setColumnOfMassMatrix(dmatrix& M, int column);
00209
00210 void calcInverseDynamics(Link* link, Vector3& out_f, Vector3& out_tau);
00211
00212 void calcTotalMomentum(Vector3& out_P, Vector3& out_L);
00213
00214 void calcTotalMomentumFromJacobian(Vector3& out_P, Vector3& out_L);
00215
00216 void setDefaultRootPosition(const Vector3& p, const Matrix33& R);
00217
00218 void getDefaultRootPosition(Vector3& out_p, Matrix33& out_R);
00219
00220 void initializeConfiguration();
00221
00222 void calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false);
00223
00224 void clearExternalForces();
00225
00226 JointPathPtr getJointPath(Link* baseLink, Link* targetLink);
00227
00228 inline void setVirtualJointForces(){
00229 if(customizerInterface){
00230 setVirtualJointForcesSub();
00231 }
00232 }
00233
00239 void updateLinkColdetModelPositions();
00240
00241 void putInformation(std::ostream &out);
00242
00243 bool installCustomizer();
00244 bool installCustomizer(BodyCustomizerInterface* customizerInterface);
00245
00246 enum ExtraJointType { EJ_XYZ, EJ_XY, EJ_Z };
00247
00248 struct ExtraJoint {
00249 std::string name;
00250 ExtraJointType type;
00251 Vector3 axis;
00252 Link* link[2];
00253 Vector3 point[2];
00254 };
00255
00256 std::vector<ExtraJoint> extraJoints;
00257
00264 void calcCMJacobian(Link *base, dmatrix &J);
00271 void calcAngularMomentumJacobian(Link *base, dmatrix &H);
00272 private:
00273
00274 bool isStaticModel_;
00275 Link* rootLink_;
00276
00277 std::string name_;
00278 std::string modelName_;
00279
00280 typedef std::vector<Link*> LinkArray;
00281 LinkArray jointIdToLinkArray;
00282
00283 LinkTraverse linkTraverse_;
00284
00285 typedef std::map<std::string, Link*> NameToLinkMap;
00286 NameToLinkMap nameToLinkMap;
00287
00288
00289 typedef std::vector<Sensor*> SensorArray;
00290 std::vector<SensorArray> allSensors;
00291
00292 typedef std::map<std::string, Sensor*> NameToSensorMap;
00293 NameToSensorMap nameToSensorMap;
00294 typedef std::map<std::string, Light*> NameToLightMap;
00295 NameToLightMap nameToLightMap;
00296
00297 double totalMass_;
00298
00299 Vector3 defaultRootPosition;
00300 Matrix33 defaultRootAttitude;
00301
00302
00303 BodyCustomizerHandle customizerHandle;
00304 BodyCustomizerInterface* customizerInterface;
00305 BodyHandleEntity bodyHandleEntity;
00306 BodyHandle bodyHandle;
00307
00308 void initialize();
00309 Link* createEmptyJoint(int jointId);
00310 void setVirtualJointForcesSub();
00311
00312 friend class CustomizedJointPath;
00313 };
00314
00315 typedef boost::shared_ptr<Body> BodyPtr;
00316
00317 };
00318
00319
00320 HRPMODEL_API std::ostream &operator<< (std::ostream& out, hrp::Body& body);
00321
00322
00323 #endif