Blender  V3.3
treejnttojacsolver.cpp
Go to the documentation of this file.
1 
4 /*
5  * TreeJntToJacSolver.cpp
6  *
7  * Created on: Nov 27, 2008
8  * Author: rubensmits
9  */
10 
11 #include "treejnttojacsolver.hpp"
12 #include <iostream>
13 
14 namespace KDL {
15 
17  tree(tree_in) {
18 }
19 
21 }
22 
24  const std::string& segmentname) {
25  //First we check all the sizes:
26  if (q_in.rows() != tree.getNrOfJoints() || jac.columns()
27  != tree.getNrOfJoints())
28  return -1;
29 
30  //Lets search the tree-element
31  SegmentMap::value_type const* it = tree.getSegmentPtr(segmentname);
32 
33  //If segmentname is not inside the tree, back out:
34  if (!it)
35  return -2;
36 
37  //Let's make the jacobian zero:
38  SetToZero(jac);
39 
40  SegmentMap::value_type const* root = tree.getSegmentPtr("root");
41 
42  Frame T_total = Frame::Identity();
43  Frame T_local, T_joint;
44  Twist t_local;
45  //Lets recursively iterate until we are in the root segment
46  while (it != root) {
47  //get the corresponding q_nr for this TreeElement:
48  unsigned int q_nr = it->second.q_nr;
49 
50  //get the pose of the joint.
51  T_joint = it->second.segment.getJoint().pose(((JntArray&)q_in)(q_nr));
52  // combine with the tip to have the tip pose
53  T_local = T_joint*it->second.segment.getFrameToTip();
54  //calculate new T_end:
55  T_total = T_local * T_total;
56 
57  //get the twist of the segment:
58  int ndof = it->second.segment.getJoint().getNDof();
59  for (int dof=0; dof<ndof; dof++) {
60  // combine joint rotation with tip position to get a reference frame for the joint
61  T_joint.p = T_local.p;
62  // in which the twist can be computed (needed for NDof joint)
63  t_local = it->second.segment.twist(T_joint, 1.0, dof);
64  //transform the endpoint of the local twist to the global endpoint:
65  t_local = t_local.RefPoint(T_total.p - T_local.p);
66  //transform the base of the twist to the endpoint
67  t_local = T_total.M.Inverse(t_local);
68  //store the twist in the jacobian:
69  jac.twists[q_nr+dof] = t_local;
70  }
71  //goto the parent
72  it = it->second.parent;
73  }//endwhile
74  //Change the base of the complete jacobian from the endpoint to the base
75  changeBase(jac, T_total.M, jac);
76 
77  return 0;
78 
79 }//end JntToJac
80 }//end namespace
81 
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:526
static Frame Identity()
Definition: frames.inl:719
Rotation M
Orientation of the Frame.
Definition: frames.hpp:529
Vector p
origine of the Frame
Definition: frames.hpp:528
Twist * twists
Definition: jacobian.hpp:39
unsigned int columns() const
Definition: jacobian.cpp:76
unsigned int rows() const
Definition: jntarray.cpp:93
TreeJntToJacSolver(const Tree &tree)
int JntToJac(const JntArray &q_in, Jacobian &jac, const std::string &segmentname)
This class encapsulates a tree kinematic interconnection structure. It is build out of segments.
Definition: tree.hpp:68
SegmentMap::value_type const * getSegmentPtr(const std::string &segment_name) const
Definition: tree.hpp:154
unsigned int getNrOfJoints() const
Definition: tree.hpp:131
represents both translational and rotational velocities.
Definition: frames.hpp:679
Twist RefPoint(const Vector &v_base_AB) const
Definition: frames.inl:322
void * tree
Definition: chain.cpp:27
void changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
Definition: jacobian.cpp:95
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:81