update-model-after-urdf.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 #
4 # Copyright (C) 2023 Inria
5 
6 import numpy as np
7 
8 try:
9  from robot_descriptions.loaders.pinocchio import load_robot_description
10 except ModuleNotFoundError:
11  print("This example loads robot descriptions from robot_descriptions.py:")
12  print("\n\tpip install robot_descriptions")
13 
14 print("Goal: load a legged robot from its URDF, then modify leg lengths")
15 
16 print("Loading robot description from URDF...")
17 robot = load_robot_description("upkie_description")
18 model = robot.model
19 
20 # Joint placement offsets compared to the leg length
21 # We know from the robot description that both are along the local y-axis
22 known_offsets = {"knee": 0.072, "wheel": 0.065}
23 
24 
25 def check_limb_lengths(limb_length: float) -> bool:
26  print(f"Checking that limbs are {limb_length} m long... ", end="")
27  for side in ("left", "right"):
28  for joint in ("knee", "wheel"):
29  joint_id = model.getJointId(f"{side}_{joint}")
30  if not np.allclose(
31  model.jointPlacements[joint_id].translation[1],
32  limb_length - known_offsets[joint],
33  ):
34  print("{side}_{joint} placement is wrong!")
35  return False
36  return True
37 
38 
39 # We know from the description that the leg length should be 24 cm
40 if check_limb_lengths(0.24):
41  print("OK, the model is as we expect it")
42 
43 
44 def update_limb_lengths(length: float) -> None:
45  """Update femur and tibia lengths in the robot model.
46 
47  Args:
48  length: New femur and tibia length, in meters.
49  """
50  for side in ("left", "right"):
51  for joint in ("knee", "wheel"):
52  joint_id = model.getJointId(f"{side}_{joint}")
53  model.jointPlacements[joint_id].translation[1] = (
54  length - known_offsets[joint]
55  )
56 
57 
58 update_limb_lengths(0.3) # update femurs and tibias to 30 cm
59 if check_limb_lengths(0.3):
60  print("OK, the update worked!")


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:33