Lorenzo manipulation floor release by LBarbieri24 · Pull Request #97 · pal-robotics/pal_mjlab

LBarbieri24 · 2026-07-20T09:37:29Z

No description provided.

…for a cnn standalone training

…ilities

saikishor · 2026-07-23T13:40:32Z

@@ -1 +1 @@
-3.13
+3.11


Change it back to 13 or 14

saikishor · 2026-07-23T13:40:45Z

    "mjlab>=1.5.2",
-    "moviepy>=2.0"
+    "moviepy>=2.0",
+    "onnxruntime<1.24",


Explain why this change

I don't know what was the problem back at the beginning but now after removing "onnxruntime<1.24" it doesn't seem to appear an error anymore.

saikishor · 2026-07-23T14:32:10Z

+      output_blocks.append(display_curriculum(task, curriculum, args.format))
+
+    terminations = collect_terminations(task, play=args.play)
+    if terminations is not None:
+      output_blocks.append(display_terminations(task, terminations, args.format))
+
+    noise = collect_noise(task, play=args.play)
+    if noise is not None:
+      output_blocks.append(display_noise(task, noise, args.format))
+
+  full_output = "\n".join(output_blocks)
+
+  if args.output:
+    with open(args.output, "w") as f:
+      f.write(full_output)
+    print(f"Results successfully written to {args.output}")
+  else:
+    print(full_output)
+
+
+if __name__ == "__main__":
+  main()


Do we need this script?

We don't actually. These were all utility scripts used mainly for debugging. I have removed them

saikishor · 2026-07-23T14:32:24Z

+#!/usr/bin/env python3
+"""
+Script to evaluate a trained policy on 100 episodes and record key metrics:
+- success rate
+- number of episodes with top surface collisions
+- mean fingertip angles wrt cube lateral surfaces the instant before contact_both_fingers
+
+Supports both ground truth state feedback and a hybrid YOLO-based estimation mode.
+"""
+


Do we need this script too?

saikishor · 2026-07-23T14:33:09Z

+def main():
+  parser = argparse.ArgumentParser(
+    description="Export a .pt checkpoint to .onnx with metadata"
+  )
+  parser.add_argument(
+    "--task",
+    type=str,
+    required=True,
+    help="Task name (e.g., Mjlab-Manipulation-Lift-Cube-Pal-Tiago-Pro-v0)",
+  )
+  parser.add_argument(
+    "--checkpoint", type=str, required=True, help="Path to the .pt checkpoint file"
+  )
+  parser.add_argument(
+    "--output",
+    type=str,
+    default=None,
+    help="Optional custom output path for the .onnx file (defaults to same folder/name as checkpoint)",
+  )
+  parser.add_argument(
+    "--device",
+    type=str,
+    default="cpu",
+    help="Device to load the model on (default: cpu)",
+  )


Why do you have to do it with a script?

I used it to convert middle .pt checkpoints into .onnx

saikishor · 2026-07-23T14:35:00Z

+import argparse
+import math
+import os
+import sys
+
+import cv2
+import mjlab.tasks  # noqa: F401
+import numpy as np
+import torch
+from mjlab.envs import ManagerBasedRlEnv
+from mjlab.rl import RslRlVecEnvWrapper
+from mjlab.sensor import CameraSensorCfg
+from mjlab.tasks.registry import load_env_cfg, load_rl_cfg
+from mjlab.utils.lab_api.math import euler_xyz_from_quat, quat_apply, quat_inv
+from mjlab.utils.torch import configure_torch_backends
+from mjlab.viewer import NativeMujocoViewer, ViserPlayViewer
+
+# Sourcing the filters from the workspace
+sys.path.append("/home/lorenzobarbieri/exchange/tiago_pro_sim_ws/src")
+try:


saikishor · 2026-07-23T14:36:00Z

+            <!-- <camera name="head_realsense_camera" pos="0.0565 -0.168 0" euler="0 -1.57 0" fovy="60"/> -->
          </body>
        </body>
+        <!--


WHy the whole block is commented?

saikishor · 2026-07-23T14:39:37Z

+class TiagoProRobot:
+  entity_cfg: EntityCfg = field(default_factory=get_tiago_pro_robot_cfg)
+  arm_joint_pattern: str = "arm_right_.*_joint"
+  gripper_joint_pattern: str = "gripper_right_finger_joint"
+  ee_site: str = "gripper_right_grasping_site"
+  fingertip_geom_pattern: str = "col_right_fingertip_.*"
+  fingertip_site_pattern: str = "gripper_right_fingertip_.*_site"
+  collision_link_pattern: str = "(arm_right|gripper_right)_.*_link"
+  arm_collision_link_pattern: str = "arm_right_.*_link"
+  gripper_collision_link_pattern: str = "gripper_right_.*_link"
+  viewer_body: str = "base_footprint"
+  camera_name: str = "head_realsense_camera"
+  wrist_camera_name: str = "wrist_realsense_camera"
+  head_camera_name: str = "head_realsense_camera"
+
+  def arm_action_cfg(self) -> Any:
+    from mjlab.envs.mdp.actions import DifferentialIKActionCfg
+
+    return DifferentialIKActionCfg(
+      entity_name="robot",
+      actuator_names=(self.arm_joint_pattern,),
+      frame_name=self.ee_site,
+      frame_type="site",
+      delta_pos_scale=0.005,  # Max displacement of 0.01m per step (0.5m/s max velocity)
+      delta_ori_scale=0.005,  # Max rotation of 0.01 rad per step (0.5 rad/s max angular velocity)
+    )
+


What this file used for?

I incorporated it in tiago_pro_constants.py now.

Copilot

Pull request overview

Adds a PAL TIAGo Pro manipulation “lift cube” RL task to the pal_mjlab task suite, including environment/MDP definitions, PPO runner configuration, and robot model/actuation updates to support the new setup.

Changes:

Introduces a new TIAGo Pro lift-cube task package (env cfg, PPO cfg, task registration) plus a manipulation MDP module (commands/obs/rewards/events/terminations/metrics).
Updates the TIAGo Pro MuJoCo XML and robot constants (actuators, collisions, initial state, camera) to match the manipulation setup.
Adds runner/export monkey patches (metadata + ONNX loading) and updates project dependencies / ignore rules.

Reviewed changes

Copilot reviewed 19 out of 22 changed files in this pull request and generated 6 comments.

Show a summary per file

File	Description
src/pal_mjlab/tasks/manipulation/tiago_pro/rl_cfg.py	Adds PPO runner/model hyperparameter config for the lift task.
src/pal_mjlab/tasks/manipulation/tiago_pro/env_cfgs.py	Defines the TIAGo Pro lift environment (scene/actions/obs/rewards/DR/terminations).
src/pal_mjlab/tasks/manipulation/tiago_pro/init.py	Registers the TIAGo Pro lift task in the mjlab task registry.
src/pal_mjlab/tasks/manipulation/README.md	Documents the manipulation task layout and tuning notes.
src/pal_mjlab/tasks/manipulation/mdp/utils.py	Adds `nan_safe` helper for reward sanitization.
src/pal_mjlab/tasks/manipulation/mdp/terminations.py	Adds manipulation termination terms (success/failure + penetration).
src/pal_mjlab/tasks/manipulation/mdp/rewards.py	Adds shaped reward/penalty terms for reaching/grasping/lifting/releasing.
src/pal_mjlab/tasks/manipulation/mdp/observations.py	Adds manipulation observation terms (object pose, EE pose, contact flags).
src/pal_mjlab/tasks/manipulation/mdp/metrics.py	Adds metrics terms for logging success, errors, and distances.
src/pal_mjlab/tasks/manipulation/mdp/events.py	Adds reset logic and DR utilities (e.g., table height randomization).
src/pal_mjlab/tasks/manipulation/mdp/curriculums.py	Adds a placeholder curriculum module.
src/pal_mjlab/tasks/manipulation/mdp/contact_sensor.py	Adds fingertip proximity/contact helper used by obs/rewards/terms.
src/pal_mjlab/tasks/manipulation/mdp/commands.py	Implements the lifting command term and procedural table/box specs.
src/pal_mjlab/tasks/manipulation/mdp/init.py	Re-exports manipulation MDP submodules.
src/pal_mjlab/tasks/manipulation/init.py	Introduces the manipulation task package entrypoint.
src/pal_mjlab/tasks/init.py	Adds monkey patches for exporter metadata and ONNX inference loading.
src/pal_mjlab/robots/pal_tiago_pro/xmls/tiago_pro.xml	Updates TIAGo Pro XML (gripper structure, collisions, camera, etc.).
src/pal_mjlab/robots/pal_tiago_pro/tiago_pro_constants.py	Updates TIAGo Pro actuator/collision setup and exports a `TiagoProRobot` helper.
src/pal_mjlab/robots/init.py	Exposes `TiagoProRobot` from the robots package.
pyproject.toml	Adds dependencies and configures uv sources/indexes for torch wheels.
.gitignore	Ignores additional artifacts (datasets, zips, results, scripts).

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

