Blender  V3.3
chainjnttojacsolver.cpp
Go to the documentation of this file.
1 
4 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 
6 // Version: 1.0
7 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
8 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
9 // URL: http://www.orocos.org/kdl
10 
11 // This library is free software; you can redistribute it and/or
12 // modify it under the terms of the GNU Lesser General Public
13 // License as published by the Free Software Foundation; either
14 // version 2.1 of the License, or (at your option) any later version.
15 
16 // This library is distributed in the hope that it will be useful,
17 // but WITHOUT ANY WARRANTY; without even the implied warranty of
18 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
19 // Lesser General Public License for more details.
20 
21 // You should have received a copy of the GNU Lesser General Public
22 // License along with this library; if not, write to the Free Software
23 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
24 
25 #include "chainjnttojacsolver.hpp"
26 
27 namespace KDL
28 {
30  chain(_chain)
31  {
32  }
33 
35  {
36  }
37 
39  {
40  assert(q_in.rows()==chain.getNrOfJoints()&&
41  q_in.rows()==jac.columns());
42 
43 
44  Frame T_local, T_joint;
45  T_total = Frame::Identity();
46  SetToZero(t_local);
47 
48  int i=chain.getNrOfSegments()-1;
49  unsigned int q_nr = chain.getNrOfJoints();
50 
51  //Lets recursively iterate until we are in the root segment
52  while (i >= 0) {
53  const Segment& segment = chain.getSegment(i);
54  int ndof = segment.getJoint().getNDof();
55  q_nr -= ndof;
56 
57  //get the pose of the joint.
58  T_joint = segment.getJoint().pose(((JntArray&)q_in)(q_nr));
59  // combine with the tip to have the tip pose
60  T_local = T_joint*segment.getFrameToTip();
61  //calculate new T_end:
62  T_total = T_local * T_total;
63 
64  for (int dof=0; dof<ndof; dof++) {
65  // combine joint rotation with tip position to get a reference frame for the joint
66  T_joint.p = T_local.p;
67  // in which the twist can be computed (needed for NDof joint)
68  t_local = segment.twist(T_joint, 1.0, dof);
69  //transform the endpoint of the local twist to the global endpoint:
70  t_local = t_local.RefPoint(T_total.p - T_local.p);
71  //transform the base of the twist to the endpoint
72  t_local = T_total.M.Inverse(t_local);
73  //store the twist in the jacobian:
74  jac.twists[q_nr+dof] = t_local;
75  }
76  i--;
77  }//endwhile
78  //Change the base of the complete jacobian from the endpoint to the base
79  changeBase(jac, T_total.M, jac);
80  return 0;
81  }
82 }
83 
ChainJntToJacSolver(const Chain &chain)
int JntToJac(const JntArray &q_in, Jacobian &jac)
This class encapsulates a serial kinematic interconnection structure. It is build out of segments.
Definition: chain.hpp:36
const Segment & getSegment(unsigned int nr) const
Definition: chain.cpp:68
unsigned int getNrOfJoints() const
Definition: chain.hpp:73
unsigned int getNrOfSegments() const
Definition: chain.hpp:78
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
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and an inertia) with...
Definition: segment.hpp:46
Twist RefPoint(const Vector &v_base_AB) const
Definition: frames.inl:322
Segment< FEdge *, Vec3r > segment
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