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
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100 private:
00105 void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext);
00110 void downwards_sweep(const Jacobian& alfa, const JntArray& torques);
00115 void constraint_calculation(const JntArray& beta);
00120 void final_upwards_sweep(JntArray &q_dotdot, JntArray &torques);
00121
00122 private:
00123 Chain chain;
00124 unsigned int nj;
00125 unsigned int ns;
00126 unsigned int nc;
00127 Twist acc_root;
00128 Jacobian alfa_N;
00129 Jacobian alfa_N2;
00130 Eigen::MatrixXd M_0_inverse;
00131 Eigen::MatrixXd Um;
00132 Eigen::MatrixXd Vm;
00133 JntArray beta_N;
00134 Eigen::VectorXd nu;
00135 Eigen::VectorXd nu_sum;
00136 Eigen::VectorXd Sm;
00137 Eigen::VectorXd tmpm;
00138 Wrench qdotdot_sum;
00139 Frame F_total;
00140
00141 struct segment_info
00142 {
00143 Frame F;
00144 Frame F_base;
00145 Twist Z;
00146 Twist v;
00147 Twist acc;
00148 Wrench U;
00149 Wrench R;
00150 Wrench R_tilde;
00151 Twist C;
00152 Twist A;
00153 ArticulatedBodyInertia H;
00154 ArticulatedBodyInertia P;
00155 ArticulatedBodyInertia P_tilde;
00156 Wrench PZ;
00157 Wrench PC;
00158 double D;
00159 Matrix6Xd E;
00160 Matrix6Xd E_tilde;
00161 Eigen::MatrixXd M;
00162 Eigen::VectorXd G;
00163 Eigen::VectorXd EZ;
00164 double nullspaceAccComp;
00165 double constAccComp;
00166 double biasAccComp;
00167 double totalBias;
00168 double u;
00169
00170 segment_info(unsigned int nc):
00171 D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0)
00172 {
00173 E.resize(6, nc);
00174 E_tilde.resize(6, nc);
00175 G.resize(nc);
00176 M.resize(nc, nc);
00177 EZ.resize(nc);
00178 E.setZero();
00179 E_tilde.setZero();
00180 M.setZero();
00181 G.setZero();
00182 EZ.setZero();
00183 };
00184 };
00185
00186 std::vector<segment_info, Eigen::aligned_allocator<segment_info> > results;
00187
00188 };
00189 }
00190
00191 #endif