Skip to content

Commit 88c688b

Browse files
juelgjuelg
authored
Merge pull request #170 from utn-mi/juelg/pinocchio-ik
Pinocchio IK
2 parents b65a950 + 520b01e commit 88c688b

23 files changed

Lines changed: 235 additions & 94 deletions

‎CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,7 @@ include(FetchContent)
4646
find_package(Eigen3 REQUIRED)
4747
find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
4848
find_package(MuJoCo REQUIRED)
49-
49+
find_package(pinocchio REQUIRED)
5050
FetchContent_Declare(
5151
libfranka
5252
GIT_REPOSITORY https://github.com/frankaemika/libfranka.git

‎Makefile‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -27,8 +27,8 @@ stubgen:
2727
pybind11-stubgen -o python --numpy-array-use-type-var rcs
2828
find ./python -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/'
2929
find ./python -not -path "./python/rcs/_core/*" -name '*.pyi' -delete
30-
find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/typing\.Literal[\1]/g'
31-
find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/\1/g'
30+
find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g'
31+
find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g'
3232
ruff check --fix python/rcs/_core
3333
isort python/rcs/_core
3434
black python/rcs/_core

‎cmake/Findpinocchio.cmake‎

Lines changed: 21 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@ if (NOT pinocchio_FOUND)
1818
endif()
1919

2020
# Check if the library file exists
21-
cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio.so OUTPUT_VARIABLE pinocchio_library_path)
21+
cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio_default.so OUTPUT_VARIABLE pinocchio_library_path)
2222
if (NOT EXISTS ${pinocchio_library_path})
2323
set(pinocchio_FOUND FALSE)
2424
if (pinocchio_FIND_REQUIRED)
@@ -27,6 +27,16 @@ if (NOT pinocchio_FOUND)
2727
return()
2828
endif()
2929

30+
# Check if the library file exists
31+
cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio_parsers.so OUTPUT_VARIABLE pinocchio_parsers_path)
32+
if (NOT EXISTS ${pinocchio_parsers_path})
33+
set(pinocchio_FOUND FALSE)
34+
if (pinocchio_FIND_REQUIRED)
35+
message(FATAL_ERROR "Could not find pinocchio parsers path. Please install pinocchio using pip.")
36+
endif()
37+
return()
38+
endif()
39+
3040
# Extract version from the library filename
3141
file(GLOB pinocchio_dist_info "${Python3_SITELIB}/pin-*.dist-info")
3242
cmake_path(GET pinocchio_dist_info FILENAME pinocchio_library_filename)
@@ -38,8 +48,17 @@ if (NOT pinocchio_FOUND)
3848
target_include_directories(pinocchio::pinocchio INTERFACE ${pinocchio_INCLUDE_DIRS})
3949
set_target_properties(
4050
pinocchio::pinocchio
41-
PROPERTIES
51+
PROPERTIES
4252
IMPORTED_LOCATION "${pinocchio_library_path}"
4353
)
54+
55+
add_library(pinocchio_parsers SHARED IMPORTED)
56+
target_include_directories(pinocchio_parsers INTERFACE ${pinocchio_INCLUDE_DIRS})
57+
set_target_properties(
58+
pinocchio_parsers
59+
PROPERTIES
60+
IMPORTED_LOCATION "${pinocchio_parsers_path}"
61+
)
4462
set(pinocchio_FOUND TRUE)
63+
4564
endif()

‎pyproject.toml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ dependencies = ["websockets>=11.0",
2424
# NOTE: when changing the mujoco version, also change it in requirements_dev.txt
2525
"mujoco==3.2.6",
2626
# NOTE: Same for pinocchio (pin)
27-
"pin==2.7.0",
27+
"pin==3.7.0",
2828
"tilburg-hand",
2929
"digit-interface",
3030
]

‎python/examples/fr3.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@ def main():
6060
if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
6161
simulation = sim.Sim(rcs.scenes["fr3_empty_world"]["mjb"])
6262
urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
63-
ik = rcs.common.IK(str(urdf_path))
63+
ik = rcs.common.RL(str(urdf_path))
6464
cfg = sim.SimRobotConfig()
6565
cfg.add_id("0")
6666
cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
@@ -92,7 +92,7 @@ def main():
9292

9393
else:
9494
urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
95-
ik = rcs.common.IK(str(urdf_path))
95+
ik = rcs.common.RL(str(urdf_path))
9696
robot = rcs.hw.FR3(ROBOT_IP, ik)
9797
robot_cfg = FR3Config()
9898
robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())

‎python/rcs/_core/common.pyi‎

