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