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

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

    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 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::const_iterator it = tree.getSegments().find(segmentname);

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

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

    SegmentMap::const_iterator root = tree.getRootSegment();

    Frame T_total = Frame::Identity();
    //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 segment:
        Frame T_local = it->second.segment.pose(q_in(q_nr));
        //calculate new T_end:
        T_total = T_local * T_total;

        //get the twist of the segment:
        if (it->second.segment.getJoint().getType() != Joint::None) {
            Twist t_local = it->second.segment.twist(q_in(q_nr), 1.0);
            //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.setColumn(q_nr,t_local);
        }//endif
        //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

    void getRelativeJacobianLoop(const UndirectedTree & undirected_tree,
                                const KDL::JntArray &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

        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(dof_index);
                KDL::Twist S_current_parent = parent_link->S(current_link,joint_pos);

                jac_col = T_total*S_current_parent;

                jac.setColumn(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);
        }

    }