Lines changed: 32 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,9 @@ __all__ = [
2121
"IdentityRotMatrix",
2222
"IdentityRotQuatVec",
2323
"IdentityTranslation",
24+
"Pin",
2425
"Pose",
26+
"RL",
2527
"RPY",
2628
"Robot",
2729
"RobotConfig",
@@ -62,11 +64,13 @@ class GripperState:
6264
pass
6365

6466
class IK:
65-
def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ...
66-
def forward(self, q0: numpy.ndarray[M, numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ...
67+
def forward(self, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ...
6768
def ik(
68-
self, pose: Pose, q0: numpy.ndarray[M, numpy.dtype[numpy.float64]], tcp_offset: Pose = ...
69-
) -> numpy.ndarray[M, numpy.dtype[numpy.float64]] | None: ...
69+
self, pose: Pose, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...
70+
) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]] | None: ...
71+
72+
class Pin(IK):
73+
def __init__(self, path: str, frame_id: str = "fr3_link8", urdf: bool = True) -> None: ...
7074

7175
class Pose:
7276
def __getstate__(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
@@ -80,26 +84,28 @@ class Pose:
8084
def __init__(
8185
self,
8286
rotation: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]],
83-
translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]],
87+
translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]],
8488
) -> None: ...
8589
@typing.overload
8690
def __init__(
8791
self,
88-
quaternion: numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]],
89-
translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]],
92+
quaternion: numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]],
93+
translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]],
9094
) -> None: ...
9195
@typing.overload
92-
def __init__(self, rpy: RPY, translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]) -> None: ...
96+
def __init__(
97+
self, rpy: RPY, translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]
98+
) -> None: ...
9399
@typing.overload
94100
def __init__(
95101
self,
96-
rpy_vector: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]],
97-
translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]],
102+
rpy_vector: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]],
103+
translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]],
98104
) -> None: ...
99105
@typing.overload
100-
def __init__(self, translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]) -> None: ...
106+
def __init__(self, translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]) -> None: ...
101107
@typing.overload
102-
def __init__(self, quaternion: numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]) -> None: ...
108+
def __init__(self, quaternion: numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]) -> None: ...
103109
@typing.overload
104110
def __init__(self, rpy: RPY) -> None: ...
105111
@typing.overload
@@ -118,11 +124,14 @@ class Pose:
118124
def limit_translation_length(self, max_length: float) -> Pose: ...
119125
def pose_matrix(self) -> numpy.ndarray[tuple[typing.Literal[4], typing.Literal[4]], numpy.dtype[numpy.float64]]: ...
120126
def rotation_m(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ...
121-
def rotation_q(self) -> numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]: ...
127+
def rotation_q(self) -> numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]: ...
122128
def rotation_rpy(self) -> RPY: ...
123129
def total_angle(self) -> float: ...
124-
def translation(self) -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ...
125-
def xyzrpy(self) -> numpy.ndarray[typing.Literal[6], numpy.dtype[numpy.float64]]: ...
130+
def translation(self) -> numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]: ...
131+
def xyzrpy(self) -> numpy.ndarray[tuple[typing.Literal[6]], numpy.dtype[numpy.float64]]: ...
132+
133+
class RL(IK):
134+
def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ...
126135

