C++ Jacobian::columns方法代码示例

 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;
 }

 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 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 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 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 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 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;
}

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);
}

 Jacobian::Jacobian(const Jacobian& arg):
                    size(arg.columns()),
                    nr_blocks(arg.nr_blocks)
 {
     twists = new Twist[size*nr_blocks];
     for(unsigned int i=0;i<size*nr_blocks;i++)
         twists[i] = arg.twists[i];
 }

 void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest)
 {
     assert(jac.columns()==src.size);
     SetToZero(dest);
     for(unsigned int i=0;i<6;i++)
         for(unsigned int j=0;j<src.size;j++)
             dest(i)+=jac(i,j)*src.data[j];
 }

int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac,
        const std::string& segmentname) {
    //First we check all the sizes:
    if (q_in.rows() != tree.getNrOfJoints() || jac.columns()
            != tree.getNrOfJoints())
        return -1;

    //Lets search the tree-element
    SegmentMap::value_type const* it = tree.getSegmentPtr(segmentname);

    //If segmentname is not inside the tree, back out:
    if (!it)
        return -2;

    //Let's make the jacobian zero:
    SetToZero(jac);

    SegmentMap::value_type const* root = tree.getSegmentPtr("root");

    Frame T_total = Frame::Identity();
	Frame T_local, T_joint;
	Twist t_local;
    //Lets recursively iterate until we are in the root segment
    while (it != root) {
        //get the corresponding q_nr for this TreeElement:
        unsigned int q_nr = it->second.q_nr;

        //get the pose of the joint.
		T_joint = it->second.segment.getJoint().pose(((JntArray&)q_in)(q_nr));
		// combine with the tip to have the tip pose
		T_local = T_joint*it->second.segment.getFrameToTip();
        //calculate new T_end:
        T_total = T_local * T_total;

        //get the twist of the segment:
		int ndof = it->second.segment.getJoint().getNDof();
		for (int dof=0; dof<ndof; dof++) {
			// combine joint rotation with tip position to get a reference frame for the joint
			T_joint.p = T_local.p;
			// in which the twist can be computed (needed for NDof joint)
            t_local = it->second.segment.twist(T_joint, 1.0, dof);
            //transform the endpoint of the local twist to the global endpoint:
            t_local = t_local.RefPoint(T_total.p - T_local.p);
            //transform the base of the twist to the endpoint
            t_local = T_total.M.Inverse(t_local);
            //store the twist in the jacobian:
            jac.twists[q_nr+dof] = t_local;
        }
        //goto the parent
        it = it->second.parent;
    }//endwhile
    //Change the base of the complete jacobian from the endpoint to the base
    changeBase(jac, T_total.M, jac);

    return 0;

}//end JntToJac

 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;
 }

inline Twist operator*(const Jacobian& jac, const JntArray& qdot)
{
  assert(jac.columns() == qdot.rows());
  Twist t;

  int naxes = qdot.rows();
  for(int i=0; i < 6; ++i)
    for(int j=0; j < naxes; ++j)
      t(i) += jac(i,j)*qdot(j);

  return t;
}

    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;
    }