+def task_success_reward(
+  env: ManagerBasedRlEnv,
+  command_name: str,
+  floor_z: float = 0.1,
+) -> torch.Tensor:
+  """Returns 1.0 if the object has reached the target and fallen to the floor, else 0.0."""
+  return object_released_on_floor_term(env, command_name, floor_z).float()


+  def get_top_penetration_depth(p_local: torch.Tensor) -> torch.Tensor:
+    x = p_local[:, 0]
+    y = p_local[:, 1]
+    z = p_local[:, 2]
+
+    is_inside = (
+      (torch.abs(x) <= half_x) & (torch.abs(y) <= half_y) & (torch.abs(z) <= half_z)
+    )
+
+    # Distances to each face
+    dist_x = half_x - torch.abs(x)
+    dist_y = half_y - torch.abs(y)
+    dist_z = half_z - torch.abs(z)
+
+    dists = torch.stack([dist_x, dist_y, dist_z], dim=-1)
+    min_dist, min_axis = torch.min(dists, dim=-1)
+
+    # Condition: inside, closest to top face (min_axis == 2) and in upper half (z > 0)
+    is_top_penetration = is_inside & (min_axis == 2) & (z > 0)
+
+    # Return penetration depth if top penetration, else 0.0
+    return torch.where(is_top_penetration, min_dist, torch.zeros_like(min_dist))
+


+  joint_action = env.action_manager.get_term("joint_pos")
+  assert isinstance(joint_action, BaseAction)


+[[tool.uv.index]]
+name = "pytorch-cu129"
+url = "https://download.pytorch.org/whl/cu129"
+explicit = true
+
+[tool.uv.sources]
+torch = [
+  { index = "pytorch-cu129", marker = "extra == 'cu128' and sys_platform == 'linux'" }
+]
+torchvision = [
+  { index = "pytorch-cu129", marker = "extra == 'cu128' and sys_platform == 'linux'" }
+]


