1 files changed, 78 insertions, 0 deletions
diff --git a/intern/itasc/kdl/treejnttojacsolver.cpp b/intern/itasc/kdl/treejnttojacsolver.cpp
new file mode 100644
index 00000000000..194f18eb959
--- /dev/null
+++ b/intern/itasc/kdl/treejnttojacsolver.cpp
@@ -0,0 +1,78 @@
+/*
+ * TreeJntToJacSolver.cpp
+ *
+ *  Created on: Nov 27, 2008
+ *      Author: rubensmits
+ */
+
+#include "treejnttojacsolver.hpp"
+#include <iostream>
+
+namespace KDL {
+
+TreeJntToJacSolver::TreeJntToJacSolver(const Tree& tree_in) :
+    tree(tree_in) {
+}
+
+TreeJntToJacSolver::~TreeJntToJacSolver() {
+}
+
+int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac,
+        const std::string& segmentname) {
+    //First we check all the sizes:
+    if (q_in.rows() != tree.getNrOfJoints() || jac.columns()
+            != tree.getNrOfJoints())
+        return -1;
+
+    //Lets search the tree-element
+    SegmentMap::const_iterator it = tree.getSegments().find(segmentname);
+
+    //If segmentname is not inside the tree, back out:
+    if (it == tree.getSegments().end())
+        return -2;
+
+    //Let's make the jacobian zero:
+    SetToZero(jac);
+
+    SegmentMap::const_iterator root = tree.getSegments().find("root");
+
+    Frame T_total = Frame::Identity();
+	Frame T_local, T_joint;
+	Twist t_local;
+    //Lets recursively iterate until we are in the root segment
+    while (it != root) {
+        //get the corresponding q_nr for this TreeElement:
+        unsigned int q_nr = it->second.q_nr;
+
+        //get the pose of the joint.
+		T_joint = it->second.segment.getJoint().pose(((JntArray&)q_in)(q_nr));
+		// combine with the tip to have the tip pose
+		T_local = T_joint*it->second.segment.getFrameToTip();
+        //calculate new T_end:
+        T_total = T_local * T_total;
+
+        //get the twist of the segment:
+		int ndof = it->second.segment.getJoint().getNDof();
+		for (int dof=0; dof<ndof; dof++) {
+			// combine joint rotation with tip position to get a reference frame for the joint
+			T_joint.p = T_local.p;
+			// in which the twist can be computed (needed for NDof joint)
+            t_local = it->second.segment.twist(T_joint, 1.0, dof);
+            //transform the endpoint of the local twist to the global endpoint:
+            t_local = t_local.RefPoint(T_total.p - T_local.p);
+            //transform the base of the twist to the endpoint
+            t_local = T_total.M.Inverse(t_local);
+            //store the twist in the jacobian:
+            jac.twists[q_nr+dof] = t_local;
+        }
+        //goto the parent
+        it = it->second.parent;
+    }//endwhile
+    //Change the base of the complete jacobian from the endpoint to the base
+    changeBase(jac, T_total.M, jac);
+
+    return 0;
+
+}//end JntToJac
+}//end namespace
+