chainhdsolver_vereshchagin.hpp

// Copyright  (C)  2009  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
// Author: Herman Bruyninckx
// Author: Azamat Shakhimardanov
// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
// URL: http://www.orocos.org/kdl
 
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
 
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.
 
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
#ifndef KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP
#define KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP
 
#include "chainidsolver.hpp"
#include "frames.hpp"
#include "articulatedbodyinertia.hpp"
 
#include<Eigen/StdVector>
 
namespace KDL
{
 
class ChainHdSolver_Vereshchagin : public KDL::SolverI
{
    typedef std::vector<Twist> Twists;
    typedef std::vector<Frame> Frames;
    typedef Eigen::Matrix<double, 6, 1 > Vector6d;
    typedef Eigen::Matrix<double, 6, 6 > Matrix6d;
    typedef Eigen::Matrix<double, 6, Eigen::Dynamic> Matrix6Xd;
 
public:
    ChainHdSolver_Vereshchagin(const Chain& chain, const Twist &root_acc, const unsigned int nc);
 
    ~ChainHdSolver_Vereshchagin()
    {
    };

    int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, const JntArray &ff_torques, JntArray &constraint_torques);

    virtual void updateInternalDataStructures();

    int setDriverWeights(const Eigen::VectorXd& w_f_ext, const Eigen::VectorXd& w_ff_torques);
 
    //Returns cartesian acceleration of links in base coordinates
    void getTransformedLinkAcceleration(Twists& x_dotdot);
 
    // Returns total torque acting on each joint (constraints + nature + external forces)
    void getTotalTorque(JntArray &total_tau);
 
    // Returns magnitude of the constraint forces acting on the end-effector: Lagrange Multiplier
    void getContraintForceMagnitude(Eigen::VectorXd &nu_);
 
    /*
    //Returns cartesian positions of links in base coordinates
    void getLinkCartesianPose(Frames& x_base);
    //Returns cartesian velocities of links in base coordinates
    void getLinkCartesianVelocity(Twists& xDot_base);
    //Returns cartesian acceleration of links in base coordinates
    void getLinkCartesianAcceleration(Twists& xDotDot_base);
    //Returns cartesian positions of links in link tip coordinates
    void getLinkPose(Frames& x_local);
    //Returns cartesian velocities of links in link tip coordinates
    void getLinkVelocity(Twists& xDot_local);
    //Returns cartesian acceleration of links in link tip coordinates
    void getLinkAcceleration(Twists& xDotdot_local);
    //Acceleration energy due to unit constraint forces at the end-effector
    void getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M);
    //Acceleration energy due to arm configuration: bias force plus input joint torques
    void getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G);
 
    void getLinkUnitForceMatrix(Matrix6Xd& E_tilde);
 
    void getLinkBiasForceMatrix(Wrenches& R_tilde);
 
    void getJointBiasAcceleration(JntArray &bias_q_dotdot);
    */
private:
    void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext);
    void downwards_sweep(const Jacobian& alfa, const JntArray& ff_torques);
    void constraint_calculation(const JntArray& beta);
    void final_upwards_sweep(JntArray &q_dotdot, JntArray &constraint_torques);
 
private:
    const Chain& chain;
    unsigned int nj;
    unsigned int ns;
    unsigned int nc;
    Twist acc_root;
    Jacobian alfa_N;
    Jacobian alfa_N2;
    Eigen::MatrixXd M_0_inverse;
    Eigen::MatrixXd Um;
    Eigen::MatrixXd Vm;
    JntArray beta_N;
    Eigen::VectorXd nu;
    Eigen::VectorXd nu_sum;
    Eigen::VectorXd Sm;
    Eigen::VectorXd tmpm;
    Eigen::VectorXd total_torques; // all the contributions that are felt at the joint: constraints + nature + external forces
    Wrench qdotdot_sum;
    Frame F_total;
    Eigen::VectorXd w_f_ext, w_ff_torques; //per-constraint-direction driver weights, see setDriverWeights
    Matrix6Xd alpha_root; //alfa in root coordinates, torques above forces
 
    struct segment_info
    {
        Frame F; //local pose with respect to previous link in segments coordinates
        Frame F_base; // pose of a segment in root coordinates
        Twist Z; //Unit twist
        Twist v; //twist
        Twist acc; //acceleration twist
        Wrench U; //wrench p of the bias forces (in cartesian space)
        Wrench R; //wrench p of the bias forces
        Wrench R_tilde; //vector of wrench p of the bias forces (new) in matrix form
        Twist C; //constraint
        Twist A; //constraint
        ArticulatedBodyInertia H; //I (expressed in 6*6 matrix)
        ArticulatedBodyInertia P; //I (expressed in 6*6 matrix)
        ArticulatedBodyInertia P_tilde; //I (expressed in 6*6 matrix)
        Wrench PZ; //vector U[i] = I_A[i]*S[i]
        Wrench PC; //vector E[i] = I_A[i]*c[i]
        double D; //vector D[i] = S[i]^T*U[i]
        Matrix6Xd E; //matrix with virtual unit constraint force due to acceleration constraints
        Matrix6Xd E_tilde;
        Eigen::MatrixXd M; //acceleration energy already generated at link i
        Eigen::VectorXd G; //magnitude of the constraint forces already generated at link i
        Eigen::VectorXd EZ; //K[i] = Etiltde'*Z
        double nullspaceAccComp; //Azamat: constribution of joint space u[i] forces to joint space acceleration
        double constAccComp; //Azamat: constribution of joint space constraint forces to joint space acceleration
        double biasAccComp; //Azamat: constribution of joint space bias forces to joint space acceleration
        double totalBias; //Azamat: R+PC (centrepital+coriolis) in joint subspace
        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
 
        //Per-driver contributions, tracked alongside the totals above so that
        //constraint_calculation() can credit each driver only partially.
        //Sum of the channels plus the rigid-body bias terms == the totals.
        Wrench U_fext, R_fext, R_tilde_fext;
        Wrench U_ff, R_ff, R_tilde_ff;
        double u_fext, u_ff;
        Eigen::VectorXd G_fext, G_ff;
 
        segment_info(unsigned int nc):
            D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0),
            u_fext(0),u_ff(0)
        {
            E.resize(6, nc);
            E_tilde.resize(6, nc);
            G.resize(nc);
            M.resize(nc, nc);
            EZ.resize(nc);
            E.setZero();
            E_tilde.setZero();
            M.setZero();
            G.setZero();
            EZ.setZero();
            G_fext.resize(nc);
            G_ff.resize(nc);
            G_fext.setZero();
            G_ff.setZero();
        };
    };
 
    std::vector<segment_info, Eigen::aligned_allocator<segment_info> > results;
 
};
}
 
#endif // KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP