KDL  1.4.0
chainidsolver_vereshchagin.hpp
Go to the documentation of this file.
1 // Copyright (C) 2009, 2011
2 
3 // Version: 1.0
4 // Author: Ruben Smits, Herman Bruyninckx, Azamat Shakhimardanov
5 // Maintainer: Ruben Smits, Azamat Shakhimardanov
6 // URL: http://www.orocos.org/kdl
7 
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12 
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // Lesser General Public License for more details.
17 
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 
22 #ifndef KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP
23 #define KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP
24 
25 #include "chainidsolver.hpp"
26 #include "frames.hpp"
28 
29 #include<Eigen/StdVector>
30 
31 namespace KDL
32 {
41 {
42  typedef std::vector<Twist> Twists;
43  typedef std::vector<Frame> Frames;
44  typedef Eigen::Matrix<double, 6, 1 > Vector6d;
45  typedef Eigen::Matrix<double, 6, 6 > Matrix6d;
46  typedef Eigen::Matrix<double, 6, Eigen::Dynamic> Matrix6Xd;
47 
48 public:
55  ChainIdSolver_Vereshchagin(const Chain& chain, Twist root_acc, unsigned int nc);
56 
58  {
59  };
60 
74  int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques);
75 
77  virtual void updateInternalDataStructures();
78  /*
79  //Returns cartesian positions of links in base coordinates
80  void getLinkCartesianPose(Frames& x_base);
81  //Returns cartesian velocities of links in base coordinates
82  void getLinkCartesianVelocity(Twists& xDot_base);
83  //Returns cartesian acceleration of links in base coordinates
84  void getLinkCartesianAcceleration(Twists& xDotDot_base);
85  //Returns cartesian positions of links in link tip coordinates
86  void getLinkPose(Frames& x_local);
87  //Returns cartesian velocities of links in link tip coordinates
88  void getLinkVelocity(Twists& xDot_local);
89  //Returns cartesian acceleration of links in link tip coordinates
90  void getLinkAcceleration(Twists& xDotdot_local);
91  //Acceleration energy due to unit constraint forces at the end-effector
92  void getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M);
93  //Acceleration energy due to arm configuration: bias force plus input joint torques
94  void getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G);
95 
96  void getLinkUnitForceMatrix(Matrix6Xd& E_tilde);
97 
98  void getLinkBiasForceMatrix(Wrenches& R_tilde);
99 
100  void getJointBiasAcceleration(JntArray &bias_q_dotdot);
101  */
102 private:
107  void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext);
112  void downwards_sweep(const Jacobian& alfa, const JntArray& torques);
117  void constraint_calculation(const JntArray& beta);
122  void final_upwards_sweep(JntArray &q_dotdot, JntArray &torques);
123 
124 private:
125  const Chain& chain;
126  unsigned int nj;
127  unsigned int ns;
128  unsigned int nc;
132  Eigen::MatrixXd M_0_inverse;
133  Eigen::MatrixXd Um;
134  Eigen::MatrixXd Vm;
136  Eigen::VectorXd nu;
137  Eigen::VectorXd nu_sum;
138  Eigen::VectorXd Sm;
139  Eigen::VectorXd tmpm;
142 
144  {
145  Frame F; //local pose with respect to previous link in segments coordinates
146  Frame F_base; // pose of a segment in root coordinates
147  Twist Z; //Unit twist
148  Twist v; //twist
149  Twist acc; //acceleration twist
150  Wrench U; //wrench p of the bias forces (in cartesian space)
151  Wrench R; //wrench p of the bias forces
152  Wrench R_tilde; //vector of wrench p of the bias forces (new) in matrix form
153  Twist C; //constraint
154  Twist A; //constraint
155  ArticulatedBodyInertia H; //I (expressed in 6*6 matrix)
156  ArticulatedBodyInertia P; //I (expressed in 6*6 matrix)
157  ArticulatedBodyInertia P_tilde; //I (expressed in 6*6 matrix)
158  Wrench PZ; //vector U[i] = I_A[i]*S[i]
159  Wrench PC; //vector E[i] = I_A[i]*c[i]
160  double D; //vector D[i] = S[i]^T*U[i]
161  Matrix6Xd E; //matrix with virtual unit constraint force due to acceleration constraints
163  Eigen::MatrixXd M; //acceleration energy already generated at link i
164  Eigen::VectorXd G; //magnitude of the constraint forces already generated at link i
165  Eigen::VectorXd EZ; //K[i] = Etiltde'*Z
166  double nullspaceAccComp; //Azamat: constribution of joint space u[i] forces to joint space acceleration
167  double constAccComp; //Azamat: constribution of joint space constraint forces to joint space acceleration
168  double biasAccComp; //Azamat: constribution of joint space bias forces to joint space acceleration
169  double totalBias; //Azamat: R+PC (centrepital+coriolis) in joint subspace
170  double u; //vector u[i] = torques(i) - S[i]^T*(p_A[i] + I_A[i]*C[i]) in joint subspace. Azamat: In code u[i] = torques(i) - s[i].totalBias
171 
172  segment_info(unsigned int nc):
174  {
175  E.resize(6, nc);
176  E_tilde.resize(6, nc);
177  G.resize(nc);
178  M.resize(nc, nc);
179  EZ.resize(nc);
180  E.setZero();
181  E_tilde.setZero();
182  M.setZero();
183  G.setZero();
184  EZ.setZero();
185  };
186  };
187 
188  std::vector<segment_info, Eigen::aligned_allocator<segment_info> > results;
189 
190 };
191 }
192 
193 #endif
KDL::ChainIdSolver_Vereshchagin::segment_info::segment_info
segment_info(unsigned int nc)
Definition: chainidsolver_vereshchagin.hpp:172
KDL::ChainIdSolver_Vereshchagin::alfa_N2
Jacobian alfa_N2
Definition: chainidsolver_vereshchagin.hpp:131
KDL::Vector::data
double data[3]
Definition: frames.hpp:163
KDL::ChainIdSolver_Vereshchagin::downwards_sweep
void downwards_sweep(const Jacobian &alfa, const JntArray &torques)
This method is a force balance sweep.
Definition: chainidsolver_vereshchagin.cpp:133
KDL::ChainIdSolver_Vereshchagin::Twists
std::vector< Twist > Twists
Definition: chainidsolver_vereshchagin.hpp:42
frames.hpp
KDL::ChainIdSolver_Vereshchagin::segment_info::P_tilde
ArticulatedBodyInertia P_tilde
Definition: chainidsolver_vereshchagin.hpp:157
KDL::Jacobian::columns
unsigned int columns() const
Definition: jacobian.cpp:75
KDL::Frame::Identity
static Frame Identity()
Definition: frames.inl:695
KDL::ChainIdSolver_Vereshchagin::segment_info::nullspaceAccComp
double nullspaceAccComp
Definition: chainidsolver_vereshchagin.hpp:166
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
KDL::Frame::Inverse
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.inl:422
KDL::ChainIdSolver_Vereshchagin::segment_info::u
double u
Definition: chainidsolver_vereshchagin.hpp:170
KDL::ChainIdSolver_Vereshchagin::segment_info::D
double D
Definition: chainidsolver_vereshchagin.hpp:160
KDL::JntArray
Definition: jntarray.hpp:69
KDL::ChainIdSolver_Vereshchagin::segment_info::EZ
Eigen::VectorXd EZ
Definition: chainidsolver_vereshchagin.hpp:165
KDL::Rotation::Inverse
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.inl:632
KDL::ChainIdSolver_Vereshchagin::segment_info::C
Twist C
Definition: chainidsolver_vereshchagin.hpp:153
KDL::Segment::getJoint
const Joint & getJoint() const
Request the joint of the segment.
Definition: segment.hpp:118
KDL::ChainIdSolver_Vereshchagin::nu_sum
Eigen::VectorXd nu_sum
Definition: chainidsolver_vereshchagin.hpp:137
KDL::ChainIdSolver_Vereshchagin::constraint_calculation
void constraint_calculation(const JntArray &beta)
This method calculates constraint force magnitudes.
Definition: chainidsolver_vereshchagin.cpp:249
frames_io.hpp
KDL::ChainIdSolver_Vereshchagin::segment_info
Definition: chainidsolver_vereshchagin.hpp:143
chainidsolver_vereshchagin.hpp
KDL::ChainIdSolver_Vereshchagin::segment_info::A
Twist A
Definition: chainidsolver_vereshchagin.hpp:154
KDL::ArticulatedBodyInertia
6D Inertia of a articulated body
Definition: articulatedbodyinertia.hpp:40
KDL::SolverI::E_NOT_UP_TO_DATE
@ E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
KDL::ChainIdSolver_Vereshchagin::segment_info::E_tilde
Matrix6Xd E_tilde
Definition: chainidsolver_vereshchagin.hpp:162
KDL::ChainIdSolver_Vereshchagin::segment_info::F
Frame F
Definition: chainidsolver_vereshchagin.hpp:145
KDL::ChainIdSolver_Vereshchagin::segment_info::P
ArticulatedBodyInertia P
Definition: chainidsolver_vereshchagin.hpp:156
KDL::ChainIdSolver_Vereshchagin::segment_info::constAccComp
double constAccComp
Definition: chainidsolver_vereshchagin.hpp:167
KDL::ChainIdSolver_Vereshchagin::segment_info::v
Twist v
Definition: chainidsolver_vereshchagin.hpp:148
KDL
Definition: articulatedbodyinertia.cpp:28
KDL::Chain::getNrOfSegments
unsigned int getNrOfSegments() const
Request the total number of segments in the chain.
Definition: chain.hpp:76
KDL::ChainIdSolver_Vereshchagin::Matrix6d
Eigen::Matrix< double, 6, 6 > Matrix6d
Definition: chainidsolver_vereshchagin.hpp:45
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
KDL::ChainIdSolver_Vereshchagin::qdotdot_sum
Wrench qdotdot_sum
Definition: chainidsolver_vereshchagin.hpp:140
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:720
KDL::ChainIdSolver_Vereshchagin::F_total
Frame F_total
Definition: chainidsolver_vereshchagin.hpp:141
KDL::ChainIdSolver_Vereshchagin::segment_info::acc
Twist acc
Definition: chainidsolver_vereshchagin.hpp:149
KDL::ChainIdSolver_Vereshchagin::segment_info::U
Wrench U
Definition: chainidsolver_vereshchagin.hpp:150
KDL::ChainIdSolver_Vereshchagin::segment_info::R_tilde
Wrench R_tilde
Definition: chainidsolver_vereshchagin.hpp:152
KDL::ChainIdSolver_Vereshchagin::ns
unsigned int ns
Definition: chainidsolver_vereshchagin.hpp:127
KDL::JntArray::rows
unsigned int rows() const
Returns the number of rows (size) of the array.
Definition: jntarray.cpp:72
KDL::Wrench::torque
Vector torque
Torque that is applied at the origin of the current ref frame.
Definition: frames.hpp:882
KDL::ChainIdSolver_Vereshchagin::results
std::vector< segment_info, Eigen::aligned_allocator< segment_info > > results
Definition: chainidsolver_vereshchagin.hpp:188
dot
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition: frameacc.inl:137
KDL::Chain::getSegment
const Segment & getSegment(unsigned int nr) const
Request the nr'd segment of the chain.
Definition: chain.cpp:68
KDL::SolverI::E_SIZE_MISMATCH
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
KDL::ChainIdSolver_Vereshchagin::ChainIdSolver_Vereshchagin
ChainIdSolver_Vereshchagin(const Chain &chain, Twist root_acc, unsigned int nc)
Constructor for the solver, it will allocate all the necessary memory.
Definition: chainidsolver_vereshchagin.cpp:31
KDL::ChainIdSolver_Vereshchagin::M_0_inverse
Eigen::MatrixXd M_0_inverse
Definition: chainidsolver_vereshchagin.hpp:132
KDL::Wrench
represents both translational and rotational acceleration.
Definition: frames.hpp:878
KDL::ChainIdSolver_Vereshchagin
Dynamics calculations by constraints based on Vereshchagin 1989.
Definition: chainidsolver_vereshchagin.hpp:40
KDL::ChainIdSolver_Vereshchagin::segment_info::biasAccComp
double biasAccComp
Definition: chainidsolver_vereshchagin.hpp:168
KDL::ChainIdSolver_Vereshchagin::nu
Eigen::VectorXd nu
Definition: chainidsolver_vereshchagin.hpp:136
KDL::Segment::pose
Frame pose(const double &q) const
Request the pose of the segment, given the joint position q.
Definition: segment.cpp:57
chainidsolver.hpp
KDL::Frame
Definition: frames.hpp:570
KDL::Segment::twist
Twist twist(const double &q, const double &qdot) const
Request the 6D-velocity of the tip of the segment, given the joint position q and the joint velocity ...
Definition: segment.cpp:62
KDL::Wrenches
std::vector< Wrench > Wrenches
Definition: chainidsolver.hpp:33
KDL::JntArray::data
Eigen::VectorXd data
Definition: jntarray.hpp:72
KDL::ChainIdSolver_Vereshchagin::Sm
Eigen::VectorXd Sm
Definition: chainidsolver_vereshchagin.hpp:138
KDL::ChainIdSolver_Vereshchagin::segment_info::PC
Wrench PC
Definition: chainidsolver_vereshchagin.hpp:159
KDL::ChainIdSolver_Vereshchagin::Vm
Eigen::MatrixXd Vm
Definition: chainidsolver_vereshchagin.hpp:134
KDL::Joint::None
@ None
Definition: joint.hpp:47
KDL::ChainIdSolver_Vereshchagin::segment_info::E
Matrix6Xd E
Definition: chainidsolver_vereshchagin.hpp:161
KDL::ChainIdSolver_Vereshchagin::segment_info::totalBias
double totalBias
Definition: chainidsolver_vereshchagin.hpp:169
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:722
KDL::Segment
Definition: segment.hpp:46
KDL::ChainIdSolver_Vereshchagin::Frames
std::vector< Frame > Frames
Definition: chainidsolver_vereshchagin.hpp:43
KDL::ChainIdSolver_Vereshchagin::segment_info::G
Eigen::VectorXd G
Definition: chainidsolver_vereshchagin.hpp:164
KDL::ChainIdSolver_Vereshchagin::chain
const Chain & chain
Definition: chainidsolver_vereshchagin.hpp:125
KDL::SolverI
Solver interface supporting storage and description of the latest error.
Definition: solveri.hpp:84
KDL::ChainIdSolver_Vereshchagin::CartToJnt
int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian &alfa, const JntArray &beta, const Wrenches &f_ext, JntArray &torques)
This method calculates joint space constraint torques and total joint space acceleration.
Definition: chainidsolver_vereshchagin.cpp:51
KDL::ChainIdSolver_Vereshchagin::acc_root
Twist acc_root
Definition: chainidsolver_vereshchagin.hpp:129
KDL::ChainIdSolver_Vereshchagin::segment_info::H
ArticulatedBodyInertia H
Definition: chainidsolver_vereshchagin.hpp:155
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
KDL::ChainIdSolver_Vereshchagin::segment_info::R
Wrench R
Definition: chainidsolver_vereshchagin.hpp:151
articulatedbodyinertia.hpp
KDL::ChainIdSolver_Vereshchagin::tmpm
Eigen::VectorXd tmpm
Definition: chainidsolver_vereshchagin.hpp:139
KDL::ChainIdSolver_Vereshchagin::Vector6d
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: chainidsolver_vereshchagin.hpp:44
KDL::Joint::getType
const JointType & getType() const
Request the type of the joint.
Definition: joint.hpp:159
KDL::ChainIdSolver_Vereshchagin::Um
Eigen::MatrixXd Um
Definition: chainidsolver_vereshchagin.hpp:133
KDL::ChainIdSolver_Vereshchagin::nj
unsigned int nj
Definition: chainidsolver_vereshchagin.hpp:126
KDL::ChainIdSolver_Vereshchagin::segment_info::Z
Twist Z
Definition: chainidsolver_vereshchagin.hpp:147
KDL::SolverI::E_NOERROR
@ E_NOERROR
No error.
Definition: solveri.hpp:91
KDL::ChainIdSolver_Vereshchagin::nc
unsigned int nc
Definition: chainidsolver_vereshchagin.hpp:128
KDL::ChainIdSolver_Vereshchagin::segment_info::M
Eigen::MatrixXd M
Definition: chainidsolver_vereshchagin.hpp:163
KDL::Wrench::force
Vector force
Force that is applied at the origin of the current ref frame.
Definition: frames.hpp:881
KDL::ChainIdSolver_Vereshchagin::updateInternalDataStructures
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainidsolver_vereshchagin.cpp:46
KDL::Chain
Definition: chain.hpp:35
KDL::Segment::getInertia
const RigidBodyInertia & getInertia() const
Request the inertia of the segment.
Definition: segment.hpp:128
KDL::ChainIdSolver_Vereshchagin::final_upwards_sweep
void final_upwards_sweep(JntArray &q_dotdot, JntArray &torques)
This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations...
Definition: chainidsolver_vereshchagin.cpp:284
KDL::ChainIdSolver_Vereshchagin::alfa_N
Jacobian alfa_N
Definition: chainidsolver_vereshchagin.hpp:130
KDL::ChainIdSolver_Vereshchagin::initial_upwards_sweep
void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext)
This method calculates all cartesian space poses, twists, bias accelerations.
Definition: chainidsolver_vereshchagin.cpp:72
KDL::ChainIdSolver_Vereshchagin::beta_N
JntArray beta_N
Definition: chainidsolver_vereshchagin.hpp:135
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Request the total number of joints in the chain.
Definition: chain.hpp:71
KDL::ChainIdSolver_Vereshchagin::Matrix6Xd
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6Xd
Definition: chainidsolver_vereshchagin.hpp:46
KDL::ChainIdSolver_Vereshchagin::segment_info::F_base
Frame F_base
Definition: chainidsolver_vereshchagin.hpp:146
KDL::ChainIdSolver_Vereshchagin::~ChainIdSolver_Vereshchagin
~ChainIdSolver_Vereshchagin()
Definition: chainidsolver_vereshchagin.hpp:57
KDL::ChainIdSolver_Vereshchagin::segment_info::PZ
Wrench PZ
Definition: chainidsolver_vereshchagin.hpp:158
KDL::SolverI::error
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
KDL::Jacobian
Definition: jacobian.hpp:36