Exemplo n.º 1
0
 // Ground's "articulated" body inertia is still the infinite mass and
 // inertia it started with; no need to look at the children.
 void realizeArticulatedBodyInertiasInward(
     const SBInstanceCache&,
     const SBTreePositionCache&,
     SBArticulatedBodyInertiaCache& abc) const
 {   ArticulatedInertia& P = updP(abc);
     P = ArticulatedInertia(SymMat33(Infinity), Mat33(Infinity),
                                    SymMat33(0));
     updPPlus(abc) = P;
 }
Exemplo n.º 2
0
    void realizeArticulatedBodyInertiasInward
       (const SBInstanceCache&          ic,
        const SBTreePositionCache&      pc,
        SBArticulatedBodyInertiaCache&  abc) const
    {
        ArticulatedInertia& P = updP(abc);
        P = ArticulatedInertia(getMk_G(pc));
        for (unsigned i=0 ; i<children.size() ; i++) {
            const PhiMatrix&          phiChild   = children[i]->getPhi(pc);
            const ArticulatedInertia& PPlusChild = children[i]->getPPlus(abc);

            P += PPlusChild.shift(phiChild.l());
        }
        updPPlus(abc) = P;
    }
Exemplo n.º 3
0
//==============================================================================
//                     REALIZE ARTICULATED BODY INERTIAS
//==============================================================================
// Compute articulated body inertia and related quantities for this body B.
// This must be called tip-to-base (inward).
//
// Given only position-related quantities from the State 
//      Mk  (this body's spatial inertia matrix)
//      Phi (composite body child-to-parent shift matrix)
//      H   (joint transition matrix; sense is transposed from Jain & Schwieters)
// we calculate dynamic quantities 
//      P   (articulated body inertia)
//    PPlus (articulated body inertia as seen through the mobilizer)
// For a prescribed mobilizer, we have PPlus==P. Otherwise we also compute
//      D   (factored mass matrix LDL' diagonal part D=~H*P*H)
//      DI  (inverse of D)
//      G   (P * H * DI)
//    PPlus (P - P * H * DI * ~H * P = P - G * ~H * P)
// and put them in the state cache.
//
// This is Algorithm 6.1 on page 106 of Jain's 2011 book modified to accommodate
// prescribed motion as described on page 323. Note that although P+ is "as 
// felt" on the inboard side of the mobilizer it is still calculated about 
// Bo just as is P.
//
// Cost is 93 flops per child plus
//   n^3 + 23*n^2 + 115*n + 12
//   e.g. pin=143, ball=591 (197/dof), free=1746 (291/dof)
// Note that per-child cost is paid just once for each non-base body in
// the whole tree; that is, each body is touched just once.
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::
realizeArticulatedBodyInertiasInward(
    const SBInstanceCache&          ic,
    const SBTreePositionCache&      pc,
    SBArticulatedBodyInertiaCache&  abc) const 
{
    ArticulatedInertia& P = updP(abc);

    // Start with the spatial inertia of the current body (in Ground frame).
    P = ArticulatedInertia(getMk_G(pc)); // 12 flops

    // For each child, we previously took its articulated body inertia P and 
    // removed the portion of that inertia that can't be felt from the parent
    // because of the joint mobilities. That is, we calculated 
    // P+ = P - P H DI ~H P, where the second term is the projection of P into
    // the mobility space of the child's inboard mobilizer. Note that if the
    // child's mobilizer is prescribed, then the entire inertia will be felt
    // by the parent so P+ = P in that case. Now we're going to shift P+
    // from child to parent: Pparent += Phi*P+*~Phi.
    // TODO: can this be optimized for the common case where the
    // child is a terminal body and hence its P is an ordinary
    // spatial inertia? (Spatial inertia shift is 37 flops vs. 72 here; not
    // really much help.)
    for (unsigned i=0; i<children.size(); ++i) {
        const PhiMatrix&          phiChild   = children[i]->getPhi(pc);
        const ArticulatedInertia& PPlusChild = children[i]->getPPlus(abc);

        // Apply the articulated body shift.
        // This takes 93 flops (72 for the shift and 21 to add it in).
        // (Note that PPlusChild==PChild if child's mobilizer is prescribed.)
        P += PPlusChild.shift(phiChild.l());
    }

    // Now compute PPlus. P+ = P for a prescribed mobilizer. Otherwise
    // it is P+ = P - P H DI ~H P = P - G*~PH. In the prescribed case
    // we leave G, D, DI untouched -- they should have been set to NaN at
    // Instance stage.
    ArticulatedInertia& PPlus = updPPlus(abc);

    if (isUDotKnown(ic)) {
        PPlus = P;  // prescribed
        return;
    }

    // This is a non-prescribed mobilizer. Compute D, DI, G then P+.

    const HType&  H  = getH(pc);
    HType&        G  = updG(abc);
    Mat<dof,dof>& D  = updD(abc);
    Mat<dof,dof>& DI = updDI(abc);

    const HType PH = P*H;   // 66*dof   flops
    D  = ~H * PH;           // 11*dof^2 flops (symmetric result)

    // this will throw an exception if the matrix is ill conditioned
    DI = D.invert();                        // ~dof^3 flops (symmetric)
    G  = PH * DI;                           // 12*dof^2-6*dof flops

    // Want P+ = P - G*~PH. We can do this in about 55*dof flops.
    // These require 9 dot products of length dof. The symmetric ones could
    // be done with 6 dot products instead for a small savings but this gives
    // us a chance to symmetrize and hopefully clean up some numerical errors.
    // The full price for all three is 54*dof-27 flops.
    Mat33 massMoment = G.row(0)*~PH.row(1); // (full)            9*(2*dof-1) flops
    Mat33 mass       = G.row(1)*~PH.row(1); // symmetric result  9*(2*dof-1) flops
    Mat33 inertia    = G.row(0)*~PH.row(0); // symmetric result  9*(2*dof-1) flops
    // These must be symmetrized due to numerical errors for 12 more flops. 
    SymMat33 symMass( mass(0,0), 
                     (mass(1,0)+mass(0,1))/2,  mass(1,1), 
                     (mass(2,0)+mass(0,2))/2, (mass(2,1)+mass(1,2))/2, mass(2,2));
    SymMat33 symInertia( 
         inertia(0,0), 
        (inertia(1,0)+inertia(0,1))/2,  inertia(1,1), 
        (inertia(2,0)+inertia(0,2))/2, (inertia(2,1)+inertia(1,2))/2, inertia(2,2));
    PPlus = P - ArticulatedInertia(symMass, massMoment, symInertia); // 21 flops
}