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 = GetTreeElementQNr(it->second);
00043         
00044         //get the pose of the segment:
00045         Frame T_local = GetTreeElementSegment(it->second).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 (GetTreeElementSegment(it->second).getJoint().getType() != Joint::None) {
00051             Twist t_local = GetTreeElementSegment(it->second).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 = GetTreeElementParent(it->second);
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 


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14