00001 // Copyright (C) 2009, 2011 00002 00003 // Version: 1.0 00004 // Author: Ruben Smits, Herman Bruyninckx, Azamat Shakhimardanov 00005 // Maintainer: Ruben Smits, Azamat Shakhimardanov 00006 // URL: http://www.orocos.org/kdl 00007 00008 // This library is free software; you can redistribute it and/or 00009 // modify it under the terms of the GNU Lesser General Public 00010 // License as published by the Free Software Foundation; either 00011 // version 2.1 of the License, or (at your option) any later version. 00012 00013 // This library is distributed in the hope that it will be useful, 00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00016 // Lesser General Public License for more details. 00017 00018 // You should have received a copy of the GNU Lesser General Public 00019 // License along with this library; if not, write to the Free Software 00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 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 namespace KDL 00030 { 00038 class ChainIdSolver_Vereshchagin 00039 { 00040 typedef std::vector<Twist> Twists; 00041 typedef std::vector<Frame> Frames; 00042 typedef Eigen::Matrix<double, 6, 1 > Vector6d; 00043 typedef Eigen::Matrix<double, 6, 6 > Matrix6d; 00044 typedef Eigen::Matrix<double, 6, Eigen::Dynamic> Matrix6Xd; 00045 00046 public: 00053 ChainIdSolver_Vereshchagin(const Chain& chain, Twist root_acc, unsigned int nc); 00054 00055 ~ChainIdSolver_Vereshchagin() 00056 { 00057 }; 00058 00070 int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques); 00071 00072 /* 00073 //Returns cartesian positions of links in base coordinates 00074 void getLinkCartesianPose(Frames& x_base); 00075 //Returns cartesian velocities of links in base coordinates 00076 void getLinkCartesianVelocity(Twists& xDot_base); 00077 //Returns cartesian acceleration of links in base coordinates 00078 void getLinkCartesianAcceleration(Twists& xDotDot_base); 00079 //Returns cartesian postions of links in link tip coordinates 00080 void getLinkPose(Frames& x_local); 00081 //Returns cartesian velocities of links in link tip coordinates 00082 void getLinkVelocity(Twists& xDot_local); 00083 //Returns cartesian acceleration of links in link tip coordinates 00084 void getLinkAcceleration(Twists& xDotdot_local); 00085 //Acceleration energy due to unit constraint forces at the end-effector 00086 void getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M); 00087 //Acceleration energy due to arm configuration: bias force plus input joint torques 00088 void getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G); 00089 00090 void getLinkUnitForceMatrix(Matrix6Xd& E_tilde); 00091 00092 void getLinkBiasForceMatrix(Wrenches& R_tilde); 00093 00094 void getJointBiasAcceleration(JntArray &bias_q_dotdot); 00095 */ 00096 private: 00101 void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext); 00106 void downwards_sweep(const Jacobian& alfa, const JntArray& torques); 00111 void constraint_calculation(const JntArray& beta); 00116 void final_upwards_sweep(JntArray &q_dotdot, JntArray &torques); 00117 00118 private: 00119 Chain chain; 00120 unsigned int nj; 00121 unsigned int ns; 00122 unsigned int nc; 00123 Twist acc_root; 00124 Jacobian alfa_N; 00125 Jacobian alfa_N2; 00126 Eigen::MatrixXd M_0_inverse; 00127 Eigen::MatrixXd Um; 00128 Eigen::MatrixXd Vm; 00129 JntArray beta_N; 00130 Eigen::VectorXd nu; 00131 Eigen::VectorXd nu_sum; 00132 Eigen::VectorXd Sm; 00133 Eigen::VectorXd tmpm; 00134 Wrench qdotdot_sum; 00135 Frame F_total; 00136 00137 struct segment_info 00138 { 00139 Frame F; //local pose with respect to previous link in segments coordinates 00140 Frame F_base; // pose of a segment in root coordinates 00141 Twist Z; //Unit twist 00142 Twist v; //twist 00143 Twist acc; //acceleration twist 00144 Wrench U; //wrench p of the bias forces (in cartesian space) 00145 Wrench R; //wrench p of the bias forces 00146 Wrench R_tilde; //vector of wrench p of the bias forces (new) in matrix form 00147 Twist C; //constraint 00148 Twist A; //constraint 00149 ArticulatedBodyInertia H; //I (expressed in 6*6 matrix) 00150 ArticulatedBodyInertia P; //I (expressed in 6*6 matrix) 00151 ArticulatedBodyInertia P_tilde; //I (expressed in 6*6 matrix) 00152 Wrench PZ; //vector U[i] = I_A[i]*S[i] 00153 Wrench PC; //vector E[i] = I_A[i]*c[i] 00154 double D; //vector D[i] = S[i]^T*U[i] 00155 Matrix6Xd E; //matrix with virtual unit constraint force due to acceleration constraints 00156 Matrix6Xd E_tilde; 00157 Eigen::MatrixXd M; //acceleration energy already generated at link i 00158 Eigen::VectorXd G; //magnitude of the constraint forces already generated at link i 00159 Eigen::VectorXd EZ; //K[i] = Etiltde'*Z 00160 double nullspaceAccComp; //Azamat: constribution of joint space u[i] forces to joint space acceleration 00161 double constAccComp; //Azamat: constribution of joint space constraint forces to joint space acceleration 00162 double biasAccComp; //Azamat: constribution of joint space bias forces to joint space acceleration 00163 double totalBias; //Azamat: R+PC (centrepital+coriolis) in joint subspace 00164 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 00165 00166 segment_info(unsigned int nc): 00167 D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0) 00168 { 00169 E.resize(6, nc); 00170 E_tilde.resize(6, nc); 00171 G.resize(nc); 00172 M.resize(nc, nc); 00173 EZ.resize(nc); 00174 E.setZero(); 00175 E_tilde.setZero(); 00176 M.setZero(); 00177 G.setZero(); 00178 EZ.setZero(); 00179 }; 00180 }; 00181 00182 std::vector<segment_info> results; 00183 00184 }; 00185 } 00186 00187 #endif