示例#1
0
 def __init__(self, model, pinocchioData):
     nc, nv = model.nimpulse, model.nv
     self.pinocchio = pinocchioData
     self.J = np.zeros([nc, nv])
     self.Jq = np.zeros([nc, nv])
     self.f = np.nan  # not set at construction type
     self.forces = pinocchio.StdVect_Force()
     for i in range(model.pinocchio.njoints):
         self.forces.append(pinocchio.Force.Zero())
     self.Vq = np.zeros([nc, nv])
示例#2
0
 def __init__(self, model, pinocchioData):
     nc, nv, ndx = model.ncontact, model.nv, model.ndx
     self.pinocchio = pinocchioData
     self.J = np.zeros([nc, nv])
     self.a0 = np.zeros(nc)
     self.Ax = np.zeros([nc, ndx])
     self.Aq = self.Ax[:, :nv]
     self.Av = self.Ax[:, nv:]
     self.f = np.nan  # not set at construction type
     self.forces = pinocchio.StdVect_Force()
     for i in range(model.pinocchio.njoints):
         self.forces.append(pinocchio.Force.Zero())
示例#3
0
    def constraint_dyn(self, x, nc=0):
        '''
        M aq + b(q,vq) + g(q) = tau_q + J(q)^T f
        = rnea(q,vq=0,aq=0,fs)
        '''
        q, tauq, fr, fl = self.x2qf(x)

        # Forces should be stored in RNEA-compatible structure
        forces = pinocchio.StdVect_Force()
        for i in range(self.rmodel.njoints):
            forces.append(pinocchio.Force.Zero())
        # Forces should be expressed at the joint for RNEA, while we store them
        # at the frame in the optim problem. Convert.
        jMr = rmodel.frames[self.idR].placement
        forces[self.jidR] = jMr * pinocchio.Force(fr)
        jMl = rmodel.frames[self.idL].placement
        forces[self.jidL] = jMl * pinocchio.Force(fl)

        #q = self.rmodel.neutralConfiguration
        aq = vq = zero(self.rmodel.nv)
        tauref = pinocchio.rnea(self.rmodel, self.rdata, q, vq, aq, forces)
        self.eq[nc:nc + self.rmodel.nv] = (tauref - tauq).flat
        return self.eq[nc:nc + self.rmodel.nv].tolist()
    h)  # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)

# Check RNEA versus ABA derivatives.

Mi = pinocchio.computeMinverse(model, data, q)
assert (absmax(Mi - inv(data.M)) < 1e-6)

D = np.dot(Mi, data.dtau_dq)
assertNumDiff(
    D, -data.ddq_dq, NUMDIFF_MODIFIER *
    h)  # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)

# ---- ABA AND RNEA with forces

# Set forces container
fs = pinocchio.StdVect_Force()
for i in range(model.njoints):
    fs.append(pinocchio.Force.Random())

# Check RNEA derivatives versus finite-diff (with forces)
a = pinocchio.aba(model, data, q, v, tau, fs)
dtau_dqn = df_dq(model, lambda q_: pinocchio.rnea(model, data, q_, v, a, fs),
                 q)
pinocchio.computeRNEADerivatives(model, data, q, v, a, fs)
dtau_dq = data.dtau_dq.copy()
assertNumDiff(
    dtau_dqn, dtau_dq, NUMDIFF_MODIFIER *
    h)  # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)

# Check ABA derivatives versus finite diff (with forces)
da_dqn = df_dq(model, lambda q_: pinocchio.aba(model, data, q_, v, tau, fs), q)
示例#5
0
def ForceVect(force_list):
    '''Convert a list of forces into a StdVect_Force'''
    res = se3.StdVect_Force()
    res.extend(f for f in force_list)
    return res
示例#6
0
def ForceDict(force_dict, N):
    '''Convert a dict of forces into a StdVect_Force'''
    res = se3.StdVect_Force()
    res.extend(se3.Force.Zero() if i not in force_dict else force_dict[i]
               for i in range(N))
    return res