24 const std::string& segmentname) {
31 SegmentMap::value_type
const* it = tree.
getSegmentPtr(segmentname);
40 SegmentMap::value_type
const* root = tree.
getSegmentPtr(
"root");
43 Frame T_local, T_joint;
48 unsigned int q_nr = it->second.q_nr;
51 T_joint = it->second.segment.getJoint().pose(((
JntArray&)q_in)(q_nr));
53 T_local = T_joint*it->second.segment.getFrameToTip();
55 T_total = T_local * T_total;
58 int ndof = it->second.segment.getJoint().getNDof();
59 for (
int dof=0; dof<ndof; dof++) {
61 T_joint.
p = T_local.
p;
63 t_local = it->second.segment.twist(T_joint, 1.0, dof);
65 t_local = t_local.
RefPoint(T_total.p - T_local.
p);
67 t_local = T_total.M.Inverse(t_local);
69 jac.
twists[q_nr+dof] = t_local;
72 it = it->second.parent;
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
TreeJntToJacSolver(const Tree &tree)
int JntToJac(const JntArray &q_in, Jacobian &jac, const std::string &segmentname)
virtual ~TreeJntToJacSolver()
This class encapsulates a tree kinematic interconnection structure. It is build out of segments.
SegmentMap::value_type const * getSegmentPtr(const std::string &segment_name) const
unsigned int getNrOfJoints() const
represents both translational and rotational velocities.
Twist RefPoint(const Vector &v_base_AB) const
void changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
void SetToZero(Jacobian &jac)