6 #ifndef CNOID_BODY_LINK_H 7 #define CNOID_BODY_LINK_H 9 #include <cnoid/Referenced> 10 #include <cnoid/EigenTypes> 13 #include <cnoid/SceneGraph> 14 #include <cnoid/ValueTree> 46 int index()
const {
return index_; }
47 bool isValid()
const {
return (index_ >= 0); }
53 bool isRoot()
const {
return !parent_; }
64 template<
class Scalar,
int Mode,
int Options>
65 void setPosition(
const Eigen::Transform<Scalar, 3, Mode, Options>& T) {
66 T_ = T.template cast<Position::Scalar>();
69 template<
typename Derived1,
typename Derived2>
70 void setPosition(
const Eigen::MatrixBase<Derived1>& rotation,
const Eigen::MatrixBase<Derived2>& translation) {
71 T_.linear() = rotation;
72 T_.translation() = translation;
75 Position::TranslationPart
p() {
return T_.translation(); }
76 Position::ConstTranslationPart
p()
const {
return T_.translation(); }
77 Position::TranslationPart
translation() {
return T_.translation(); }
78 Position::ConstTranslationPart
translation()
const {
return T_.translation(); }
80 template<
typename Derived>
82 T_.translation() = p.template cast<Affine3::Scalar>();
85 Position::LinearPart
R() {
return T_.linear(); }
86 Position::ConstLinearPart
R()
const {
return T_.linear(); }
87 Position::LinearPart
rotation() {
return T_.linear(); }
88 Position::ConstLinearPart
rotation()
const {
return T_.linear(); }
90 template<
typename Derived>
92 T_.linear() = R.template cast<Affine3::Scalar>();
97 T_.linear() = a.template cast<Affine3::Scalar>().toRotationMatrix();
104 Position::ConstTranslationPart
b()
const {
return Tb_.translation(); }
107 Position::ConstLinearPart
Rb()
const {
return Tb_.linear(); }
116 ROTATIONAL_JOINT = REVOLUTE_JOINT,
128 PSEUDO_CONTINUOUS_TRACK = 4,
132 AGX_CRAWLER_JOINT = 6
143 std::string jointTypeString()
const;
149 double q()
const {
return q_; }
150 double&
q() {
return q_; }
151 double dq()
const {
return dq_; }
152 double&
dq() {
return dq_; }
153 double ddq()
const {
return ddq_; }
154 double&
ddq() {
return ddq_; }
155 double u()
const {
return u_; }
156 double&
u() {
return u_; }
182 double m()
const {
return m_; }
183 double mass()
const {
return m_; }
189 double Jm2()
const {
return Jm2_; }
193 Vector6::ConstFixedSegmentReturnType<3>::Type
f_ext()
const {
return F_ext_.head<3>(); }
194 Vector6::FixedSegmentReturnType<3>::Type
f_ext() {
return F_ext_.head<3>(); }
195 Vector6::ConstFixedSegmentReturnType<3>::Type
tau_ext()
const {
return F_ext_.tail<3>(); }
196 Vector6::FixedSegmentReturnType<3>::Type
tau_ext() {
return F_ext_.tail<3>(); }
198 const std::string&
name()
const {
return name_; }
207 virtual void prependChild(
Link* link);
208 virtual void appendChild(
Link* link);
209 bool removeChild(
Link* link);
215 template<
typename Derived>
217 Tb_.translation() = offset;
219 template<
typename Derived>
221 Tb_.linear() = offset;
225 Tb_.linear() = a.template cast<Affine3::Scalar>().toRotationMatrix();
228 template<
typename Derived>
237 void setJointRange(
double lower,
double upper) { q_lower_ = lower; q_upper_ = upper; }
245 void setName(
const std::string& name);
247 void setShape(
SgNode* shape);
248 void setVisualShape(
SgNode* shape);
249 void setCollisionShape(
SgNode* shape);
259 template<
typename T> T info(
const std::string& key)
const;
260 template<
typename T> T info(
const std::string& key,
const T& defaultValue)
const;
261 template<
typename T>
void setInfo(
const std::string& key,
const T& value);
268 #ifdef CNOID_BACKWARD_COMPATIBILITY 270 const double& ulimit()
const {
return q_upper_; }
271 const double& llimit()
const {
return q_lower_; }
272 const double& uvlimit()
const {
return dq_upper_; }
273 const double& lvlimit()
const {
return dq_lower_; }
275 Matrix3 segmentAttitude()
const {
return R() * Rs_; }
276 void setSegmentAttitude(
const Matrix3& Ra) { R() = Ra * Rs_.transpose(); }
311 SgNodePtr visualShape_;
312 SgNodePtr collisionShape_;
317 void setBody(
Body* newBody);
318 void setBodySub(
Body* newBody);
void setTranslation(const Eigen::MatrixBase< Derived > &p)
Definition: Link.h:81
Vector3 & w()
Definition: Link.h:166
double dq_lower() const
the upper limit of joint velocities
Definition: Link.h:161
double Jm2() const
Equivalent rotor inertia: n^2*Jm [kg.m^2].
Definition: Link.h:189
Position::ConstTranslationPart translation() const
Definition: Link.h:78
Vector3 & v()
Definition: Link.h:164
void setRotation(const Eigen::MatrixBase< Derived > &R)
Definition: Link.h:91
void setJointId(int id)
Definition: Link.h:234
const Vector3 & wc() const
center of mass (world coordinate)
Definition: Link.h:177
Definition: SceneGraph.h:142
const Vector3 & dv() const
Definition: Link.h:167
Vector6 & F_ext()
Definition: Link.h:192
Position & position()
Definition: Link.h:61
const Body * body() const
Definition: Link.h:56
void setOffsetRotation(const Eigen::MatrixBase< Derived > &offset)
Definition: Link.h:220
Position::ConstLinearPart offsetRotation() const
Definition: Link.h:108
bool isFreeJoint() const
Definition: Link.h:139
Link * sibling() const
Definition: Link.h:50
const Vector3 & centerOfMassGlobal() const
Definition: Link.h:178
const Matrix3 & I() const
Definition: Link.h:186
Position::ConstTranslationPart p() const
Definition: Link.h:76
Definition: ValueTree.h:224
void setJointRange(double lower, double upper)
Definition: Link.h:237
double q_lower() const
the lower limit of joint values
Definition: Link.h:159
double ddq() const
Definition: Link.h:153
Position::ConstLinearPart rotation() const
Definition: Link.h:88
Position::TranslationPart p()
Definition: Link.h:75
double & initialJointDisplacement()
Definition: Link.h:266
void setJointVelocityRange(double lower, double upper)
Definition: Link.h:238
void setEquivalentRotorInertia(double Jm2)
Definition: Link.h:243
int index() const
Definition: Link.h:46
const Vector3 & jointAxis() const
Definition: Link.h:146
Vector6::ConstFixedSegmentReturnType< 3 >::Type tau_ext() const
Definition: Link.h:195
Vector6::FixedSegmentReturnType< 3 >::Type tau_ext()
Definition: Link.h:196
SgNode * shape() const
Definition: Link.h:200
Definition: Referenced.h:67
Position::LinearPart rotation()
Definition: Link.h:87
Vector3 & dv()
Definition: Link.h:168
Vector3 & wc()
Definition: Link.h:179
void setAttitude(const Matrix3 &Ra)
Definition: Link.h:253
void setInfo(const std::string &key, const T &value)
void setIndex(int index)
Definition: Link.h:205
const Mapping * info() const
Definition: Link.h:256
SgNode * visualShape() const
Definition: Link.h:201
void setPosition(const Eigen::MatrixBase< Derived1 > &rotation, const Eigen::MatrixBase< Derived2 > &translation)
Definition: Link.h:70
Vector3 & dw()
Definition: Link.h:170
const Position & T() const
Definition: Link.h:59
double initialJointDisplacement() const
Definition: Link.h:265
ref_ptr< SgNode > SgNodePtr
Definition: SceneGraph.h:138
double mass() const
inertia tensor (self local, around c)
Definition: Link.h:183
JointType
Definition: Link.h:113
Position::ConstTranslationPart b() const
Definition: Link.h:104
const Vector3 & dw() const
Definition: Link.h:169
Matrix3 & Rs()
Definition: Link.h:110
const Vector6 & F_ext() const
Definition: Link.h:191
const Vector3 & centerOfMass() const
Definition: Link.h:174
double & dq()
Definition: Link.h:152
Position::ConstLinearPart Rb() const
Definition: Link.h:107
Link * child() const
Definition: Link.h:51
Position::ConstTranslationPart offsetTranslation() const
Definition: Link.h:105
void setMass(double m)
Definition: Link.h:241
double dq() const
Definition: Link.h:151
void setJointAxis(const Vector3 &axis)
Definition: Link.h:235
const Vector3 & w() const
Definition: Link.h:165
const Vector3 & d() const
Definition: Link.h:147
const std::string & name() const
Definition: Link.h:198
const Vector3 & v() const
Definition: Link.h:163
Eigen::Transform< double, 3, Eigen::AffineCompact > Position
Definition: EigenTypes.h:73
Body * body()
Definition: Link.h:55
JointType jointType() const
Definition: Link.h:137
double dq_upper() const
the upper limit of joint velocities
Definition: Link.h:160
int jointId() const
Definition: Link.h:135
Link * parent() const
Definition: Link.h:49
void setOffsetRotation(const Eigen::AngleAxis< T > &a)
Definition: Link.h:224
const Vector3 & c() const
center of mass (self local)
Definition: Link.h:173
double & q()
Definition: Link.h:150
const Position & Tb() const
Definition: Link.h:102
Mapping * info()
Definition: Link.h:257
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
Position & Tb()
Definition: Link.h:101
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Definition: Link.h:34
double u() const
Definition: Link.h:155
bool isValid() const
Definition: Link.h:47
const Matrix3 & Rs() const
Definition: Link.h:111
bool isRotationalJoint() const
Definition: Link.h:140
Vector6::ConstFixedSegmentReturnType< 3 >::Type f_ext() const
Definition: Link.h:193
void setInertia(const Matrix3 &I)
Definition: Link.h:242
Position & T()
Definition: Link.h:58
Matrix3 attitude() const
Definition: Link.h:252
void setJointType(JointType type)
Definition: Link.h:233
Vector6::FixedSegmentReturnType< 3 >::Type f_ext()
Definition: Link.h:194
const Position & position() const
Definition: Link.h:62
double m() const
mass
Definition: Link.h:182
ref_ptr< Link > LinkPtr
Definition: Link.h:22
Eigen::Vector3d Vector3
Definition: EigenTypes.h:58
ref_ptr< Mapping > MappingPtr
Definition: ValueTree.h:416
const Vector3 & a() const
Definition: Link.h:145
void setOffsetPosition(const Position &T)
Definition: Link.h:211
Position::TranslationPart translation()
Definition: Link.h:77
void setOffsetTranslation(const Eigen::MatrixBase< Derived > &offset)
Definition: Link.h:216
#define CNOID_EXPORT
Definition: Util/exportdecl.h:37
Matrix3 calcRfromAttitude(const Matrix3 &Ra)
Definition: Link.h:254
SgNode * collisionShape() const
Definition: Link.h:202
bool isSlideJoint() const
Definition: Link.h:141
Eigen::Matrix< double, 6, 1 > Vector6
Definition: EigenTypes.h:63
void setAccumulatedSegmentRotation(const Eigen::MatrixBase< Derived > &Rs)
Definition: Link.h:229
double q() const
Definition: Link.h:149
bool isRoot() const
Definition: Link.h:53
double q_upper() const
the upper limit of joint values
Definition: Link.h:158
Position::ConstLinearPart R() const
Definition: Link.h:86
void setRotation(const Eigen::AngleAxis< T > &a)
Definition: Link.h:96
double & ddq()
Definition: Link.h:154
void setCenterOfMass(const Vector3 &c)
Definition: Link.h:240
void setPosition(const Eigen::Transform< Scalar, 3, Mode, Options > &T)
Definition: Link.h:65
bool isFixedJoint() const
Definition: Link.h:138
Position::LinearPart R()
Definition: Link.h:85
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:57
double & u()
Definition: Link.h:156