Example #1
0
def FDab_X0(model, q, qd):
    Xup = [None] * model.NB
    X0 = [None] * model.NB
    for i in range(model.NB):
        XJ, S = featherstone.jcalc(model.pitch[i], q[i])
        Xup[i] = XJ * model.Xtree[i]
        if model.parent[i] == -1:
            X0[i] = Xup[i]
        else:
            X0[i] = Xup[i] * X0[model.parent[i]]
    return X0
Example #2
0
def FDab_X0(model, q, qd):
  Xup = [None] * model.NB
  X0 = [None] * model.NB
  for i in xrange(model.NB):
    XJ, S = featherstone.jcalc( model.pitch[i], q[i] )
    Xup[i] = XJ * model.Xtree[i]
    if model.parent[i] == -1:
      X0[i] = Xup[i]
    else:
      X0[i] = Xup[i] * X0[model.parent[i]]
  return X0
Example #3
0
def spatial_velocities_from_model(model, q, qd):
  result = [None] * model.NB
  Xup = [None] * model.NB
  for i in xrange(model.NB):
    XJ, S = featherstone.jcalc( model.pitch[i], q[i] )
    if (S is None):
      vJ = qd[i]
    else:
      vJ = S*qd[i]
    Xup[i] = XJ * model.Xtree[i]
    if model.parent[i] == -1:
      result[i] = vJ
    else:
      result[i] = Xup[i]*result[model.parent[i]] + vJ
  return result
Example #4
0
File: utils.py Project: dials/cctbx
def spatial_velocities_from_model(model, q, qd):
    result = [None] * model.NB
    Xup = [None] * model.NB
    for i in range(model.NB):
        XJ, S = featherstone.jcalc(model.pitch[i], q[i])
        if (S is None):
            vJ = qd[i]
        else:
            vJ = S * qd[i]
        Xup[i] = XJ * model.Xtree[i]
        if model.parent[i] == -1:
            result[i] = vJ
        else:
            result[i] = Xup[i] * result[model.parent[i]] + vJ
    return result