Пример #1
0
 bool Equal(const MomentumJacobian& a,const MomentumJacobian& b,double eps)
 {
     if(a.rows()==b.rows()&&a.columns()==b.columns()){
         return a.data.isApprox(b.data,eps);
     }else
         return false;
 }
Пример #2
0
 bool changeRefPoint(const MomentumJacobian& src1, const Vector& base_AB, MomentumJacobian& 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;
 }
Пример #3
0
 bool changeRefFrame(const MomentumJacobian& src1,const Frame& frame, MomentumJacobian& 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;
 }
Пример #4
0
 bool changeBase(const MomentumJacobian& src1, const Rotation& rot, MomentumJacobian& 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;
 }
Пример #5
0
 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;
 }
Пример #6
0
 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;
 }
Пример #7
0
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()]);


            }

        }
    }

    //We have then to translate the reference point of the obtained jacobian to the com
    //The Ic[traversal.order[0]->getLink(index)] contain the spatial inertial of all the tree
    //expressed in link coordite frames
    Vector com = Ic[traversal.getBaseLink()->getLinkIndex()].getCOG();
    H.changeRefPoint(com);

    InertiaCOM = Frame(com)*Ic[traversal.getBaseLink()->getLinkIndex()];

    return 0;
}