contact-cholesky.py
Go to the documentation of this file.
1 from pathlib import Path
2 
3 import pinocchio as pin
4 
5 pinocchio_model_dir = Path(__file__).parent.parent / "models"
6 urdf_filename = (
7  pinocchio_model_dir
8  / "example-robot-data/robots/anymal_b_simple_description/robots/anymal.urdf"
9 )
10 model = pin.buildModelFromUrdf(urdf_filename)
11 data = model.createData()
12 
13 q0 = pin.neutral(model)
14 
15 # Add feet frames
16 feet_names = ["LH_FOOT", "RH_FOOT", "LF_FOOT", "RF_FOOT"]
17 feet_frame_ids = []
18 for foot_name in feet_names:
19  frame_id = model.getFrameId(foot_name)
20  feet_frame_ids.append(frame_id)
21 
22 contact_models = []
23 for fid in feet_frame_ids:
24  frame = model.frames[fid]
25  cmodel = pin.RigidConstraintModel(
26  pin.ContactType.CONTACT_3D,
27  frame.parent,
28  frame.placement,
29  pin.LOCAL_WORLD_ALIGNED,
30  )
31  contact_models.append(cmodel)
32 
33 contact_data = [cmodel.createData() for cmodel in contact_models]
34 
35 pin.initConstraintDynamics(model, data, contact_models)
36 pin.crba(model, data, q0)
37 
38 data.contact_chol.compute(model, data, contact_models, contact_data)
39 
40 delassus_matrix = data.contact_chol.getInverseOperationalSpaceInertiaMatrix()
41 delassus_matrix_inv = data.contact_chol.getOperationalSpaceInertiaMatrix()


pinocchio
Author(s):
autogenerated on Mon Dec 16 2024 03:41:00