44 Frame T_local, T_joint;
54 int ndof =
segment.getJoint().getNDof();
60 T_local = T_joint*
segment.getFrameToTip();
62 T_total = T_local * T_total;
64 for (
int dof=0; dof<ndof; dof++) {
66 T_joint.
p = T_local.
p;
68 t_local =
segment.twist(T_joint, 1.0, dof);
70 t_local = t_local.
RefPoint(T_total.p - T_local.
p);
72 t_local = T_total.M.Inverse(t_local);
74 jac.
twists[q_nr+dof] = t_local;
ChainJntToJacSolver(const Chain &chain)
int JntToJac(const JntArray &q_in, Jacobian &jac)
This class encapsulates a serial kinematic interconnection structure. It is build out of segments.
const Segment & getSegment(unsigned int nr) const
unsigned int getNrOfJoints() const
unsigned int getNrOfSegments() const
represents a frame transformation in 3D space (rotation + translation)
Rotation M
Orientation of the Frame.
Vector p
origine of the Frame
unsigned int columns() const
unsigned int rows() const
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and an inertia) with...
Twist RefPoint(const Vector &v_base_AB) const
Segment< FEdge *, Vec3r > segment
void changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
void SetToZero(Jacobian &jac)