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 #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     //Returns cartesian positions of links in base coordinates
00080     void getLinkCartesianPose(Frames& x_base);
00081     //Returns cartesian velocities of links in base coordinates
00082     void getLinkCartesianVelocity(Twists& xDot_base);
00083     //Returns cartesian acceleration of links in base coordinates
00084     void getLinkCartesianAcceleration(Twists& xDotDot_base);
00085     //Returns cartesian positions of links in link tip coordinates
00086     void getLinkPose(Frames& x_local);
00087     //Returns cartesian velocities of links in link tip coordinates
00088     void getLinkVelocity(Twists& xDot_local);
00089     //Returns cartesian acceleration of links in link tip coordinates
00090     void getLinkAcceleration(Twists& xDotdot_local);
00091     //Acceleration energy due to unit constraint forces at the end-effector
00092     void getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M);
00093     //Acceleration energy due to arm configuration: bias force plus input joint torques
00094     void getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G);
00095 
00096     void getLinkUnitForceMatrix(Matrix6Xd& E_tilde);
00097 
00098     void getLinkBiasForceMatrix(Wrenches& R_tilde);
00099 
00100     void getJointBiasAcceleration(JntArray &bias_q_dotdot);
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; //local pose with respect to previous link in segments coordinates
00146         Frame F_base; // pose of a segment in root coordinates
00147         Twist Z; //Unit twist
00148         Twist v; //twist
00149         Twist acc; //acceleration twist
00150         Wrench U; //wrench p of the bias forces (in cartesian space)
00151         Wrench R; //wrench p of the bias forces
00152         Wrench R_tilde; //vector of wrench p of the bias forces (new) in matrix form
00153         Twist C; //constraint
00154         Twist A; //constraint
00155         ArticulatedBodyInertia H; //I (expressed in 6*6 matrix)
00156         ArticulatedBodyInertia P; //I (expressed in 6*6 matrix)
00157         ArticulatedBodyInertia P_tilde; //I (expressed in 6*6 matrix)
00158         Wrench PZ; //vector U[i] = I_A[i]*S[i]
00159         Wrench PC; //vector E[i] = I_A[i]*c[i]
00160         double D; //vector D[i] = S[i]^T*U[i]
00161         Matrix6Xd E; //matrix with virtual unit constraint force due to acceleration constraints
00162         Matrix6Xd E_tilde;
00163         Eigen::MatrixXd M; //acceleration energy already generated at link i
00164         Eigen::VectorXd G; //magnitude of the constraint forces already generated at link i
00165         Eigen::VectorXd EZ; //K[i] = Etiltde'*Z
00166         double nullspaceAccComp; //Azamat: constribution of joint space u[i] forces to joint space acceleration
00167         double constAccComp; //Azamat: constribution of joint space constraint forces to joint space acceleration
00168         double biasAccComp; //Azamat: constribution of joint space bias forces to joint space acceleration
00169         double totalBias; //Azamat: R+PC (centrepital+coriolis) in joint subspace
00170         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
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


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:22