Skip to content

test: pinoccio kinematics #232

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
juelg merged 1 commit into from
Nov 21, 2025
Merged

test: pinoccio kinematics #232

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion .clang-tidy
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@
Checks: 'clang-diagnostic-*,clang-analyzer-*,bug-prone-*,-clang-diagnostic-ignored-optimization-argument'
WarningsAsErrors: ''
HeaderFilterRegex: 'src/*.h'
AnalyzeTemporaryDtors: false
FormatStyle: google
CheckOptions:
- key: llvm-else-after-return.WarnOnConditionVariables
Expand Down
24 changes: 21 additions & 3 deletions assets/panda/mjcf/panda_0.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,22 @@
<mujoco model="panda">
<default>
<default class="panda">
<material specular="0.5" shininess="0.25"/>
<joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973"/>
<general dyntype="none" biastype="affine" ctrlrange="-2.8973 2.8973" forcerange="-87 87"/>
<default class="finger">
<joint axis="0 1 0" type="slide" range="0 0.04"/>
</default>

<default class="panda/visual">
<geom type="mesh" contype="0" conaffinity="0" group="2"/>
</default>
<default class="panda/collision">
<geom type="mesh" group="3"/>
</default>
<site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
</default>
</default>
<worldbody>
<light name="top_0" pos="0 0 2" mode="trackcom"/>
<body name="link0_0" childclass="panda">
Expand Down Expand Up @@ -91,9 +109,9 @@
<geom mesh="link7_6" material="dark_grey" class="panda/visual"/>
<geom mesh="link7_7" material="white" class="panda/visual"/>
<geom mesh="link7_c" class="panda/collision"/>
<body name="attachment_0" pos="0 0 0.107" quat="0.3826834 0 0 0.9238795">
<site name="attachment_site_0"/>
</body>
<site name="attachment_site_0" pos="0 0 0.107" quat="0.3826834 0 0 0.9238795" />
<!-- <site name="attachment_site_0"/>
</body> -->
</body>
</body>
</body>
Expand Down
18 changes: 0 additions & 18 deletions assets/panda/mjcf/panda_common.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,24 +3,6 @@

<option integrator="implicitfast" impratio="10"/>

<default>
<default class="panda">
<material specular="0.5" shininess="0.25"/>
<joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973"/>
<general dyntype="none" biastype="affine" ctrlrange="-2.8973 2.8973" forcerange="-87 87"/>
<default class="finger">
<joint axis="0 1 0" type="slide" range="0 0.04"/>
</default>

<default class="panda/visual">
<geom type="mesh" contype="0" conaffinity="0" group="2"/>
</default>
<default class="panda/collision">
<geom type="mesh" group="3"/>
</default>
<site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
</default>
</default>

<asset>
<material class="panda" name="white" rgba="1 1 1 1"/>
Expand Down
18 changes: 18 additions & 0 deletions assets/panda/mjcf/panda_unnamed.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,22 @@
<mujoco model="panda">
<default>
<default class="panda">
<material specular="0.5" shininess="0.25"/>
<joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973"/>
<general dyntype="none" biastype="affine" ctrlrange="-2.8973 2.8973" forcerange="-87 87"/>
<default class="finger">
<joint axis="0 1 0" type="slide" range="0 0.04"/>
</default>

<default class="panda/visual">
<geom type="mesh" contype="0" conaffinity="0" group="2"/>
</default>
<default class="panda/collision">
<geom type="mesh" group="3"/>
</default>
<site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
</default>
</default>
<worldbody>
<light name="top" pos="0 0 2" mode="trackcom"/>
<body name="link0" childclass="panda">
Expand Down
79 changes: 79 additions & 0 deletions python/tests/test_kinematics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,79 @@
import numpy as np
import pytest

import rcs
from rcs import common

# Map robot types to their end-effector frame names
ROBOT_FRAME_IDS = {
common.RobotType.FR3: "attachment_site_0",
common.RobotType.Panda: "attachment_site_0",
common.RobotType.XArm7: "attachment_site",
common.RobotType.SO101: "gripper",
common.RobotType.UR5e: "attachment_site",
}


# only for scene that have empty_world in key
@pytest.mark.parametrize("scene_name", [k for k in rcs.scenes if "empty_world" in k])
def test_kinematics_identity(scene_name):
scene = rcs.scenes[scene_name]

# Determine model path and type
model_path = scene.mjcf_robot

# Get frame ID
if scene.robot_type not in ROBOT_FRAME_IDS:
pytest.skip(f"Frame ID not defined for robot type {scene.robot_type}")

frame_id = ROBOT_FRAME_IDS[scene.robot_type]

# Initialize Pinocchio interface
try:
pin = common.Pin(model_path, frame_id, False)
except Exception as e:
pytest.fail(f"Failed to initialize Pin for {scene_name}: {e}")

# Get home configuration
try:
meta_config = common.robots_meta_config(scene.robot_type)
q_home = meta_config.q_home
except Exception as e:
pytest.fail(f"Failed to get meta config for {scene_name}: {e}")

# Test 1: FK at home
# Identity pose (no TCP offset)
tcp_offset = common.Pose()

pose_home = pin.forward(q_home, tcp_offset)
assert isinstance(pose_home, common.Pose)

# Test 2: IK at home pose should return a solution (ideally close to q_home, but IK is redundant)
# We use q_home as initial guess
q_sol = pin.inverse(pose_home, q_home, tcp_offset)

assert q_sol is not None, "IK failed for home pose"

# Verify the solution with FK
pose_sol = pin.forward(q_sol, tcp_offset)

# Check if pose_sol is close to pose_home
assert pose_sol.is_close(
pose_home, eps_r=1e-4, eps_t=1e-4
), f"FK(IK(pose)) does not match pose.\nOriginal: {pose_home}\nResult: {pose_sol}"

# Test 3: Perturbed configuration
# Add small noise to q_home to test non-trivial pose
# Ensure we stay within limits if possible, but for small noise it should be fine
np.random.seed(42)
q_perturbed = q_home + np.random.uniform(-0.1, 0.1, size=q_home.shape)

pose_perturbed = pin.forward(q_perturbed, tcp_offset) # type: ignore
q_sol_perturbed = pin.inverse(pose_perturbed, q_home, tcp_offset) # Use q_home as seed

assert q_sol_perturbed is not None, "IK failed for perturbed pose"

pose_sol_perturbed = pin.forward(q_sol_perturbed, tcp_offset)
assert pose_sol_perturbed.is_close(
pose_perturbed, eps_r=1e-3, eps_t=1e-3
), f"FK(IK(perturbed_pose)) does not match.\nOriginal: {pose_perturbed}\nResult: {pose_sol_perturbed}"