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 {
00037 typedef std::vector<Twist> Twists;
00038 typedef std::vector<Frame> Frames;
00039 typedef Eigen::Matrix<double, 6, 1 > Vector6d;
00040 typedef Eigen::Matrix<double, 6, 6 > Matrix6d;
00041 typedef Eigen::Matrix<double, 6, Eigen::Dynamic> Matrix6Xd;
00042 
00043 class ChainIdSolver_Vereshchagin
00044 {
00045 public:
00052     ChainIdSolver_Vereshchagin(const Chain& chain, Twist root_acc, unsigned int nc);
00053 
00054     ~ChainIdSolver_Vereshchagin()
00055     {
00056     };
00057 
00069     int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques);
00070 
00071     /*
00072     //Returns cartesian positions of links in base coordinates
00073     void getLinkCartesianPose(Frames& x_base);
00074     //Returns cartesian velocities of links in base coordinates
00075     void getLinkCartesianVelocity(Twists& xDot_base);
00076     //Returns cartesian acceleration of links in base coordinates
00077     void getLinkCartesianAcceleration(Twists& xDotDot_base);
00078     //Returns cartesian postions of links in link tip coordinates
00079     void getLinkPose(Frames& x_local);
00080     //Returns cartesian velocities of links in link tip coordinates
00081     void getLinkVelocity(Twists& xDot_local);
00082     //Returns cartesian acceleration of links in link tip coordinates
00083     void getLinkAcceleration(Twists& xDotdot_local);
00084     //Acceleration energy due to unit constraint forces at the end-effector
00085     void getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M);
00086     //Acceleration energy due to arm configuration: bias force plus input joint torques
00087     void getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G);
00088 
00089     void getLinkUnitForceMatrix(Matrix6Xd& E_tilde);
00090 
00091     void getLinkBiasForceMatrix(Wrenches& R_tilde);
00092 
00093     void getJointBiasAcceleration(JntArray &bias_q_dotdot);
00094     */
00095 private:
00100     void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext);
00105     void downwards_sweep(const Jacobian& alfa, const JntArray& torques);
00110     void constraint_calculation(const JntArray& beta);
00115     void final_upwards_sweep(JntArray &q_dotdot, JntArray &torques);
00116 
00117 private:
00118     Chain chain;
00119     unsigned int nj;
00120     unsigned int ns;
00121     unsigned int nc;
00122     Twist acc_root;
00123     Jacobian alfa_N;
00124     Jacobian alfa_N2;
00125     Eigen::MatrixXd M_0_inverse;
00126     Eigen::MatrixXd Um;
00127     Eigen::MatrixXd Vm;
00128     JntArray beta_N;
00129     Eigen::VectorXd nu;
00130     Eigen::VectorXd nu_sum;
00131     Eigen::VectorXd Sm;
00132     Eigen::VectorXd tmpm;
00133     Wrench qdotdot_sum;
00134     Frame F_total;
00135 
00136     struct segment_info
00137     {
00138         Frame F; //local pose with respect to previous link in segments coordinates
00139         Frame F_base; // pose of a segment in root coordinates
00140         Twist Z; //Unit twist
00141         Twist v; //twist
00142         Twist acc; //acceleration twist
00143         Wrench U; //wrench p of the bias forces (in cartesian space)
00144         Wrench R; //wrench p of the bias forces
00145         Wrench R_tilde; //vector of wrench p of the bias forces (new) in matrix form
00146         Twist C; //constraint
00147         Twist A; //constraint
00148         ArticulatedBodyInertia H; //I (expressed in 6*6 matrix)
00149         ArticulatedBodyInertia P; //I (expressed in 6*6 matrix)
00150         ArticulatedBodyInertia P_tilde; //I (expressed in 6*6 matrix)
00151         Wrench PZ; //vector U[i] = I_A[i]*S[i]
00152         Wrench PC; //vector E[i] = I_A[i]*c[i]
00153         double D; //vector D[i] = S[i]^T*U[i]
00154         Matrix6Xd E; //matrix with virtual unit constraint force due to acceleration constraints
00155         Matrix6Xd E_tilde;
00156         Eigen::MatrixXd M; //acceleration energy already generated at link i
00157         Eigen::VectorXd G; //magnitude of the constraint forces already generated at link i
00158         Eigen::VectorXd EZ; //K[i] = Etiltde'*Z
00159         double nullspaceAccComp; //Azamat: constribution of joint space u[i] forces to joint space acceleration
00160         double constAccComp; //Azamat: constribution of joint space constraint forces to joint space acceleration
00161         double biasAccComp; //Azamat: constribution of joint space bias forces to joint space acceleration
00162         double totalBias; //Azamat: R+PC (centrepital+coriolis) in joint subspace
00163         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
00164 
00165         segment_info(unsigned int nc)
00166         {
00167             E.resize(6, nc);
00168             E_tilde.resize(6, nc);
00169             G.resize(nc);
00170             M.resize(nc, nc);
00171             EZ.resize(nc);
00172             E.setZero();
00173             E_tilde.setZero();
00174             M.setZero();
00175             G.setZero();
00176             EZ.setZero();
00177         };
00178     };
00179 
00180     std::vector<segment_info> results;
00181 
00182 };
00183 }
00184 
00185 #endif


orocos_kdl
Author(s): Ruben Smits, Erwin Aertbelien, Orocos Developers
autogenerated on Sat Dec 28 2013 17:17:25