C++ Jacobian类代码示例

 bool Equal(const Jacobian& a,const Jacobian& b,double eps)
 {
     if(a.rows()==b.rows()&&a.columns()==b.columns()){
         return a.data.isApprox(b.data,eps);
     }else
         return false;
 }

int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Jacobian& jdot, int seg_nr)
{
    unsigned int segmentNr;
    if(seg_nr<0)
        segmentNr=chain.getNrOfSegments();
    else
        segmentNr = seg_nr;

    //Initialize Jacobian to zero since only segmentNr columns are computed
    SetToZero(jdot) ;

    if(q_in.q.rows()!=chain.getNrOfJoints()||nr_of_unlocked_joints_!=jdot.columns())
        return (error = E_JAC_DOT_FAILED);
    else if(segmentNr>chain.getNrOfSegments())
        return (error = E_JAC_DOT_FAILED);

    // First compute the jacobian in the Hybrid representation
    jac_solver_.JntToJac(q_in.q,jac_,segmentNr);

    // Change the reference frame and/or the reference point
    switch(representation_)
    {
        case HYBRID:
            // Do Nothing as it is the default in KDL;
            break;
        case BODYFIXED:
            // Ref Frame {ee}, Ref Point {ee}
            fk_solver_.JntToCart(q_in.q,F_bs_ee_,segmentNr);
            jac_.changeBase(F_bs_ee_.M.Inverse());
            break;
        case INTERTIAL:
            // Ref Frame {bs}, Ref Point {bs}
            fk_solver_.JntToCart(q_in.q,F_bs_ee_,segmentNr);
            jac_.changeRefPoint(-F_bs_ee_.p);
            break;
        default:
            return (error = E_JAC_DOT_FAILED);
    }

    // Let's compute Jdot in the corresponding representation
    int k=0;
    for(unsigned int i=0;i<segmentNr;++i)
    {
        //Only increase joint nr if the segment has a joint
        if(chain.getSegment(i).getJoint().getType()!=Joint::None){

            for(unsigned int j=0;j<chain.getNrOfJoints();++j)
            {
                // Column J is the sum of all partial derivatives  ref (41)
                if(!locked_joints_[j])
                    jac_dot_k_ += getPartialDerivative(jac_,j,k,representation_) * q_in.qdot(j);
            }
            jdot.setColumn(k++,jac_dot_k_);
            SetToZero(jac_dot_k_);
        }
    }

    return (error = E_NOERROR);
}

 bool changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest)
 {
     if(src1.columns()!=dest.columns())
         return false;
     for(unsigned int i=0;i<src1.columns();i++)
         dest.setColumn(i,rot*src1.getColumn(i));;
     return true;
 }

 bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest)
 {
     if(src1.columns()!=dest.columns())
         return false;
     for(unsigned int i=0;i<src1.columns();i++)
         dest.setColumn(i,frame*src1.getColumn(i));
     return true;
 }

 bool changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest)
 {
     if(src1.columns()!=dest.columns())
         return false;
     for(unsigned int i=0;i<src1.columns();i++)
         dest.setColumn(i,src1.getColumn(i).RefPoint(base_AB));
     return true;
 }

 bool multiplyInertiaJacobian(const Jacobian& src, const RigidBodyInertia& I, MomentumJacobian& dest)
 {
     if(src.columns()!=dest.columns())
         return false;
     for(unsigned int i=0;i<src.columns();i++)
         dest.setColumn(i,I*src.getColumn(i));;
     return true;
 }

 bool divideJacobianInertia(const MomentumJacobian& src, const RigidBodyInertia& I, Jacobian& dest)
 {
     /** \todo if the inertia matrix is singular ? */
     if(src.columns()!=dest.columns() || I.getMass() == 0)
         return false;
     for(unsigned int i=0;i<src.columns();i++)
         dest.setColumn(i,src.getColumn(i)/I);
     return true;
 }

 bool Equal(const Jacobian& a,const Jacobian& b,double eps)
 {
     if(a.rows()==b.rows()&&a.columns()==b.columns()){
         bool rc=true;
         for(unsigned int i=0;i<a.columns();i++)
             rc&=Equal(a.twists[i],b.twists[i],eps);
         return rc;
     }else
         return false;
 }

void TranSVD::jacobian_reverse(Jacobian &jac)
{
	Transformable &data = jac.base_numeric_parameters;
	Eigen::SparseMatrix<double> old_matrix = jac.get_matrix(jac.observation_list(), super_parameter_names);
	Eigen::SparseMatrix<double> base_jacobian;
	base_jacobian = old_matrix * Vt;
	jac.matrix = base_jacobian;
	jac.base_numeric_par_names = base_parameter_names;
	reverse(data);
}

void TranSVD::jacobian_forward(Jacobian &jac)
{
	Transformable &data = jac.base_numeric_parameters;
	Eigen::SparseMatrix<double> old_matrix = jac.get_matrix(jac.observation_list(), base_parameter_names);
	Eigen::SparseMatrix<double> super_jacobian;
	super_jacobian = old_matrix * Vt.transpose();
	jac.matrix = super_jacobian;
	jac.base_numeric_par_names = super_parameter_names;

	forward(data);
}

