5 import pinocchio
as pin
6 from test_case
import PinocchioTestCase
as TestCase
9 @unittest.skipUnless(pin.WITH_URDFDOM,
"Needs URDFDOM")
12 self.
current_file = os.path.dirname(str(os.path.abspath(__file__)))
19 "example-robot-data/robots/talos_data",
36 self.
model = pin.buildModelFromUrdf(
40 self.
q0 = self.
model.referenceConfigurations[
"half_sitting"]
45 feet_name = [
"left_sole_link",
"right_sole_link"]
46 frame_ids = [model.getFrameId(frame_name)
for frame_name
in feet_name]
49 v = np.zeros((model.nv))
50 a = np.zeros((model.nv))
51 data = model.createData()
53 contact_models_list = []
54 contact_datas_list = []
57 contact_models_vec = pin.StdVec_RigidConstraintModel()
58 contact_datas_vec = pin.StdVec_RigidConstraintData()
59 cones_vec = pin.StdVec_CoulombFrictionCone()
61 for frame_id
in frame_ids:
62 frame = model.frames[frame_id]
63 contact_model = pin.RigidConstraintModel(
64 pin.ContactType.CONTACT_3D, model, frame.parentJoint, frame.placement
67 contact_models_list.append(contact_model)
68 contact_datas_list.append(contact_model.createData())
69 cones_list.append(pin.CoulombFrictionCone(0.4))
71 contact_models_vec.append(contact_model)
72 contact_datas_vec.append(contact_model.createData())
73 cones_vec.append(pin.CoulombFrictionCone(0.4))
76 for m
in contact_models_list:
77 constraint_dim += m.size()
80 R = np.zeros(constraint_dim)
81 constraint_correction = np.zeros(constraint_dim)
82 lambda_guess = np.zeros(constraint_dim)
83 prox_settings = pin.ProximalSettings(1e-12, 1e-6, 1)
87 tau1 = pin.contactInverseDynamics(
98 constraint_correction,
104 tau2 = pin.contactInverseDynamics(
115 constraint_correction,
121 tau3 = pin.contactInverseDynamics(
132 constraint_correction,
136 self.assertApprox(tau1, tau2)
137 self.assertApprox(tau1, tau3)
140 if __name__ ==
"__main__":