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 
#define GetTreeElementSegment(tree_element)
Definition: tree.hpp:62
SegmentMap::const_iterator getRootSegment() const
Definition: tree.hpp:186
int JntToJac(const JntArray &q_in, Jacobian &jac, const std::string &segmentname)
unsigned int columns() const
Definition: jacobian.cpp:74
#define GetTreeElementQNr(tree_element)
Definition: tree.hpp:61
unsigned int getNrOfJoints() const
Definition: tree.hpp:159
bool changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
Definition: jacobian.cpp:103
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
const SegmentMap & getSegments() const
Definition: tree.hpp:218
TreeJntToJacSolver(const Tree &tree)
unsigned int rows() const
Definition: jntarray.cpp:70
#define GetTreeElementParent(tree_element)
Definition: tree.hpp:60
Rotation M
Orientation of the Frame.
Definition: frames.hpp:575
static Frame Identity()
Definition: frames.hpp:701
represents both translational and rotational velocities.
Definition: frames.hpp:723
IMETHOD void SetToZero(Vector &v)
Definition: frames.hpp:1069
void setColumn(unsigned int i, const Twist &t)
Definition: jacobian.cpp:148
Vector p
origine of the Frame
Definition: frames.hpp:574
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
Twist RefPoint(const Vector &v_base_AB) const
Definition: frames.hpp:303
This class encapsulates a tree kinematic interconnection structure. It is built out of segments...
Definition: tree.hpp:99


orocos_kdl
Author(s):
autogenerated on Thu Apr 13 2023 02:19:14