KDL  1.3.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 namespace KDL
30 {
39 {
40  typedef std::vector<Twist> Twists;
41  typedef std::vector<Frame> Frames;
42  typedef Eigen::Matrix<double, 6, 1 > Vector6d;
43  typedef Eigen::Matrix<double, 6, 6 > Matrix6d;
44  typedef Eigen::Matrix<double, 6, Eigen::Dynamic> Matrix6Xd;
45 
46 public:
53  ChainIdSolver_Vereshchagin(const Chain& chain, Twist root_acc, unsigned int nc);
54 
56  {
57  };
58 
70  int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques);
71 
72  /*
73  //Returns cartesian positions of links in base coordinates
74  void getLinkCartesianPose(Frames& x_base);
75  //Returns cartesian velocities of links in base coordinates
76  void getLinkCartesianVelocity(Twists& xDot_base);
77  //Returns cartesian acceleration of links in base coordinates
78  void getLinkCartesianAcceleration(Twists& xDotDot_base);
79  //Returns cartesian postions of links in link tip coordinates
80  void getLinkPose(Frames& x_local);
81  //Returns cartesian velocities of links in link tip coordinates
82  void getLinkVelocity(Twists& xDot_local);
83  //Returns cartesian acceleration of links in link tip coordinates
84  void getLinkAcceleration(Twists& xDotdot_local);
85  //Acceleration energy due to unit constraint forces at the end-effector
86  void getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M);
87  //Acceleration energy due to arm configuration: bias force plus input joint torques
88  void getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G);
89 
90  void getLinkUnitForceMatrix(Matrix6Xd& E_tilde);
91 
92  void getLinkBiasForceMatrix(Wrenches& R_tilde);
93 
94  void getJointBiasAcceleration(JntArray &bias_q_dotdot);
95  */
96 private:
101  void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext);
106  void downwards_sweep(const Jacobian& alfa, const JntArray& torques);
111  void constraint_calculation(const JntArray& beta);
116  void final_upwards_sweep(JntArray &q_dotdot, JntArray &torques);
117 
118 private:
120  unsigned int nj;
121  unsigned int ns;
122  unsigned int nc;
126  Eigen::MatrixXd M_0_inverse;
127  Eigen::MatrixXd Um;
128  Eigen::MatrixXd Vm;
129  JntArray beta_N;
130  Eigen::VectorXd nu;
131  Eigen::VectorXd nu_sum;
132  Eigen::VectorXd Sm;
133  Eigen::VectorXd tmpm;
136 
138  {
139  Frame F; //local pose with respect to previous link in segments coordinates
140  Frame F_base; // pose of a segment in root coordinates
141  Twist Z; //Unit twist
142  Twist v; //twist
143  Twist acc; //acceleration twist
144  Wrench U; //wrench p of the bias forces (in cartesian space)
145  Wrench R; //wrench p of the bias forces
146  Wrench R_tilde; //vector of wrench p of the bias forces (new) in matrix form
147  Twist C; //constraint
148  Twist A; //constraint
149  ArticulatedBodyInertia H; //I (expressed in 6*6 matrix)
150  ArticulatedBodyInertia P; //I (expressed in 6*6 matrix)
151  ArticulatedBodyInertia P_tilde; //I (expressed in 6*6 matrix)
152  Wrench PZ; //vector U[i] = I_A[i]*S[i]
153  Wrench PC; //vector E[i] = I_A[i]*c[i]
154  double D; //vector D[i] = S[i]^T*U[i]
155  Matrix6Xd E; //matrix with virtual unit constraint force due to acceleration constraints
156  Matrix6Xd E_tilde;
157  Eigen::MatrixXd M; //acceleration energy already generated at link i
158  Eigen::VectorXd G; //magnitude of the constraint forces already generated at link i
159  Eigen::VectorXd EZ; //K[i] = Etiltde'*Z
160  double nullspaceAccComp; //Azamat: constribution of joint space u[i] forces to joint space acceleration
161  double constAccComp; //Azamat: constribution of joint space constraint forces to joint space acceleration
162  double biasAccComp; //Azamat: constribution of joint space bias forces to joint space acceleration
163  double totalBias; //Azamat: R+PC (centrepital+coriolis) in joint subspace
164  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
165 
166  segment_info(unsigned int nc):
167  D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0)
168  {
169  E.resize(6, nc);
170  E_tilde.resize(6, nc);
171  G.resize(nc);
172  M.resize(nc, nc);
173  EZ.resize(nc);
174  E.setZero();
175  E_tilde.setZero();
176  M.setZero();
177  G.setZero();
178  EZ.setZero();
179  };
180  };
181 
182  std::vector<segment_info> results;
183 
184 };
185 }
186 
187 #endif
Matrix6Xd E_tilde
Definition: chainidsolver_vereshchagin.hpp:156
double totalBias
Definition: chainidsolver_vereshchagin.hpp:163
ArticulatedBodyInertia P
Definition: chainidsolver_vereshchagin.hpp:150
std::vector< Frame > Frames
Definition: chainidsolver_vereshchagin.hpp:41
Eigen::VectorXd G
Definition: chainidsolver_vereshchagin.hpp:158
Eigen::VectorXd tmpm
Definition: chainidsolver_vereshchagin.hpp:133
Wrench U
Definition: chainidsolver_vereshchagin.hpp:144
ArticulatedBodyInertia P_tilde
Definition: chainidsolver_vereshchagin.hpp:151
std::vector< Wrench > Wrenches
Definition: chainidsolver.hpp:32
Twist acc_root
Definition: chainidsolver_vereshchagin.hpp:123
Eigen::MatrixXd Um
Definition: chainidsolver_vereshchagin.hpp:127
double u
Definition: chainidsolver_vereshchagin.hpp:164
Wrench R
Definition: chainidsolver_vereshchagin.hpp:145
Definition: jacobian.hpp:35
Frame F
Definition: chainidsolver_vereshchagin.hpp:139
Chain chain
Definition: chainidsolver_vereshchagin.hpp:119
This class encapsulates a serial kinematic interconnection structure.
Definition: chain.hpp:35
Eigen::VectorXd EZ
Definition: chainidsolver_vereshchagin.hpp:159
double constAccComp
Definition: chainidsolver_vereshchagin.hpp:161
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: chainidsolver_vereshchagin.hpp:42
Twist v
Definition: chainidsolver_vereshchagin.hpp:142
JntArray beta_N
Definition: chainidsolver_vereshchagin.hpp:129
Eigen::VectorXd Sm
Definition: chainidsolver_vereshchagin.hpp:132
Wrench PC
Definition: chainidsolver_vereshchagin.hpp:153
Wrench PZ
Definition: chainidsolver_vereshchagin.hpp:152
double D
Definition: chainidsolver_vereshchagin.hpp:154
ArticulatedBodyInertia H
Definition: chainidsolver_vereshchagin.hpp:149
Jacobian alfa_N2
Definition: chainidsolver_vereshchagin.hpp:125
Wrench R_tilde
Definition: chainidsolver_vereshchagin.hpp:146
unsigned int nj
Definition: chainidsolver_vereshchagin.hpp:120
represents both translational and rotational velocities.
Definition: frames.hpp:720
Twist acc
Definition: chainidsolver_vereshchagin.hpp:143
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:46
Definition: articulatedbodyinertia.cpp:28
6D Inertia of a articulated body
Definition: articulatedbodyinertia.hpp:40
~ChainIdSolver_Vereshchagin()
Definition: chainidsolver_vereshchagin.hpp:55
Twist Z
Definition: chainidsolver_vereshchagin.hpp:141
std::vector< segment_info > results
Definition: chainidsolver_vereshchagin.hpp:182
void constraint_calculation(const JntArray &beta)
This method calculates constraint force magnitudes.
Definition: chainidsolver_vereshchagin.cpp:241
Jacobian alfa_N
Definition: chainidsolver_vereshchagin.hpp:124
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:276
Wrench qdotdot_sum
Definition: chainidsolver_vereshchagin.hpp:134
Twist A
Definition: chainidsolver_vereshchagin.hpp:148
Frame F_base
Definition: chainidsolver_vereshchagin.hpp:140
double biasAccComp
Definition: chainidsolver_vereshchagin.hpp:162
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
Eigen::MatrixXd M
Definition: chainidsolver_vereshchagin.hpp:157
void downwards_sweep(const Jacobian &alfa, const JntArray &torques)
This method is a force balance sweep.
Definition: chainidsolver_vereshchagin.cpp:125
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6Xd
Definition: chainidsolver_vereshchagin.hpp:44
unsigned int nc
Definition: chainidsolver_vereshchagin.hpp:122
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:64
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
Twist C
Definition: chainidsolver_vereshchagin.hpp:147
represents both translational and rotational acceleration.
Definition: frames.hpp:878
double nullspaceAccComp
Definition: chainidsolver_vereshchagin.hpp:160
Eigen::VectorXd nu_sum
Definition: chainidsolver_vereshchagin.hpp:131
Eigen::MatrixXd M_0_inverse
Definition: chainidsolver_vereshchagin.hpp:126
Definition: chainidsolver_vereshchagin.hpp:137
Eigen::VectorXd nu
Definition: chainidsolver_vereshchagin.hpp:130
Frame F_total
Definition: chainidsolver_vereshchagin.hpp:135
std::vector< Twist > Twists
Definition: chainidsolver_vereshchagin.hpp:40
unsigned int ns
Definition: chainidsolver_vereshchagin.hpp:121
Matrix6Xd E
Definition: chainidsolver_vereshchagin.hpp:155
Dynamics calculations by constraints based on Vereshchagin 1989.
Definition: chainidsolver_vereshchagin.hpp:38
Eigen::Matrix< double, 6, 6 > Matrix6d
Definition: chainidsolver_vereshchagin.hpp:43
segment_info(unsigned int nc)
Definition: chainidsolver_vereshchagin.hpp:166
Eigen::MatrixXd Vm
Definition: chainidsolver_vereshchagin.hpp:128