Esempio n. 1
0
def invdyn(model):
    """Model inverse dynamics"""
        
    tau = []
    for i in range(0, len(model.parent)):
        pbody_ind = model.parent[i]
        pbody = model.bodies[pbody_ind]
        body  = model.bodies[i + 1]
        joint = pbody.joints[body]
                   
        vJ = joint.S*joint.qd;
        joint.Xup = joint.Xj * joint.Xtree;
        
        if model.parent[i] == 0:
            body.v = vJ;
            body.a = joint.Xup * -model.a_grav + joint.S*joint.qdd;
        else:
            body.v = joint.Xup*pbody.v + vJ;
            body.a = joint.Xup*pbody.a + joint.S*joint.qdd + sv.cross_m_m(body.v,vJ);

        body.f = body.rbI*body.a + sv.cross_m_f(body.v, body.rbI*body.v) ;
        if body.f_ext != None:
            body.f = body.f - body.f_ext
            
    for i in range(len(model.parent)-1,-1, -1):
        pbody_ind = model.parent[i]
        pbody = model.bodies[pbody_ind]
        body  = model.bodies[i + 1]
        joint = pbody.joints[body]
        
        joint.tau = joint.S.transpose() * body.f;
        tau.append(joint.tau)
        if model.parent[i] != 0:
            pbody.f = pbody.f + joint.Xup.transpose()*body.f;
    
    return tau
Esempio n. 2
0
def fwdyn_ab(model):
    """Forward dynamics of articulated body"""

    qdd = []    
    for i in range(0, len(model.parent)):
        pbody_ind = model.parent[i]
        pbody = model.bodies[pbody_ind]
        body  = model.bodies[i + 1]
        joint = pbody.joints[body]
    
        vJ = joint.S*joint.qd;
        joint.Xup = joint.Xj * joint.Xtree;
        
        if model.parent[i] == 0:
            body.v = vJ;
            body.c = np.zeros((6,1))
        else:
            body.v = joint.Xup*pbody.v + vJ;
            body.c = sv.cross_m_m(body.v, vJ)
        
        body.IA = body.rbI
        body.pA = sv.cross_m_f(body.v, body.rbI*body.v) 

        if body.f_ext != None:
            body.pA = body.pA - body.f_ext
        
        if pbody.Xup != None:
            body.Xup = joint.Xup * pbody.Xup
        
            
    for i in range(len(model.parent)-1,-1, -1):
        pbody_ind = model.parent[i]
        pbody = model.bodies[pbody_ind]
        body  = model.bodies[i + 1]
        joint = pbody.joints[body]

        joint.U = body.IA * joint.S
        joint.d = joint.S.transpose() * joint.U
        joint.u = joint.tau - (joint.S.transpose() * body.pA)
        
        if(np.linalg.det(joint.d) == 0.0):
            joint.d_inv = joint.d*0.0  # trick for zero dof joint
        else:
            joint.d_inv = np.linalg.inv(joint.d)

        if model.parent[i] != 0:
            Ia = body.IA - (joint.d_inv * joint.U * joint.U.transpose())     
            pa = body.pA + (Ia*body.c) + joint.U * joint.d_inv * joint.u             
            pbody.IA = pbody.IA + (joint.Xup.transpose() * Ia * joint.Xup)
            pbody.pA = pbody.pA + (joint.Xup.transpose() * pa)
            

    for i in range(0, len(model.parent)):

        pbody_ind = model.parent[i]
        pbody = model.bodies[pbody_ind]
        body  = model.bodies[i + 1]
        joint = pbody.joints[body]

        if model.parent[i] == 0:
            body.a = joint.Xup * -model.a_grav + body.c
        else:
            body.a = joint.Xup * pbody.a + body.c
            
        joint.qdd = joint.d_inv*(joint.u - joint.U.transpose()*body.a);
        body.a = body.a + joint.S * joint.qdd
        qdd.append(joint.qdd)
        
    return qdd