Blender V2.61 - r43446

treejnttojacsolver.cpp

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