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 */ 00014 #ifndef HRPMODEL_LINK_H_INCLUDED 00015 #define HRPMODEL_LINK_H_INCLUDED 00016 00017 #include <string> 00018 #include <ostream> 00019 #include <vector> 00020 #include <boost/intrusive_ptr.hpp> 00021 #include <hrpUtil/Eigen3d.h> 00022 #include <hrpCollision/ColdetModel.h> 00023 #include "Config.h" 00024 00025 namespace hrp { 00026 class Link; 00027 } 00028 00029 HRPMODEL_API std::ostream& operator<<(std::ostream &out, hrp::Link& link); 00030 00031 namespace hrp { 00032 00033 class Body; 00034 class Sensor; 00035 class Light; 00036 00037 class ColdetModel; 00038 typedef boost::intrusive_ptr<ColdetModel> ColdetModelPtr; 00039 00040 class HRPMODEL_API Link { 00041 00042 public: 00043 00044 Link(); 00045 Link(const Link& link); 00046 virtual ~Link(); 00047 00048 bool isValid() { return (index >= 0); } 00049 void addChild(Link* link); 00050 bool detachChild(Link* link); 00051 bool isRoot() { return !parent; } 00052 00053 void setAttitude(const Matrix33& R) { this->R = R * Rs.transpose(); } 00054 Matrix33 attitude() { return Matrix33(this->R * Rs); } 00055 Matrix33 calcRfromAttitude(const Matrix33& R) { return Matrix33(R * Rs.transpose()); } 00056 00061 void calcSubMassCM(); 00062 00067 void calcSubMassInertia(Matrix33& subIw); 00068 00072 void setSegmentAttitude(const Matrix33& R) { this->R = R * Rs.transpose(); } 00073 00077 Matrix33 segmentAttitude() { return Matrix33(this->R * Rs); } 00078 00079 void updateColdetModelPosition() { 00080 coldetModel->setPosition(R, p); 00081 } 00082 00083 Body* body; 00084 00085 int index; 00086 int jointId; 00087 00088 std::string name; 00089 00090 enum JointType { 00091 FIXED_JOINT, 00092 FREE_JOINT, 00093 ROTATIONAL_JOINT, 00094 SLIDE_JOINT 00095 }; 00096 00097 JointType jointType; 00098 00099 Link* parent; 00100 Link* sibling; 00101 Link* child; 00102 00103 Vector3 p; 00104 00113 Matrix33 R; 00114 00115 Vector3 v; 00116 Vector3 w; 00117 Vector3 dv; 00118 Vector3 dw; 00119 00120 double q; 00121 double dq; 00122 double ddq; 00123 double u; 00124 00125 Vector3 a; 00126 Vector3 d; 00127 Vector3 b; 00128 00129 Matrix33 Rs; 00130 00131 double m; 00132 Matrix33 I; 00133 Vector3 c; 00134 Vector3 wc; 00135 00136 Vector3 vo; 00137 Vector3 dvo; 00138 00141 Vector3 sw; 00142 00145 Vector3 sv; 00146 00147 Vector3 cv; 00148 Vector3 cw; 00149 00150 Vector3 fext; 00151 Vector3 tauext; 00152 00153 // needed ? 00154 //Vector3 f; ///< force from the parent link 00155 //Vector3 tau; ///< torque from the parent link 00156 00157 Matrix33 Iww; 00158 Matrix33 Iwv; 00159 Matrix33 Ivv; 00160 Vector3 pf; 00161 Vector3 ptau; 00162 Vector3 hhv; 00163 Vector3 hhw; 00164 double uu; 00165 double dd; 00166 00167 double Jm2; 00168 00169 double ulimit; 00170 double llimit; 00171 double uvlimit; 00172 double lvlimit; 00173 double climit; 00174 00175 double defaultJointValue; 00176 double torqueConst; 00177 double encoderPulse; 00178 double Ir; 00179 double gearRatio; 00180 double gearEfficiency; 00181 double rotorResistor; 00182 00183 bool isHighGainMode; 00184 bool isCrawler; 00185 00186 ColdetModelPtr coldetModel; 00187 00188 struct ConstraintForce { 00189 Vector3 point; 00190 Vector3 force; 00191 }; 00192 00193 typedef std::vector<ConstraintForce> ConstraintForceArray; 00194 ConstraintForceArray constraintForces; 00195 00196 double subm; 00197 Vector3 submwc; 00198 std::vector<Sensor *> sensors; 00199 std::vector<Light *> lights; 00200 private: 00201 00202 Link& operator=(const Link& link); // no implementation is given to disable the copy operator 00203 void setBodyIter(Body* body); 00204 friend std::ostream& ::operator<<(std::ostream &out, hrp::Link& link); 00205 void putInformation(std::ostream& out); // for the iostream output 00206 }; 00207 00208 }; 00209 00210 00211 #endif