orocos_kdl
treejnttojacsolver.cpp
Go to the documentation of this file.
1 /*
2  * TreeJntToJacSolver.cpp
3  *
4  * Created on: Nov 27, 2008
5  * Author: rubensmits
6  */
7 
8 #include "treejnttojacsolver.hpp"
9 #include <iostream>
10 #include "kinfam_io.hpp"
11 
12 namespace KDL {
13 
15  tree(tree_in) {
16 }
17 
19 }
20 
21 int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, const std::string& segmentname) {
22  //First we check all the sizes:
23  if (q_in.rows() != tree.getNrOfJoints() || jac.columns() != tree.getNrOfJoints())
24  return -1;
25 
26  //Lets search the tree-element
27  SegmentMap::const_iterator it = tree.getSegments().find(segmentname);
28 
29  //If segmentname is not inside the tree, back out:
30  if (it == tree.getSegments().end())
31  return -2;
32 
33  //Let's make the jacobian zero:
34  SetToZero(jac);
35 
36  SegmentMap::const_iterator root = tree.getRootSegment();
37 
38  Frame T_total = Frame::Identity();
39  //Lets recursively iterate until we are in the root segment
40  while (it != root) {
41  //get the corresponding q_nr for this TreeElement:
42  unsigned int q_nr = GetTreeElementQNr(it->second);
43 
44  //get the pose of the segment:
45  Frame T_local = GetTreeElementSegment(it->second).pose(q_in(q_nr));
46  //calculate new T_end:
47  T_total = T_local * T_total;
48 
49  //get the twist of the segment:
50  if (GetTreeElementSegment(it->second).getJoint().getType() != Joint::Fixed) {
51  Twist t_local = GetTreeElementSegment(it->second).twist(q_in(q_nr), 1.0);
52  //transform the endpoint of the local twist to the global endpoint:
53  t_local = t_local.RefPoint(T_total.p - T_local.p);
54  //transform the base of the twist to the endpoint
55  t_local = T_total.M.Inverse(t_local);
56  //store the twist in the jacobian:
57  jac.setColumn(q_nr,t_local);
58  }//endif
59  //goto the parent
60  it = GetTreeElementParent(it->second);
61  }//endwhile
62  //Change the base of the complete jacobian from the endpoint to the base
63  changeBase(jac, T_total.M, jac);
64 
65  return 0;
66 
67 }//end JntToJac
68 }//end namespace
69 
KDL::Jacobian::columns
unsigned int columns() const
Definition: jacobian.cpp:74
KDL::Tree::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: tree.hpp:159
KDL::Frame::Identity
static Frame Identity()
Definition: frames.inl:700
std::string
KDL::Jacobian::setColumn
void setColumn(unsigned int i, const Twist &t)
Definition: jacobian.cpp:148
kinfam_io.hpp
KDL::JntArray
Definition: jntarray.hpp:69
KDL::Tree::getRootSegment
SegmentMap::const_iterator getRootSegment() const
Definition: tree.hpp:186
KDL::TreeJntToJacSolver::tree
KDL::Tree tree
Definition: treejnttojacsolver.hpp:32
std::map::find
T find(T... args)
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:574
KDL::TreeJntToJacSolver::JntToJac
int JntToJac(const JntArray &q_in, Jacobian &jac, const std::string &segmentname)
Definition: treejnttojacsolver.cpp:21
KDL::Joint::Fixed
@ Fixed
Definition: joint.hpp:47
GetTreeElementParent
#define GetTreeElementParent(tree_element)
Definition: tree.hpp:60
iostream
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::Tree::getSegments
const SegmentMap & getSegments() const
Definition: tree.hpp:218
KDL::JntArray::rows
unsigned int rows() const
Definition: jntarray.cpp:70
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::TreeJntToJacSolver::~TreeJntToJacSolver
virtual ~TreeJntToJacSolver()
Definition: treejnttojacsolver.cpp:18
KDL::Tree
This class encapsulates a tree kinematic interconnection structure. It is built out of segments.
Definition: tree.hpp:99
KDL::changeBase
bool changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
Definition: jacobian.cpp:103
KDL::Twist::RefPoint
Twist RefPoint(const Vector &v_base_AB) const
Definition: frames.inl:302
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:575
treejnttojacsolver.hpp
std::map::end
T end(T... args)
GetTreeElementSegment
#define GetTreeElementSegment(tree_element)
Definition: tree.hpp:62
KDL::SetToZero
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:79
KDL::TreeJntToJacSolver::TreeJntToJacSolver
TreeJntToJacSolver(const Tree &tree)
Definition: treejnttojacsolver.cpp:14
GetTreeElementQNr
#define GetTreeElementQNr(tree_element)
Definition: tree.hpp:61
KDL::Jacobian
Definition: jacobian.hpp:36