void Jdot_diff(const Jacobian& J_q,
                     const Jacobian& J_qdt,
                     const double& dt,
                     Jacobian& Jdot)
{
    assert(J_q.columns() == J_qdt.columns());
    assert(J_q.columns() == Jdot.columns());
    for(int l=0;l<6;l++)
    for(int c=0;c<J_q.columns();c++)
        Jdot(l,c) = (J_qdt(l,c) - J_q(l,c))/dt;
}

    int ChainJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, int seg_nr)
    {
        unsigned int segmentNr;
        if(seg_nr<0)
            segmentNr=chain.getNrOfSegments();
        else
            segmentNr = seg_nr;

        //Initialize Jacobian to zero since only segmentNr colunns are computed
        SetToZero(jac) ;

        if(q_in.rows()!=chain.getNrOfJoints()||nr_of_unlocked_joints_!=jac.columns())
            return -1;
        else if(segmentNr>chain.getNrOfSegments())
            return -1;

        T_tmp = Frame::Identity();
        SetToZero(t_tmp);
        int j=0;
        int k=0;
        Frame total;
        for (unsigned int i=0;i<segmentNr;i++) {
            //Calculate new Frame_base_ee
            if(chain.getSegment(i).getJoint().getType()!=Joint::None){
            	//pose of the new end-point expressed in the base
                total = T_tmp*chain.getSegment(i).pose(q_in(j));
                //changing base of new segment's twist to base frame if it is not locked
                //t_tmp = T_tmp.M*chain.getSegment(i).twist(1.0);
                if(!locked_joints_[j])
                    t_tmp = T_tmp.M*chain.getSegment(i).twist(q_in(j),1.0);
            }else{
                total = T_tmp*chain.getSegment(i).pose(0.0);

            }

            //Changing Refpoint of all columns to new ee
            changeRefPoint(jac,total.p-T_tmp.p,jac);

            //Only increase jointnr if the segment has a joint
            if(chain.getSegment(i).getJoint().getType()!=Joint::None){
                //Only put the twist inside if it is not locked
                if(!locked_joints_[j])
                    jac.setColumn(k++,t_tmp);
                j++;
            }

            T_tmp = total;
        }
        return 0;
    }

    void getFloatingBaseJacobianLoop(const UndirectedTree & undirected_tree,
                                     const GeneralizedJntPositions &q,
                                     const Traversal & traversal,
                                     const int link_index,
                                     Jacobian & jac)
    {
        Frame T_total = Frame::Identity(); //The transformation between link_index frame and current_link frame

        assert(link_index < (int)undirected_tree.getNrOfLinks());

        KDL::CoDyCo::LinkMap::const_iterator current_link;
        current_link = undirected_tree.getLink(link_index);

        //All the columns not modified are zero
        SetToZero(jac);

        KDL::CoDyCo::LinkMap::const_iterator parent_link=traversal.getParentLink(current_link);
        while(current_link != traversal.getBaseLink()) {
            double joint_pos = 0.0;
            if( current_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
                KDL::Twist jac_col;

                int dof_index = current_link->getAdjacentJoint(parent_link)->getDOFIndex();

                joint_pos = q.jnt_pos(dof_index);
                KDL::Twist S_current_parent = parent_link->S(current_link,joint_pos);

                jac_col = T_total*S_current_parent;

                assert(6+dof_index < (int)jac.columns());
                assert( dof_index < (int)undirected_tree.getNrOfDOFs() );
                jac.setColumn(6+dof_index,jac_col);
            }

            KDL::Frame X_current_parent = parent_link->pose(current_link,joint_pos);

            T_total = T_total*X_current_parent;

            current_link = parent_link;
            parent_link = traversal.getParentLink(current_link);
        }

        //Setting the floating part of the Jacobian
        T_total = T_total*KDL::Frame(q.base_pos.M.Inverse());

        jac.data.block(0,0,6,6) = TwistTransformationMatrix(T_total);

        jac.changeBase(T_total.M.Inverse());
    }

int TreeIdSolver_Vereshchagin::CartToJnt(const JntArray& q, const JntArray& q_dot, JntArray& q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques)
{
    //Check sizes always
    if (q.rows() != nj || q_dot.rows() != nj || q_dotdot.rows() != nj || torques.rows() != nj)// || f_ext.size() != ns)
    {
        std::cout << "Error 1" << std::endl;
        return -1;
    }
    if (alfa.columns() != nc || beta.rows() != nc)
        //  if (alfas.size() != nc || betas.size() != nc) // number of constraints should also become a vector. because nc defines a number of columns in constraint jacobian matrix A(alfa)
    {
        std::cout << "Error 2" << std::endl;
        return -2;
    }

    //do an upward recursion for position and velocities
    this->initial_upwards_sweep(q, q_dot, q_dotdot, f_ext);
    //do an inward recursion for inertia, forces and constraints
    this->downwards_sweep(alfa, torques);
    //Solve for the constraint forces
    //    this->constraint_calculation(beta);
    //do an upward recursion to propagate the result
    //    this->final_upwards_sweep(q_dotdot, torques);
    return 0;
}

void changeRepresentation(Jacobian& J,const Frame& F_bs_ee,const int& representation)
{
    switch(representation)
    {
        case ChainJntToJacDotSolver::HYBRID:
            break;
        case ChainJntToJacDotSolver::BODYFIXED:
            // Ref Frame {ee}, Ref Point {ee}
            J.changeBase(F_bs_ee.M.Inverse());
            break;
        case ChainJntToJacDotSolver::INTERTIAL:
            // Ref Frame {bs}, Ref Point {bs}
            J.changeRefPoint(-F_bs_ee.p);
            break;
    }
}