treejnttojacsolver.cpp
Go to the documentation of this file.00001 /* 00002 * TreeJntToJacSolver.cpp 00003 * 00004 * Created on: Nov 27, 2008 00005 * Author: rubensmits 00006 */ 00007 00008 #include "treejnttojacsolver.hpp" 00009 #include <iostream> 00010 #include "kinfam_io.hpp" 00011 00012 namespace KDL { 00013 00014 TreeJntToJacSolver::TreeJntToJacSolver(const Tree& tree_in) : 00015 tree(tree_in) { 00016 } 00017 00018 TreeJntToJacSolver::~TreeJntToJacSolver() { 00019 } 00020 00021 int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, const std::string& segmentname) { 00022 //First we check all the sizes: 00023 if (q_in.rows() != tree.getNrOfJoints() || jac.columns() != tree.getNrOfJoints()) 00024 return -1; 00025 00026 //Lets search the tree-element 00027 SegmentMap::const_iterator it = tree.getSegments().find(segmentname); 00028 00029 //If segmentname is not inside the tree, back out: 00030 if (it == tree.getSegments().end()) 00031 return -2; 00032 00033 //Let's make the jacobian zero: 00034 SetToZero(jac); 00035 00036 SegmentMap::const_iterator root = tree.getRootSegment(); 00037 00038 Frame T_total = Frame::Identity(); 00039 //Lets recursively iterate until we are in the root segment 00040 while (it != root) { 00041 //get the corresponding q_nr for this TreeElement: 00042 unsigned int q_nr = it->second.q_nr; 00043 00044 //get the pose of the segment: 00045 Frame T_local = it->second.segment.pose(q_in(q_nr)); 00046 //calculate new T_end: 00047 T_total = T_local * T_total; 00048 00049 //get the twist of the segment: 00050 if (it->second.segment.getJoint().getType() != Joint::None) { 00051 Twist t_local = it->second.segment.twist(q_in(q_nr), 1.0); 00052 //transform the endpoint of the local twist to the global endpoint: 00053 t_local = t_local.RefPoint(T_total.p - T_local.p); 00054 //transform the base of the twist to the endpoint 00055 t_local = T_total.M.Inverse(t_local); 00056 //store the twist in the jacobian: 00057 jac.setColumn(q_nr,t_local); 00058 }//endif 00059 //goto the parent 00060 it = it->second.parent; 00061 }//endwhile 00062 //Change the base of the complete jacobian from the endpoint to the base 00063 changeBase(jac, T_total.M, jac); 00064 00065 return 0; 00066 00067 }//end JntToJac 00068 }//end namespace 00069