C++ Traversal::getParentLink方法代码示例

    int getFramesLoop(const UndirectedTree & undirected_tree,
                      const KDL::JntArray &q,
                      const Traversal & traversal,
                      std::vector<Frame> & X_base,
                      KDL::Frame world2base)
    {
          for(int i=0; i < (int)traversal.getNrOfVisitedLinks(); i++) {
            double joint_pos;
            LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
            int link_nmbr = link_it->getLinkIndex();
            if( i == 0 ) {
                assert( traversal.getParentLink(link_nmbr) == undirected_tree.getInvalidLinkIterator() );
                X_base[link_nmbr] = world2base;
            } else {
                LinkMap::const_iterator parent_it = traversal.getParentLink(link_it);
                int parent_nmbr = parent_it->getLinkIndex();

                if( link_it->getAdjacentJoint(parent_it)->getJoint().getType() != Joint::None ) {
                    int dof_nr = link_it->getAdjacentJoint(parent_it)->getDOFIndex();
                    joint_pos = q(dof_nr);
                } else {
                    joint_pos =  0.0;
                }
                KDL::Frame X_parent_son = link_it->pose(parent_it,joint_pos);
                X_base[link_nmbr] = X_base[parent_nmbr]*X_parent_son;
            }
        }
        return 0;
    }

    int getWorldFrameLoop(const UndirectedTree & undirected_tree,
                          const KDL::CoDyCo::GeneralizedJntPositions &q,
                          const Traversal & traversal,
                          const int distal_link_index,
                          Frame & frame_world_link)
    {
        LinkMap::const_iterator distal_it = undirected_tree.getLink(distal_link_index);
        LinkMap::const_iterator proximal_it = traversal.getBaseLink();

        Frame currentFrame;
        Frame resultFrame = Frame::Identity();
        for(LinkMap::const_iterator link=distal_it; link != proximal_it; link = traversal.getParentLink(link) ) {
            LinkMap::const_iterator parent_link = traversal.getParentLink(link);
            assert( parent_link != undirected_tree.getInvalidLinkIterator() );

            double joint_position;

            if( link->getAdjacentJoint(parent_link)->getJoint().getType() != Joint::None ) {
                joint_position = q.jnt_pos((link->getAdjacentJoint(parent_link))->getDOFIndex());
            } else {
                joint_position =0;
            }

            currentFrame = link->pose(parent_link,
                                             joint_position);

            resultFrame = currentFrame*resultFrame;
        }

        frame_world_link = q.base_pos*resultFrame;

        return 0;
    }

void dynamicsRegressorLoop(const UndirectedTree & ,
                         const KDL::JntArray &q,
                         const Traversal & traversal,
                         const std::vector<Frame>& X_b,
                         const std::vector<Twist>& v,
                         const std::vector<Twist>& a,
                         Eigen::MatrixXd & dynamics_regressor)
{
        dynamics_regressor.setZero();

        Eigen::Matrix<double, 6, 10> netWrenchRegressor_i;

        // Store the base_world translational transform in world orientation
        KDL::Frame world_base_X_world_world = KDL::Frame(-(X_b[traversal.getBaseLink()->getLinkIndex()].p));

        for(int l =(int)traversal.getNrOfVisitedLinks()-1; l >= 0; l-- ) {
            LinkMap::const_iterator link = traversal.getOrderedLink(l);
            int i = link->getLinkIndex();

            //Each link affects the dynamics of the joints from itself to the base
            netWrenchRegressor_i = netWrenchRegressor(v[i],a[i]);

            //Base dynamics
            // The base dynamics is expressed with the orientation of the world but
            // with respect to the base origin
            dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(world_base_X_world_world*X_b[i])*netWrenchRegressor_i;

            //dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(X_b[i])*netWrenchRegressor_i;

            LinkMap::const_iterator child_link = link;
            LinkMap::const_iterator parent_link=traversal.getParentLink(link);
            while( child_link != traversal.getOrderedLink(0) ) {
                if( child_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
                    #ifndef NDEBUG
                    //std::cerr << "Calculating regressor columns for link " << link->getName() << " and joint " << child_link->getAdjacentJoint(parent_link)->getName() << std::endl;
                    #endif
                    int dof_index = child_link->getAdjacentJoint(parent_link)->getDOFIndex();
                    int child_index = child_link->getLinkIndex();
                    Frame X_j_i = X_b[child_index].Inverse()*X_b[i];
                    dynamics_regressor.block(6+dof_index,10*i,1,10) =
                            toEigen(parent_link->S(child_link,q(dof_index))).transpose()*WrenchTransformationMatrix(X_j_i)*netWrenchRegressor_i;
                }
                child_link = parent_link;
                #ifndef NDEBUG
                //std::cout << "Getting parent link of link of index " << child_link->getName() << " " << child_link->getLinkIndex() << std::endl;
                //std::cout << "Current base " << traversal.order[0]->getName() << " " << traversal.order[0]->getLinkIndex() << std::endl;
                #endif
                parent_link = traversal.getParentLink(child_link);
            }
        }
}

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

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

    }

