Body.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
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         // sensor access methods
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           The motion equation for calcMassMatrix()
00202           |       |   | dv   |   |    |   | fext      |
00203           | out_M | * | dw   | + | b1 | = | tauext    |
00204           |       |   |ddq   |   |    |   | u         |
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         // sensor = sensors[type][sensorId]
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         // Members for customizer
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


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:52