6 #ifndef CNOID_BODY_BODY_H_INCLUDED
7 #define CNOID_BODY_BODY_H_INCLUDED
11 #include <cnoid/YamlNodes>
12 #include <cnoid/Referenced>
13 #include <cnoid/EigenTypes>
14 #include <boost/shared_ptr.hpp>
23 typedef boost::intrusive_ptr<Body>
BodyPtr;
49 static void addCustomizerDirectory(
const std::string& path);
56 virtual BodyPtr duplicate()
const;
58 inline const std::string&
name() {
61 inline void setName(
const std::string& name) {
71 void setRootLink(
Link* link);
76 void updateLinkTree();
87 return jointIdToLinkArray.size();
97 return jointIdToLinkArray[id];
103 inline const std::vector<Link*>&
joints()
const {
104 return jointIdToLinkArray;
112 return linkTraverse_.numLinks();
121 return linkTraverse_.link(index);
125 return linkTraverse_;
132 return linkTraverse_;
138 Link* link(
const std::string& name)
const;
148 Sensor* createSensor(
Link* link,
int sensorType,
int id,
const std::string& name);
150 void addSensor(
Sensor* sensor,
int sensorType,
int id );
153 return allSensors[sensorType][sensorId];
157 return allSensors[sensorType].size();
161 return allSensors.size();
164 void clearSensorValues();
166 template <
class TSensor>
inline TSensor*
sensor(
int id)
const {
167 return static_cast<TSensor*
>(allSensors[TSensor::TYPE][id]);
170 template <
class TSensor>
inline TSensor*
sensor(
const std::string& name)
const {
172 NameToSensorMap::const_iterator p = nameToSensorMap.find(name);
173 if(p != nameToSensorMap.end()){
174 sensor =
dynamic_cast<TSensor*
>(p->second);
183 return isStaticModel_;
186 double calcTotalMass();
198 void initializeConfiguration();
199 void calcForwardKinematics(
bool calcVelocity =
false,
bool calcAcceleration =
false);
200 void clearExternalForces();
202 void setVirtualJointForces();
211 void updateLinkColdetModelPositions();
213 void putInformation(std::ostream &out);
215 bool installCustomizer();
245 std::string modelName_;
247 typedef std::vector<Link*> LinkArray;
248 LinkArray jointIdToLinkArray;
252 typedef std::map<std::string, Link*> NameToLinkMap;
253 NameToLinkMap nameToLinkMap;
256 typedef std::vector<Sensor*> SensorArray;
257 std::vector<SensorArray> allSensors;
259 typedef std::map<std::string, Sensor*> NameToSensorMap;
260 NameToSensorMap nameToSensorMap;
278 Link* createEmptyJoint(
int jointId);
279 void setVirtualJointForcesSub();
281 friend class CustomizedJointPath;