void dynamicsRegressorFixedBaseLoop(const UndirectedTree & ,
                         const KDL::JntArray &q,
                         const Traversal & traversal,
                         const std::vector<Frame>& X_b,
                         const std::vector<Twist>& v,
                         const std::vector<Twist>& a,
                         Eigen::MatrixXd & dynamics_regressor)
{
        dynamics_regressor.setZero();

        Eigen::Matrix<double, 6, 10> netWrenchRegressor_i;

        for(int l =(int)traversal.getNrOfVisitedLinks()-1; l >= 0; l-- ) {
            LinkMap::const_iterator link = traversal.getOrderedLink(l);
            int i = link->getLinkIndex();

            //Each link affects the dynamics of the joints from itself to the base
            netWrenchRegressor_i = netWrenchRegressor(v[i],a[i]);

             //dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(X_b[i])*netWrenchRegressor_i;

            LinkMap::const_iterator child_link = link;
            LinkMap::const_iterator parent_link=traversal.getParentLink(link);
            while( child_link != traversal.getOrderedLink(0) ) {
                if( child_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
                    int dof_index = child_link->getAdjacentJoint(parent_link)->getDOFIndex();
                    int child_index = child_link->getLinkIndex();
                    Frame X_j_i = X_b[child_index].Inverse()*X_b[i];
                    dynamics_regressor.block(dof_index,10*i,1,10) =
                            toEigen(parent_link->S(child_link,q(dof_index))).transpose()*WrenchTransformationMatrix(X_j_i)*netWrenchRegressor_i;
                }
                child_link = parent_link;
                parent_link = traversal.getParentLink(child_link);
            }
        }
}

