Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #ifndef KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP
00023 #define KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP
00024
00025 #include "chainidsolver.hpp"
00026 #include "frames.hpp"
00027 #include "articulatedbodyinertia.hpp"
00028
00029 #include<Eigen/StdVector>
00030
00031 namespace KDL
00032 {
00040 class ChainIdSolver_Vereshchagin : KDL::SolverI
00041 {
00042 typedef std::vector<Twist> Twists;
00043 typedef std::vector<Frame> Frames;
00044 typedef Eigen::Matrix<double, 6, 1 > Vector6d;
00045 typedef Eigen::Matrix<double, 6, 6 > Matrix6d;
00046 typedef Eigen::Matrix<double, 6, Eigen::Dynamic> Matrix6Xd;
00047
00048 public:
00055 ChainIdSolver_Vereshchagin(const Chain& chain, Twist root_acc, unsigned int nc);
00056
00057 ~ChainIdSolver_Vereshchagin()
00058 {
00059 };
00060
00074 int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques);
00075
00077 virtual void updateInternalDataStructures();
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102 private:
00107 void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext);
00112 void downwards_sweep(const Jacobian& alfa, const JntArray& torques);
00117 void constraint_calculation(const JntArray& beta);
00122 void final_upwards_sweep(JntArray &q_dotdot, JntArray &torques);
00123
00124 private:
00125 const Chain& chain;
00126 unsigned int nj;
00127 unsigned int ns;
00128 unsigned int nc;
00129 Twist acc_root;
00130 Jacobian alfa_N;
00131 Jacobian alfa_N2;
00132 Eigen::MatrixXd M_0_inverse;
00133 Eigen::MatrixXd Um;
00134 Eigen::MatrixXd Vm;
00135 JntArray beta_N;
00136 Eigen::VectorXd nu;
00137 Eigen::VectorXd nu_sum;
00138 Eigen::VectorXd Sm;
00139 Eigen::VectorXd tmpm;
00140 Wrench qdotdot_sum;
00141 Frame F_total;
00142
00143 struct segment_info
00144 {
00145 Frame F;
00146 Frame F_base;
00147 Twist Z;
00148 Twist v;
00149 Twist acc;
00150 Wrench U;
00151 Wrench R;
00152 Wrench R_tilde;
00153 Twist C;
00154 Twist A;
00155 ArticulatedBodyInertia H;
00156 ArticulatedBodyInertia P;
00157 ArticulatedBodyInertia P_tilde;
00158 Wrench PZ;
00159 Wrench PC;
00160 double D;
00161 Matrix6Xd E;
00162 Matrix6Xd E_tilde;
00163 Eigen::MatrixXd M;
00164 Eigen::VectorXd G;
00165 Eigen::VectorXd EZ;
00166 double nullspaceAccComp;
00167 double constAccComp;
00168 double biasAccComp;
00169 double totalBias;
00170 double u;
00171
00172 segment_info(unsigned int nc):
00173 D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0)
00174 {
00175 E.resize(6, nc);
00176 E_tilde.resize(6, nc);
00177 G.resize(nc);
00178 M.resize(nc, nc);
00179 EZ.resize(nc);
00180 E.setZero();
00181 E_tilde.setZero();
00182 M.setZero();
00183 G.setZero();
00184 EZ.setZero();
00185 };
00186 };
00187
00188 std::vector<segment_info, Eigen::aligned_allocator<segment_info> > results;
00189
00190 };
00191 }
00192
00193 #endif