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