4 sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
5 from test_case
import PinocchioTestCase
as TestCase
7 import pinocchio
as pin
8 from pinocchio
import casadi
as cpin
17 self.
model = pin.buildSampleModelHumanoidRandom()
23 qmax = np.full((self.
model.nq, 1), np.pi)
24 self.
q = pin.randomConfiguration(self.
model, -qmax, qmax)
25 self.
v = np.random.rand(self.
model.nv)
26 self.
a = np.random.rand(self.
model.nv)
36 for i
in range(self.
model.njoints):
41 self.
fext[0].setZero()
50 cM = cpin.crba(cmodel, cdata, self.
cq)
51 f_crba = casadi.Function(
"crba", [self.
cq], [cM])
53 arg_crba = [list(self.
q)]
54 M = f_crba.call(arg_crba)[0].full()
56 M_ref = pin.crba(model, data, self.
q)
58 self.assertApprox(M, M_ref)
67 ctau = cpin.rnea(cmodel, cdata, self.
cq, self.
cv, self.
ca)
68 f_rnea = casadi.Function(
"rnea", [self.
cq, self.
cv, self.
ca], [ctau])
70 arg_rnea = [list(self.
q), list(self.
v), list(self.
a)]
71 tau = f_rnea.call(arg_rnea)[0].full()
73 tau_ref = pin.rnea(model, data, self.
q, self.
v, self.
a)
75 self.assertApprox(tau, tau_ref)
77 ctau_fext = cpin.rnea(cmodel, cdata, self.
cq, self.
cv, self.
ca, self.
cfext)
78 carg_in = [self.
cq, self.
cv, self.
ca]
81 f_rnea_fext = casadi.Function(
"rnea_fext", carg_in, [ctau_fext])
83 arg_rnea_fext = arg_rnea
85 arg_rnea_fext += [f.vector]
86 tau_fext = f_rnea_fext.call(arg_rnea_fext)[0].full()
88 tau_fext_ref = pin.rnea(model, data, self.
q, self.
v, self.
a, self.
fext)
90 self.assertApprox(tau_fext, tau_fext_ref)
99 cddq = cpin.aba(cmodel, cdata, self.
cq, self.
cv, self.
ctau)
100 f_aba = casadi.Function(
"aba", [self.
cq, self.
cv, self.
ctau], [cddq])
102 arg_aba = [list(self.
q), list(self.
v), list(self.
tau)]
103 a = f_aba.call(arg_aba)[0].full()
105 a_ref = pin.aba(model, data, self.
q, self.
v, self.
tau)
107 self.assertApprox(a, a_ref)
109 cddq_fext = cpin.aba(cmodel, cdata, self.
cq, self.
cv, self.
ctau, self.
cfext)
110 carg_in = [self.
cq, self.
cv, self.
ctau]
112 carg_in += [f.vector]
113 f_aba_fext = casadi.Function(
"aba_fext", carg_in, [cddq_fext])
115 arg_aba_fext = arg_aba
117 arg_aba_fext += [f.vector]
118 a_fext = f_aba_fext.call(arg_aba_fext)[0].full()
120 a_fext_ref = pin.aba(model, data, self.
q, self.
v, self.
tau, self.
fext)
122 self.assertApprox(a_fext, a_fext_ref)
126 Minv = pin.computeMinverse(model, self.
data, self.
q)
128 data2 = model.createData()
129 M = pin.crba(model, data2, self.
q)
131 self.assertApprox(np.linalg.inv(M), Minv)
134 if __name__ ==
"__main__":