+#### Core Task Registration & Configs (`tiago_pro/`)
+- [`tiago_pro/__init__.py`](file:///home/lorenzobarbieri/pal_mjlab_manipulation/pal_mjlab/src/pal_mjlab/tasks/manipulation/tiago_pro/__init__.py): Registers `Mjlab-Manipulation-Lift-Cube-Pal-Tiago-Pro-v0` into the `mjlab` task registry using `ManipulationOnPolicyRunner`.
+- [`tiago_pro/env_cfgs.py`](file:///home/lorenzobarbieri/pal_mjlab_manipulation/pal_mjlab/src/pal_mjlab/tasks/manipulation/tiago_pro/env_cfgs.py): Defines `lift_env_cfg()`, configuring simulation parameters ($dt = 0.005\text{ s}$, decimation = 4 $\rightarrow 50\text{ Hz}$ control rate, 4s episodes), scene entities (robot, table, box), sensor suite, asymmetric actor/critic observations, reward terms, domain randomizations, and terminations.
+- [`tiago_pro/rl_cfg.py`](file:///home/lorenzobarbieri/pal_mjlab_manipulation/pal_mjlab/src/pal_mjlab/tasks/manipulation/tiago_pro/rl_cfg.py): Defines `lift_ppo_runner_cfg()`, providing hyperparameter settings for PPO training via RSL-RL (Actor/Critic MLPs: `512 x 256 x 128`, ELU activations, adaptive LR schedule).
+
+#### MDP Infrastructure (`mdp/`)
+- [`mdp/commands.py`](file:///home/lorenzobarbieri/pal_mjlab_manipulation/pal_mjlab/src/pal_mjlab/tasks/manipulation/mdp/commands.py): Implements `LiftingCommand` and `LiftingCommandCfg`. Generates random box spawn poses and target goal 3D positions in space, procedural MuJoCo XML specs for table (`get_table_spec`) and box (`get_box_spec`), and manages episode metrics (`reached`, `at_goal_time`, `grasped_distance`).
+- [`mdp/contact_sensor.py`](file:///home/lorenzobarbieri/pal_mjlab_manipulation/pal_mjlab/src/pal_mjlab/tasks/manipulation/mdp/contact_sensor.py): Provides `site_contact_both_fingers()`, which checks proximity and contact between both right fingertips and the target object.
+- [`mdp/events.py`](file:///home/lorenzobarbieri/pal_mjlab_manipulation/pal_mjlab/src/pal_mjlab/tasks/manipulation/mdp/events.py): Contains custom domain randomization terms including `randomize_table_height` (shifts table top while keeping it grounded) and `reset_joints_mixed` (initializes arm joints near default or goal configurations).
+- [`mdp/observations.py`](file:///home/lorenzobarbieri/pal_mjlab_manipulation/pal_mjlab/src/pal_mjlab/tasks/manipulation/mdp/observations.py): Features spatial observation calculators relative to the robot root frame (`object_position_in_robot_root_frame`, `object_yaw_in_robot_root_frame`, `ee_position_in_robot_base_frame`, `reached_flag`, `object_both__contact_fingers`).
+- [`mdp/rewards.py`](file:///home/lorenzobarbieri/pal_mjlab_manipulation/pal_mjlab/src/pal_mjlab/tasks/manipulation/mdp/rewards.py): Collection of shaped reward terms (`reaching_object`, `gripper_open_during_approach`, `lifting_object`, `object_goal_tracking`, `post_reached_ee_stability`, `post_reached_gripper_open`) and penalty terms (`top_surface_penetration_penalty`, `object_table_sliding_penalty`, `fingertip_cube_alignment_reward_adaptive`, `arm_right_1_joint_limit_penalty`, `self_collisions`).
+- [`mdp/terminations.py`](file:///home/lorenzobarbieri/pal_mjlab_manipulation/pal_mjlab/src/pal_mjlab/tasks/manipulation/mdp/terminations.py): Defines episode termination conditions (`object_released_on_floor_term`, `cube_contact_with_table_after_reached_term`, `cube_fell_off_table_term`, `top_surface_penetration_term`, `nan_term`).
+- [`mdp/utils.py`](file:///home/lorenzobarbieri/pal_mjlab_manipulation/pal_mjlab/src/pal_mjlab/tasks/manipulation/mdp/utils.py): Provides `@nan_safe` wrapper decorator to sanitize potential NaNs/Infs in reward outputs.


+#   - Base:      col_base
+#   - Torso/Head:col_torso_head
+#   - Right arm joints 3, 5, 7: col_upper_right_arm, col_lower_right_arm, col_arm_right_7
+# (right arm joint 1 has no collision geom in the XML)


LBarbieri24 added 30 commits July 20, 2026 11:42

tiago pro implementation

abb8af5

small change

163553b

removed left arm

a866379

removed all the not used camera datas from the simulation

f3d44ee

adjusted timeout time

228f0ad

reduced reaching std and added contact penalizations with the table

ac93f25

fixed gripper thing

585b209

head camera and maybe a reset fix

91f4141

fixed reset problem and removed termination

314d3ab

cube falling and joint vel regulation

d35be83

reducing force threshold

8989650

10 seconds episode

ed69c83

added gripper and commented out everything to go back to initial setup

2ae1ed6

ups

51f871c

solve

b31117b

almost static start and just lifting

b54776c

moved the gripper's contact sites, changed the cnn and initial setup …

3c0dd4d

…for a cnn standalone training

pretrained cnn. Change the env_cfgs and rl before training on kirby

60b14e3

omniscent setup working. Also added two different cnn training possib…

20725dd

…ilities

trained cnn and convnext, rearranged folders

4de8867

different velocity reduction method

9f39ab2

correct camera orientation and added environment diversity

9622d01

switched to joint pos commands

be023ac

small correction

62cabc3

back to original reaching reward. changed some weights

83cdcd6

fixed environment randomization

f749c9c

fixed keypoints synthetization

f996eab

changed gripper's control to relative pos. Starting open

f3975b8

tries on kirby. Not randomizing anything

8e72f71

made a bit of order and fixed the scale

dc1c9cf

uv sync after rebase

cd81d16

LBarbieri24 force-pushed the lorenzo_manipulation_floor_release branch from e46c436 to cd81d16 Compare July 20, 2026 09:57

LBarbieri24 and others added 9 commits July 20, 2026 12:13

Tests fixes

4b98dc8

enforce onnx_runtime version

76f9bd5

success changes

9487a31

nan instabilities

b58f240

observation expansion

d026655

fix for kirby

6e4c63e

table cube termination and metrics fix

9697c81

tests withut action rate

11eb00b

contact reward changed

24c4c45

saikishor reviewed Jul 23, 2026

View reviewed changes

saikishor added 2 commits July 23, 2026 16:14

Merge branch 'main' into lorenzo_manipulation_floor_release

1285690

Fix pre-commit changes

57fb4d5

saikishor reviewed Jul 23, 2026

View reviewed changes

saikishor requested changes Jul 23, 2026

View reviewed changes

saikishor reviewed Jul 23, 2026

View reviewed changes

lbarbieri25 added 8 commits July 23, 2026 18:15

cleaned up a bit

db9a629

removed the utility functions that weren't necessary

55e8f98

trying different truncation logic. Ulterior clean of the code

7cf190d

fixed tests problems

f20e755

removed the freezed reward mechanism

bcf93f6

solved test error

84a468d

added a readme to the manipulation task folder

760a65a

fix to pass test check

77b68af

LBarbieri24 marked this pull request as ready for review July 30, 2026 10:02

Copilot AI review requested due to automatic review settings July 30, 2026 10:02

Copilot started reviewing on behalf of LBarbieri24 July 30, 2026 10:02 View session

Copilot AI reviewed Jul 30, 2026

View reviewed changes

Navigation Menu

Uh oh!

Lorenzo manipulation floor release - #97

Lorenzo manipulation floor release#97
LBarbieri24 wants to merge 106 commits into
mainfrom
lorenzo_manipulation_floor_release

LBarbieri24 commented Jul 20, 2026

Uh oh!

saikishor Jul 23, 2026

Uh oh!

saikishor Jul 23, 2026

Uh oh!

LBarbieri24 Jul 23, 2026

Uh oh!

saikishor Jul 23, 2026

Uh oh!

LBarbieri24 Jul 23, 2026 •

edited

Loading

Uh oh!

saikishor Jul 23, 2026

Uh oh!

saikishor Jul 23, 2026

Uh oh!

LBarbieri24 Jul 23, 2026

Uh oh!

saikishor Jul 23, 2026

Uh oh!

saikishor Jul 23, 2026

Uh oh!

saikishor Jul 23, 2026

Uh oh!

LBarbieri24 Jul 23, 2026

Uh oh!

Copilot AI left a comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

		joint_action = env.action_manager.get_term("joint_pos")
		assert isinstance(joint_action, BaseAction)

		@@ -1 +1 @@
		3.13
		3.11

Uh oh!

Conversation

LBarbieri24 commented Jul 20, 2026

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

LBarbieri24 Jul 23, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

LBarbieri24 Jul 23, 2026 •

edited

Loading