int crba_momentum_jacobian_loop(const UndirectedTree & undirected_tree,
                                const Traversal & traversal,
                                const JntArray & q,
                                std::vector<RigidBodyInertia> & Ic,
                                MomentumJacobian & H,
                                RigidBodyInertia & InertiaCOM
                               )
{
#ifndef NDEBUG
    if( undirected_tree.getNrOfLinks() != traversal.getNrOfVisitedLinks() ||
            undirected_tree.getNrOfDOFs() != q.rows() ||
            Ic.size() != undirected_tree.getNrOfLinks() ||
            H.columns() != (undirected_tree.getNrOfDOFs() + 6) )
    {
        std::cerr << "crba_momentum_jacobian_loop: input data error" << std::endl;
        return -1;
    }
#endif

    double q_;
    Wrench F = Wrench::Zero();

    //Sweep from root to leaf
    for(int i=0; i<(int)traversal.getNrOfVisitedLinks(); i++)
    {
        LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
        int link_index = link_it->getLinkIndex();

        //Collect RigidBodyInertia
        Ic[link_index]=link_it->getInertia();

    }

    for(int i=(int)traversal.getNrOfVisitedLinks()-1; i >= 1; i-- ) {
        int dof_id;
        LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
        int link_index = link_it->getLinkIndex();

        LinkMap::const_iterator parent_it = traversal.getParentLink(link_index);
        int parent_index = parent_it->getLinkIndex();

        if( link_it->getAdjacentJoint(parent_it)->getNrOfDOFs() == 1 ) {
            dof_id = link_it->getAdjacentJoint(parent_it)->getDOFIndex();
            q_ = q(dof_id);
        } else {
            q_ = 0.0;
            dof_id = -1;
        }

        Ic[parent_index] = Ic[parent_index]+link_it->pose(parent_it,q_)*Ic[link_index];

        if( link_it->getAdjacentJoint(parent_it)->getNrOfDOFs() == 1 ) {
            KDL::Twist S_link_parent = parent_it->S(link_it,q_);
            F = Ic[link_index]*S_link_parent;

            if( traversal.getParentLink(link_it) != undirected_tree.getInvalidLinkIterator() ) {
                double q__;
                int dof_id_;
                LinkMap::const_iterator predecessor_it = traversal.getParentLink(link_it);
                LinkMap::const_iterator successor_it = link_it;

                while(true) {

                    if( predecessor_it->getAdjacentJoint(successor_it)->getNrOfDOFs() == 1 ) {
                        q__ = q( predecessor_it->getAdjacentJoint(successor_it)->getDOFIndex());
                    } else {
                        q__ = 0.0;
                    }

                    F = successor_it->pose(predecessor_it,q__)*F;

                    successor_it = predecessor_it;
                    predecessor_it = traversal.getParentLink(predecessor_it);

                    if( predecessor_it == undirected_tree.getInvalidLinkIterator() ) {
                        break;
                    }

                    if( predecessor_it->getAdjacentJoint(successor_it)->getNrOfDOFs() == 1 ) {
                        dof_id_ =  predecessor_it->getAdjacentJoint(successor_it)->getDOFIndex();
                        q__ = q(dof_id_);
                    } else {
                        q__ = 0.0;
                        dof_id_ = -1;
                    }


                }
                if( dof_id >= 0 ) {
                    H.data.block(0,6+dof_id,6,1) = toEigen(F);
                }

                //The first 6x6 submatrix of the Momentum Jacobian are simply the spatial inertia
                //of all the structure expressed in the base reference frame
                H.data.block(0,0,6,6) = toEigen(Ic[traversal.getBaseLink()->getLinkIndex()]);


            }

        }
//.........这里部分代码省略.........

int crba_fixed_base_loop(const UndirectedTree & undirected_tree, const Traversal & traversal, const JntArray & q, std::vector<RigidBodyInertia> & Ic, JntSpaceInertiaMatrix & H) {
    double q_;
    Wrench F;

    //Sweep from root to leaf
    for(int i=0; i<(int)traversal.getNrOfVisitedLinks(); i++)
    {
        LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
        int link_index = link_it->getLinkIndex();

        //Collect RigidBodyInertia
        Ic[link_index] = link_it->getInertia();

    }

    for(int i=(int)traversal.getNrOfVisitedLinks()-1; i >= 1; i-- ) {
        int dof_id;
        LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
        int link_index = link_it->getLinkIndex();

        LinkMap::const_iterator parent_it = traversal.getParentLink(link_index);
        int parent_index = parent_it->getLinkIndex();

        if( link_it->getAdjacentJoint(parent_it)->getNrOfDOFs() == 1 ) {
            dof_id = link_it->getAdjacentJoint(parent_it)->getDOFIndex();
            q_ = q(dof_id);
        } else {
            q_ = 0.0;
            dof_id = -1;
        }

        Ic[parent_index] = Ic[parent_index]+link_it->pose(parent_it,q_)*Ic[link_index];

        if( link_it->getAdjacentJoint(parent_it)->getNrOfDOFs() == 1 ) {
            KDL::Twist S_link_parent = parent_it->S(link_it,q_);
            F = Ic[link_index]*S_link_parent;
            H(dof_id,dof_id) = dot(S_link_parent,F);

            {
                assert(parent_it != undirected_tree.getInvalidLinkIterator());
                double q__;
                int dof_id_;
                LinkMap::const_iterator predecessor_it = traversal.getParentLink(link_it);
                LinkMap::const_iterator successor_it = link_it;
                while( true ) {

                    if( predecessor_it->getAdjacentJoint(successor_it)->getNrOfDOFs() == 1 ) {
                        q__ = q( predecessor_it->getAdjacentJoint(successor_it)->getDOFIndex());
                    } else {
                        q__ = 0.0;
                    }

                    F = successor_it->pose(predecessor_it,q__)*F;

                    successor_it = predecessor_it;
                    predecessor_it = traversal.getParentLink(predecessor_it);

                    if( predecessor_it == undirected_tree.getInvalidLinkIterator() ) {
                        break;
                    }

                    if( predecessor_it->getAdjacentJoint(successor_it)->getNrOfDOFs() == 1 ) {
                        dof_id_ =  predecessor_it->getAdjacentJoint(successor_it)->getDOFIndex();
                        q__ = q(dof_id_);
                    } else {
                        q__ = 0.0;
                        dof_id_ = -1;
                    }

                    Twist S_successor_predecessor = predecessor_it->S(successor_it,q__);

                    if( dof_id_ >= 0 ) {
                        H(dof_id_,dof_id) = dot(S_successor_predecessor,F);
                        H(dof_id,dof_id_) = H(dof_id_,dof_id);
                    }


                }
            }



        }
    }

    return 0;
}

int crba_floating_base_loop(const UndirectedTree & undirected_tree,
                            const Traversal & traversal,
                            const GeneralizedJntPositions & q,
                            std::vector<RigidBodyInertia> & Ic,
                            FloatingJntSpaceInertiaMatrix & H) {
    Wrench F = Wrench::Zero();
    Wrench buffer_F = F;

    //Sweep from root to leaf
    for(int i=0; i<(int)traversal.getNrOfVisitedLinks(); i++)
    {
        LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
        int link_index = link_it->getLinkIndex();

        //Collect RigidBodyInertia
        Ic[link_index]=link_it->getInertia();

    }

    for(int i=(int)traversal.getNrOfVisitedLinks()-1; i >= 1; i-- ) {
        double row_dof_position;
        int row_dof_id;
        LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
        int link_index = link_it->getLinkIndex();

        LinkMap::const_iterator parent_it = traversal.getParentLink(link_index);
        int parent_index = parent_it->getLinkIndex();

        if( link_it->getAdjacentJoint(parent_it)->getNrOfDOFs() == 1 ) {
            row_dof_id = link_it->getAdjacentJoint(parent_it)->getDOFIndex();
            row_dof_position = q.jnt_pos(row_dof_id);
        } else {
            row_dof_position = 0.0;
            row_dof_id = -1;
        }

        RigidBodyInertia buf;
        buf = Ic[parent_index]+link_it->pose(parent_it,row_dof_position)*Ic[link_index];
        Ic[parent_index] = buf;


        if( link_it->getAdjacentJoint(parent_it)->getNrOfDOFs() == 1 ) {
            KDL::Twist S_link_parent = parent_it->S(link_it,row_dof_position);
            F = Ic[link_index]*S_link_parent;
            H(6+row_dof_id,6+row_dof_id) = dot(S_link_parent,F);

            if( traversal.getParentLink(link_it) != undirected_tree.getInvalidLinkIterator() ) {
                double column_dof_position;
                int column_dof_id;
                LinkMap::const_iterator predecessor_it = traversal.getParentLink(link_it);
                LinkMap::const_iterator successor_it = link_it;
                while(true) {

                    if( predecessor_it->getAdjacentJoint(successor_it)->getNrOfDOFs() == 1 ) {
                        column_dof_position = q.jnt_pos( predecessor_it->getAdjacentJoint(successor_it)->getDOFIndex());
                    } else {
                        column_dof_position = 0.0;
                    }

#ifndef NDEBUG
                    //std::cout << "F migrated from frame " << successor_it->getLinkIndex() << " to frame " << successor_it->getLinkIndex() << std::endl;
#endif
                    buffer_F = successor_it->pose(predecessor_it,column_dof_position)*F;
                    F = buffer_F;

                    successor_it = predecessor_it;
                    predecessor_it = traversal.getParentLink(predecessor_it);

                    if( predecessor_it == undirected_tree.getInvalidLinkIterator() ) {
                        break;
                    }


                    if( predecessor_it->getAdjacentJoint(successor_it)->getNrOfDOFs() == 1 ) {
                        column_dof_id =  predecessor_it->getAdjacentJoint(successor_it)->getDOFIndex();
                        column_dof_position = q.jnt_pos(column_dof_id);
                    } else {
                        column_dof_position = 0.0;
                        column_dof_id = -1;
                    }

                    Twist S_successor_predecessor = predecessor_it->S(successor_it,column_dof_position);

                    if( column_dof_id >= 0 ) {
                        H(6+column_dof_id,6+row_dof_id) = dot(S_successor_predecessor,F);
                        H(6+row_dof_id,6+column_dof_id) = H(6+column_dof_id,6+row_dof_id);
                    }


                }
                if( row_dof_id >= 0 ) {
                    buffer_F = q.base_pos.M*F;
                    F = buffer_F;
                    H.data.block(0,6+row_dof_id,6,1) = toEigen(F);
                    H.data.block(6+row_dof_id,0,1,6) = toEigen(F).transpose();
                }



            }
//.........这里部分代码省略.........