127136
class RPY:
128137
pitch: float
@@ -133,11 +142,11 @@ class RPY:
133142
@typing.overload
134143
def __init__(self, roll: float = 0.0, pitch: float = 0.0, yaw: float = 0.0) -> None: ...
135144
@typing.overload
136-
def __init__(self, rpy: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]) -> None: ...
145+
def __init__(self, rpy: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]) -> None: ...
137146
def __setstate__(self, arg0: tuple) -> None: ...
138147
def __str__(self) -> str: ...
139-
def as_quaternion_vector(self) -> numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]: ...
140-
def as_vector(self) -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ...
148+
def as_quaternion_vector(self) -> numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]: ...
149+
def as_vector(self) -> numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]: ...
141150
def is_close(self, other: RPY, eps: float = 1e-08) -> bool: ...
142151
def rotation_matrix(
143152
self,
@@ -147,13 +156,13 @@ class Robot:
147156
def get_base_pose_in_world_coordinates(self) -> Pose: ...
148157
def get_cartesian_position(self) -> Pose: ...
149158
def get_ik(self) -> IK | None: ...
150-
def get_joint_position(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ...
159+
def get_joint_position(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ...
151160
def get_parameters(self) -> RobotConfig: ...
152161
def get_state(self) -> RobotState: ...
153162
def move_home(self) -> None: ...
154163
def reset(self) -> None: ...
155164
def set_cartesian_position(self, pose: Pose) -> None: ...
156-
def set_joint_position(self, q: numpy.ndarray[M, numpy.dtype[numpy.float64]]) -> None: ...
165+
def set_joint_position(self, q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]) -> None: ...
157166
def to_pose_in_robot_coordinates(self, pose_in_world_coordinates: Pose) -> Pose: ...
158167
def to_pose_in_world_coordinates(self, pose_in_robot_coordinates: Pose) -> Pose: ...
159168

@@ -168,7 +177,7 @@ class RobotMetaConfig:
168177
@property
169178
def joint_limits(self) -> numpy.ndarray[tuple[typing.Literal[2], N], numpy.dtype[numpy.float64]]: ...
170179
@property
171-
def q_home(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ...
180+
def q_home(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ...
172181

173182
class RobotPlatform:
174183
"""
@@ -236,8 +245,8 @@ class RobotType:
236245

237246
def FrankaHandTCPOffset() -> numpy.ndarray[tuple[typing.Literal[4], typing.Literal[4]], numpy.dtype[numpy.float64]]: ...
238247
def IdentityRotMatrix() -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ...
239-
def IdentityRotQuatVec() -> numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]: ...
240-
def IdentityTranslation() -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ...
248+
def IdentityRotQuatVec() -> numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]: ...
249+
def IdentityTranslation() -> numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]: ...
241250
def _bootstrap_egl(fn_addr: int, display: int, context: int) -> None: ...
242251
def robots_meta_config(robot_type: RobotType) -> RobotMetaConfig: ...
243252

‎python/rcs/_core/hw/__init__.pyi‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,7 @@ class FR3(rcs._core.common.Robot):
5555
def __init__(self, ip: str, ik: rcs._core.common.IK | None = None) -> None: ...
5656
def automatic_error_recovery(self) -> None: ...
5757
def controller_set_joint_position(
58-
self, desired_q: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]
58+
self, desired_q: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]]
5959
) -> None: ...
6060
def double_tap_robot_to_continue(self) -> None: ...
6161
def get_parameters(self) -> FR3Config: ...
@@ -91,7 +91,7 @@ class FR3Config(rcs._core.common.RobotConfig):
9191
def __init__(self) -> None: ...
9292

9393
class FR3Load:
94-
f_x_cload: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]] | None
94+
f_x_cload: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] | None
9595
load_inertia: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]] | None
9696
load_mass: float
9797
def __init__(self) -> None: ...

‎python/rcs/_core/sim.pyi‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -67,9 +67,9 @@ class CameraType:
6767
class FrameSet:
6868
def __init__(self) -> None: ...
6969
@property
70-
def color_frames(self) -> dict[str, numpy.ndarray[M, numpy.dtype[numpy.uint8]]]: ...
70+
def color_frames(self) -> dict[str, numpy.ndarray[tuple[M], numpy.dtype[numpy.uint8]]]: ...
7171
@property
72-
def depth_frames(self) -> dict[str, numpy.ndarray[M, numpy.dtype[numpy.float32]]]: ...
72+
def depth_frames(self) -> dict[str, numpy.ndarray[tuple[M], numpy.dtype[numpy.float32]]]: ...
7373
@property
7474
def timestamp(self) -> float: ...
7575

@@ -142,7 +142,7 @@ class SimRobot(rcs._core.common.Robot):
142142
) -> None: ...
143143
def get_parameters(self) -> SimRobotConfig: ...
144144
def get_state(self) -> SimRobotState: ...
145-
def set_joints_hard(self, q: numpy.ndarray[M, numpy.dtype[numpy.float64]]) -> None: ...
145+
def set_joints_hard(self, q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]) -> None: ...
146146
def set_parameters(self, cfg: SimRobotConfig) -> bool: ...
147147

148148
class SimRobotConfig(rcs._core.common.RobotConfig):
@@ -173,9 +173,9 @@ class SimRobotState(rcs._core.common.RobotState):
173173
@property
174174
def is_moving(self) -> bool: ...
175175
@property
176-
def previous_angles(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ...
176+
def previous_angles(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ...
177177
@property
178-
def target_angles(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ...
178+
def target_angles(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ...
179179

180180
default_free: CameraType # value = <CameraType.default_free: 3>
181181
fixed: CameraType # value = <CameraType.fixed: 2>

‎python/rcs/envs/creators.py‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -84,7 +84,7 @@ def __call__( # type: ignore
8484
"""
8585
if urdf_path is None:
8686
urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
87-
ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None
87+
ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None
8888
robot = rcs.hw.FR3(ip, ik)
8989
robot.set_parameters(robot_cfg)
9090

@@ -136,7 +136,7 @@ def __call__( # type: ignore
136136
) -> gym.Env:
137137

138138
urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
139-
ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None
139+
ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None
140140
robots: dict[str, rcs.hw.FR3] = {}
141141
for ip in ips:
142142
robots[ip] = rcs.hw.FR3(ip, ik)
@@ -232,7 +232,7 @@ def __call__( # type: ignore
232232
mjcf = rcs.scenes[mjcf]["mjb"]
233233
simulation = sim.Sim(mjcf)
234234

235-
ik = rcs.common.IK(str(urdf_path))
235+
ik = rcs.common.RL(str(urdf_path))
236236
robot = rcs.sim.SimRobot(simulation, ik, robot_cfg)
237237
env: gym.Env = RobotEnv(robot, control_mode)
238238
env = RobotSimWrapper(env, simulation, sim_wrapper)

‎python/rcs/envs/sim.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -204,7 +204,7 @@ def env_from_xml_paths(
204204
# TODO: this needs to support non FR3 robots
205205
assert isinstance(env.unwrapped, RobotEnv)
206206
simulation = sim.Sim(mjmld)
207-
ik = rcs.common.IK(urdf, max_duration_ms=300)
207+
ik = rcs.common.RL(urdf, max_duration_ms=300)
208208
cfg = default_fr3_sim_robot_cfg(mjmld, id)
209209
cfg.realtime = False
210210
if tcp_offset is not None:

0 commit comments

Comments
 (0)