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
162  Matrix6Xd E_tilde;
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):
173  D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0)
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
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Definition: chain.hpp:35
Eigen::Matrix< double, 6, 1 > Vector6d
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
represents both translational and rotational velocities.
Definition: frames.hpp:720
int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian &alfa, const JntArray &beta, const Wrenches &f_ext, JntArray &torques)
6D Inertia of a articulated body
void constraint_calculation(const JntArray &beta)
void final_upwards_sweep(JntArray &q_dotdot, JntArray &torques)
ChainIdSolver_Vereshchagin(const Chain &chain, Twist root_acc, unsigned int nc)
void downwards_sweep(const Jacobian &alfa, const JntArray &torques)
std::vector< Wrench > Wrenches
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6Xd
void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext)
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
represents both translational and rotational acceleration.
Definition: frames.hpp:878
std::vector< segment_info, Eigen::aligned_allocator< segment_info > > results
Dynamics calculations by constraints based on Vereshchagin 1989. for a chain. This class creates inst...
Eigen::Matrix< double, 6, 6 > Matrix6d


orocos_kdl
Author(s):
autogenerated on Tue Sep 1 2020 03:18:51