1 /** \file itasc/kdl/treejnttojacsolver.cpp
2 * \ingroup itasc
3 */
4 /*
5 * TreeJntToJacSolver.cpp
6 *
7 * Created on: Nov 27, 2008
8 * Author: rubensmits
9 */
10
11 #include "treejnttojacsolver.hpp"
12 #include <iostream>
13
14 namespace KDL {
15
TreeJntToJacSolver(const Tree & tree_in)16 TreeJntToJacSolver::TreeJntToJacSolver(const Tree& tree_in) :
17 tree(tree_in) {
18 }
19
~TreeJntToJacSolver()20 TreeJntToJacSolver::~TreeJntToJacSolver() {
21 }
22
JntToJac(const JntArray & q_in,Jacobian & jac,const std::string & segmentname)23 int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac,
24 const std::string& segmentname) {
25 //First we check all the sizes:
26 if (q_in.rows() != tree.getNrOfJoints() || jac.columns()
27 != tree.getNrOfJoints())
28 return -1;
29
30 //Lets search the tree-element
31 SegmentMap::value_type const* it = tree.getSegmentPtr(segmentname);
32
33 //If segmentname is not inside the tree, back out:
34 if (!it)
35 return -2;
36
37 //Let's make the jacobian zero:
38 SetToZero(jac);
39
40 SegmentMap::value_type const* root = tree.getSegmentPtr("root");
41
42 Frame T_total = Frame::Identity();
43 Frame T_local, T_joint;
44 Twist t_local;
45 //Lets recursively iterate until we are in the root segment
46 while (it != root) {
47 //get the corresponding q_nr for this TreeElement:
48 unsigned int q_nr = it->second.q_nr;
49
50 //get the pose of the joint.
51 T_joint = it->second.segment.getJoint().pose(((JntArray&)q_in)(q_nr));
52 // combine with the tip to have the tip pose
53 T_local = T_joint*it->second.segment.getFrameToTip();
54 //calculate new T_end:
55 T_total = T_local * T_total;
56
57 //get the twist of the segment:
58 int ndof = it->second.segment.getJoint().getNDof();
59 for (int dof=0; dof<ndof; dof++) {
60 // combine joint rotation with tip position to get a reference frame for the joint
61 T_joint.p = T_local.p;
62 // in which the twist can be computed (needed for NDof joint)
63 t_local = it->second.segment.twist(T_joint, 1.0, dof);
64 //transform the endpoint of the local twist to the global endpoint:
65 t_local = t_local.RefPoint(T_total.p - T_local.p);
66 //transform the base of the twist to the endpoint
67 t_local = T_total.M.Inverse(t_local);
68 //store the twist in the jacobian:
69 jac.twists[q_nr+dof] = t_local;
70 }
71 //goto the parent
72 it = it->second.parent;
73 }//endwhile
74 //Change the base of the complete jacobian from the endpoint to the base
75 changeBase(jac, T_total.M, jac);
76
77 return 0;
78
79 }//end JntToJac
80 }//end namespace
81
82