chainidsolver_vereshchagin.hpp
Go to the documentation of this file.
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


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14