2 import pinocchio
as pin
5 from test_case
import PinocchioTestCase
as TestCase
10 self.
model = pin.buildSampleModelHumanoidRandom()
14 self.assertEqual(model.nbodies, 1)
15 self.assertEqual(model.nq, 0)
16 self.assertEqual(model.nv, 0)
17 model.name =
"empty_model"
23 idx, pin.JointModelRY(), pin.SE3.Identity(),
"joint_" + str(idx + 1)
31 me = np.array([MAX_EFF])
32 mv = np.array([MAX_VEL])
33 lb = np.array([MIN_POS])
34 ub = np.array([MAX_POS])
39 "joint_" + str(idx + 1),
46 self.assertEqual(model.nbodies, 1)
47 self.assertEqual(model.njoints, 3)
48 self.assertEqual(model.nq, 2)
49 self.assertEqual(model.nv, 2)
51 self.assertEqual(float(model.effortLimit[1]), MAX_EFF)
52 self.assertEqual(float(model.velocityLimit[1]), MAX_VEL)
53 self.assertEqual(float(model.lowerPositionLimit[1]), MIN_POS)
54 self.assertEqual(float(model.upperPositionLimit[1]), MAX_POS)
59 self.assertEqual(model.nbodies, nb)
60 self.assertEqual(model.nq, nb - 1 + 6)
61 self.assertEqual(model.nv, nb - 1 + 5)
65 model.inertias[1] = model.inertias[2]
66 self.assertApprox(model.inertias[1].np, model.inertias[2].np)
70 model.jointPlacements[1] = model.jointPlacements[2]
71 self.assertApprox(model.jointPlacements[1].np, model.jointPlacements[2].np)
72 self.assertEqual(model.parents[0], 0)
73 self.assertEqual(model.parents[1], 0)
74 model.parents[2] = model.parents[1]
75 self.assertEqual(model.parents[2], model.parents[1])
76 self.assertEqual(model.names[0],
"universe")
79 self.assertApprox(self.
model.gravity.np, np.array([0, 0, -9.81, 0, 0, 0]).T)
83 data = model.createData()
87 qddot =
zero(model.nv)
88 for i
in range(model.nbodies):
89 data.a[i] = pin.Motion.Zero()
91 pin.rnea(model, data, q, qdot, qddot)
92 for i
in range(model.nbodies):
93 self.assertApprox(data.v[i].np,
zero(6))
94 self.assertApprox(data.a_gf[0].np, -model.gravity.np)
95 self.assertApprox(data.f[-1], model.inertias[-1] * data.a_gf[-1])
99 model.referenceConfigurations[
"neutral"] = pin.neutral(model)
100 q_neutral = model.referenceConfigurations[
"neutral"]
103 self.assertApprox(model.referenceConfigurations[
"neutral"], q_neutral)
109 filename =
"model.pickle"
110 with open(filename,
"wb")
as f:
111 pickle.dump(model, f)
113 with open(filename,
"rb")
as f:
114 model_copy = pickle.load(f)
116 self.assertTrue(model == model_copy)
119 if __name__ ==
"__main__":