2 #ifndef CNOID_BODY_JACOBIAN_H 3 #define CNOID_BODY_JACOBIAN_H 16 template<
int elementMask,
int rowOffset,
int colOffset,
bool useTargetLinkLocalPos>
20 const bool isTranslationValid = (elementMask & 0x7);
29 MatrixXd::ColXpr Ji = out_J.col(i + colOffset);
39 if(isTranslationValid){
41 if(useTargetLinkLocalPos){
42 arm = targetLink->
p() + targetLink->
R() * targetLinkLocalPos - link->
p();
44 arm = targetLink->
p() - link->
p();
46 const Vector3 dp = omega.cross(arm);
47 if(elementMask & 0x1) Ji(row++) = dp.x();
48 if(elementMask & 0x2) Ji(row++) = dp.y();
49 if(elementMask & 0x4) Ji(row++) = dp.z();
51 if(elementMask & 0x8) Ji(row++) = omega.x();
52 if(elementMask & 0x10) Ji(row++) = omega.y();
53 if(elementMask & 0x20) Ji(row ) = omega.z();
59 if(isTranslationValid){
64 if(elementMask & 0x1) Ji(row++) = dp.x();
65 if(elementMask & 0x2) Ji(row++) = dp.y();
66 if(elementMask & 0x4) Ji(row++) = dp.z();
68 if(elementMask & 0x8) Ji(row++) = 0.0;
69 if(elementMask & 0x10) Ji(row++) = 0.0;
70 if(elementMask & 0x20) Ji(row ) = 0.0;
75 if(elementMask & 0x1) Ji(row++) = 0.0;
76 if(elementMask & 0x2) Ji(row++) = 0.0;
77 if(elementMask & 0x4) Ji(row++) = 0.0;
78 if(elementMask & 0x8) Ji(row++) = 0.0;
79 if(elementMask & 0x10) Ji(row++) = 0.0;
80 if(elementMask & 0x20) Ji(row ) = 0.0;
85 if(link == targetLink){
91 MatrixXd::ColXpr Ji = out_J.col(i + colOffset);
93 if(elementMask & 0x1) Ji(row++) = 0.0;
94 if(elementMask & 0x2) Ji(row++) = 0.0;
95 if(elementMask & 0x4) Ji(row++) = 0.0;
96 if(elementMask & 0x8) Ji(row++) = 0.0;
97 if(elementMask & 0x10) Ji(row++) = 0.0;
98 if(elementMask & 0x20) Ji(row ) = 0.0;
103 template<
int elementMask,
int rowOffset,
int colOffset>
105 static const Vector3 targetLinkLocalPos(Vector3::Zero());
106 setJacobian<elementMask, rowOffset, colOffset, false>(
107 path, targetLink, targetLinkLocalPos, out_J);
void calcCMJacobian(Body *body, Link *base, Eigen::MatrixXd &J)
compute CoM Jacobian
Definition: Jacobian.cpp:68
Link * joint(int index) const
Definition: JointPath.h:46
void calcAngularMomentumJacobian(Body *body, Link *base, Eigen::MatrixXd &H)
compute Angular Momentum Jacobian
Definition: Jacobian.cpp:151
Position::TranslationPart p()
Definition: Link.h:75
int numJoints() const
Definition: JointPath.h:42
translational joint (1 dof)
Definition: Link.h:118
void setJacobian(const JointPath &path, Link *targetLink, const Vector3 &targetLinkLocalPos, MatrixXd &out_J)
Definition: Jacobian.h:17
bool isJointDownward(int index) const
Definition: JointPath.h:58
Definition: JointPath.h:21
const Vector3 & d() const
Definition: Link.h:147
The header file of the JointPath class.
JointType jointType() const
Definition: Link.h:137
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
Eigen::Vector3d Vector3
Definition: EigenTypes.h:58
const Vector3 & a() const
Definition: Link.h:145
#define CNOID_EXPORT
Definition: Util/exportdecl.h:37
Position::LinearPart R()
Definition: Link.h:85