diff options
Diffstat (limited to 'intern/itasc/kdl/treejnttojacsolver.cpp')
-rw-r--r-- | intern/itasc/kdl/treejnttojacsolver.cpp | 78 |
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 + |