From e3c28fd9d6d6fb1920119238181f4b077673c36e Mon Sep 17 00:00:00 2001 From: Dan Wahl Date: Tue, 21 Jul 2026 19:23:35 -0500 Subject: [PATCH 01/21] feat(kinova_vla_test_sim): add Kinova Gen3 color-cube stacking sim config MuJoCo simulation of a Kinova Gen3 (7-DoF) + Robotiq 2F-85 stacking colored cubes, for testing VLA policy execution via ExecutePolicy. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01Xkm9aPVJ33L5QhRYs8M3u6 --- .../kinova_vla_test_sim/CMakeLists.txt | 20 + .../kinova_vla_test_sim/README.md | 5 + .../kinova_vla_test_sim/config/config.yaml | 39 + .../mujoco/assets/kinova/base_link.STL | 3 + .../mujoco/assets/kinova/base_link.mtl | 22 + .../mujoco/assets/kinova/base_link.obj | 3 + .../assets/kinova/bracelet_no_vision_link.STL | 3 + .../assets/kinova/bracelet_no_vision_link.mtl | 12 + .../assets/kinova/bracelet_no_vision_link.obj | 3 + .../kinova/bracelet_with_vision_link.STL | 3 + .../kinova/bracelet_with_vision_link.mtl | 12 + .../kinova/bracelet_with_vision_link.obj | 3 + .../assets/kinova/end_effector_link.STL | 3 + .../mujoco/assets/kinova/forearm_link.STL | 3 + .../mujoco/assets/kinova/forearm_link.mtl | 12 + .../mujoco/assets/kinova/forearm_link.obj | 3 + .../mujoco/assets/kinova/half_arm_1_link.STL | 3 + .../mujoco/assets/kinova/half_arm_1_link.mtl | 12 + .../mujoco/assets/kinova/half_arm_1_link.obj | 3 + .../mujoco/assets/kinova/half_arm_2_link.STL | 3 + .../mujoco/assets/kinova/half_arm_2_link.mtl | 12 + .../mujoco/assets/kinova/half_arm_2_link.obj | 3 + .../mujoco/assets/kinova/shoulder_link.STL | 3 + .../mujoco/assets/kinova/shoulder_link.mtl | 12 + .../mujoco/assets/kinova/shoulder_link.obj | 3 + .../assets/kinova/spherical_wrist_1_link.STL | 3 + .../assets/kinova/spherical_wrist_1_link.mtl | 12 + .../assets/kinova/spherical_wrist_1_link.obj | 3 + .../assets/kinova/spherical_wrist_2_link.STL | 3 + .../assets/kinova/spherical_wrist_2_link.mtl | 12 + .../assets/kinova/spherical_wrist_2_link.obj | 3 + .../mujoco/assets/rafti_finger.mtl | 12 + .../mujoco/assets/rafti_finger.obj | 3 + .../assets/rafti_finger_collision_1.mtl | 12 + .../assets/rafti_finger_collision_1.obj | 3 + .../assets/rafti_finger_collision_2.mtl | 12 + .../assets/rafti_finger_collision_2.obj | 3 + .../assets/rafti_finger_collision_3.mtl | 12 + .../assets/rafti_finger_collision_3.obj | 3 + .../assets/rafti_finger_collision_4.mtl | 12 + .../assets/rafti_finger_collision_4.obj | 3 + .../mujoco/assets/robotiq_2f85/base.stl | 3 + .../assets/robotiq_2f85/finger_link.stl | 3 + .../assets/robotiq_2f85/finger_tip_link.stl | 3 + .../robotiq_2f85/inner_knuckle_link.stl | 3 + .../assets/robotiq_2f85/knuckle_link.stl | 3 + .../mujoco/assets/robotiq_2f85/pad.stl | 3 + .../assets/robotiq_2f85/silicone_pad.stl | 3 + .../description/mujoco/cube_stack_scene.xml | 140 ++++ .../description/mujoco/gen3_7dof.xml | 731 ++++++++++++++++++ .../description/picknik_kinova_gen3.xacro | 696 +++++++++++++++++ .../objectives/execute_color_stack_policy.xml | 50 ++ .../kinova_vla_test_sim/package.xml | 30 + 53 files changed, 1982 insertions(+) create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/CMakeLists.txt create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/README.md create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/base_link.STL create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/base_link.mtl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/base_link.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_no_vision_link.STL create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_no_vision_link.mtl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_no_vision_link.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link.STL create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link.mtl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/end_effector_link.STL create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/forearm_link.STL create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/forearm_link.mtl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/forearm_link.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_1_link.STL create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_1_link.mtl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_1_link.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_2_link.STL create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_2_link.mtl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_2_link.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/shoulder_link.STL create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/shoulder_link.mtl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/shoulder_link.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_1_link.STL create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_1_link.mtl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_1_link.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_2_link.STL create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_2_link.mtl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_2_link.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger.mtl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_1.mtl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_1.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_2.mtl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_2.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_3.mtl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_3.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_4.mtl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_4.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/base.stl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/finger_link.stl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/finger_tip_link.stl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/inner_knuckle_link.stl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/knuckle_link.stl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/pad.stl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/silicone_pad.stl create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/cube_stack_scene.xml create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/gen3_7dof.xml create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/picknik_kinova_gen3.xacro create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/execute_color_stack_policy.xml create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/package.xml diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/CMakeLists.txt b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/CMakeLists.txt new file mode 100644 index 000000000..2fd804624 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.22) +project(kinova_vla_test_sim) + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY + config + description + objectives + DESTINATION + share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/README.md b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/README.md new file mode 100644 index 000000000..0d717c5b4 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/README.md @@ -0,0 +1,5 @@ +# kinova_vla_test_sim + +A MoveIt Pro configuration to simulate a Kinova Gen3 (7-DoF) + Robotiq 2F-85 stacking colored cubes in MuJoCo, for testing VLA policy execution. + +For detailed documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/) diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml new file mode 100644 index 000000000..f542ae1e2 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml @@ -0,0 +1,39 @@ +############################################################### +# +# This configures the robot to work with MoveIt Pro +# +############################################################### + +based_on_package: "kinova_sim" + +hardware: + simulated: True + launch_control_node: True + launch_robot_state_publisher: True + + robot_description: + urdf: + package: "kinova_vla_test_sim" + path: "description/picknik_kinova_gen3.xacro" + srdf: + package: "kinova_sim" + path: "config/moveit/picknik_kinova_gen3_base.srdf" + urdf_params: + - mujoco_keyframe: "default" + - arm: "gen3" + - dof: "7" + - vision: "false" + - gripper: "robotiq_2f_85" + - use_fake_hardware: "%>> hardware.simulated" + - fake_sensor_commands: "false" + - external_camera: "true" + - wrist_realsense: "true" + - use_internal_bus_gripper_comm: "false" + - mujoco_model: "description/mujoco/cube_stack_scene.xml" + - mujoco_viewer: false + +objectives: + objective_library_paths: + colorstack_objectives: + package_name: "kinova_vla_test_sim" + relative_path: "objectives" diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/base_link.STL b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/base_link.STL new file mode 100644 index 000000000..5f6aff0d1 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/base_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3e38517e653d58f4160cfdcee34c9dea983d8b3ecb8f2c147444f58235335260 +size 183884 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/base_link.mtl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/base_link.mtl new file mode 100644 index 000000000..a771bd2d5 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/base_link.mtl @@ -0,0 +1,22 @@ +# Blender 4.1.1 MTL File: 'None' +# www.blender.org + +newmtl Blue_002 +Ns 250.000000 +Ka 1.000000 1.000000 1.000000 +Kd 0.000000 0.000000 1.000000 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.000000 +d 1.000000 +illum 2 + +newmtl White_002 +Ns 250.000000 +Ka 1.000000 1.000000 1.000000 +Kd 1.000000 1.000000 1.000000 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.000000 +d 1.000000 +illum 2 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/base_link.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/base_link.obj new file mode 100644 index 000000000..502aa742f --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/base_link.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d209049c7784b700dacd3ff6b92e5a5e653b175b81cd40cdc3ac08c2c4115a63 +size 221550 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_no_vision_link.STL b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_no_vision_link.STL new file mode 100644 index 000000000..705d64f18 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_no_vision_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5b2ec7dafbf6a3c9a2cac717d18132a8a1efd9fcb8f2c40d9c12c8ece7323ec8 +size 1334384 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_no_vision_link.mtl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_no_vision_link.mtl new file mode 100644 index 000000000..66af0bf77 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_no_vision_link.mtl @@ -0,0 +1,12 @@ +# Blender 4.1.1 MTL File: 'None' +# www.blender.org + +newmtl White +Ns 250.000000 +Ka 1.000000 1.000000 1.000000 +Kd 1.000000 1.000000 1.000000 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.000000 +d 1.000000 +illum 2 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_no_vision_link.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_no_vision_link.obj new file mode 100644 index 000000000..a17b9d033 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_no_vision_link.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:190b2a97e289f208cd1e9cc053d1fdd2d7bd147909e026784d3a854d473b0c20 +size 1362966 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link.STL b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link.STL new file mode 100644 index 000000000..72f81c50b --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8bb05e420a88532b4eee3948a0047fb8e7f341c3e5cc971d54e97e2aaffcf878 +size 1164084 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link.mtl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link.mtl new file mode 100644 index 000000000..fdb35325d --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link.mtl @@ -0,0 +1,12 @@ +# Blender 4.1.1 MTL File: 'None' +# www.blender.org + +newmtl white +Ns 250.000000 +Ka 1.000000 1.000000 1.000000 +Kd 1.000000 1.000000 1.000000 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.000000 +d 1.000000 +illum 2 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link.obj new file mode 100644 index 000000000..66914c558 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:479465e12fa09738aa1853d6033839e61bb2f8976088f2b180842feca3fe39d7 +size 1560243 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/end_effector_link.STL b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/end_effector_link.STL new file mode 100644 index 000000000..3e2754219 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/end_effector_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5b6fb58e61fa475939767d68a446f97f1bff02c0e5935a3ea8bb51e6515783d8 +size 80 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/forearm_link.STL b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/forearm_link.STL new file mode 100644 index 000000000..0c616b5f7 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/forearm_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a00d96d2c7ec37e9fe4c5fb6d3fa2b4386dfd48fea5ac25632f4cb83b3505105 +size 456284 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/forearm_link.mtl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/forearm_link.mtl new file mode 100644 index 000000000..b435e8789 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/forearm_link.mtl @@ -0,0 +1,12 @@ +# Blender 4.1.1 MTL File: 'None' +# www.blender.org + +newmtl White.001 +Ns 250.000000 +Ka 1.000000 1.000000 1.000000 +Kd 1.000000 1.000000 1.000000 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.000000 +d 1.000000 +illum 2 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/forearm_link.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/forearm_link.obj new file mode 100644 index 000000000..92be4a04e --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/forearm_link.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:72520fe6116385b46ad20320e03f7b1ca8741ecab6b1cec196c554fdb903b58f +size 507752 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_1_link.STL b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_1_link.STL new file mode 100644 index 000000000..00b636d56 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8df461a71780d8e2d09104a507584424279d85173adf14867e359f7e58b3bba7 +size 514184 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_1_link.mtl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_1_link.mtl new file mode 100644 index 000000000..ac03f8c26 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_1_link.mtl @@ -0,0 +1,12 @@ +# Blender 4.1.1 MTL File: 'None' +# www.blender.org + +newmtl White.002 +Ns 250.000000 +Ka 1.000000 1.000000 1.000000 +Kd 1.000000 1.000000 1.000000 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.000000 +d 1.000000 +illum 2 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_1_link.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_1_link.obj new file mode 100644 index 000000000..e6d13e0d9 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_1_link.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d5c370bf5b89c7aad4387a52c1feead7e70e0d00b515e2d90aa096bad930a7c0 +size 562125 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_2_link.STL b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_2_link.STL new file mode 100644 index 000000000..375f68608 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_2_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8a9f38de21648804b3ab7076ec73726031f2ade7c95eb2541d070553e025407b +size 489184 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_2_link.mtl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_2_link.mtl new file mode 100644 index 000000000..07c787567 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_2_link.mtl @@ -0,0 +1,12 @@ +# Blender 4.1.1 MTL File: 'None' +# www.blender.org + +newmtl White.003 +Ns 250.000000 +Ka 1.000000 1.000000 1.000000 +Kd 1.000000 1.000000 1.000000 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.000000 +d 1.000000 +illum 2 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_2_link.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_2_link.obj new file mode 100644 index 000000000..b8847e002 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/half_arm_2_link.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bff684ba381830badc7e51e402394e16648d05a16639fa9934dac566b33b6663 +size 485542 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/shoulder_link.STL b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/shoulder_link.STL new file mode 100644 index 000000000..71dbb2133 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/shoulder_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a98e232e5197f09e9cd520e7a450c21bcec74bc49e2a19ce7cf86c5dc25f2e18 +size 476984 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/shoulder_link.mtl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/shoulder_link.mtl new file mode 100644 index 000000000..37d1f301d --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/shoulder_link.mtl @@ -0,0 +1,12 @@ +# Blender 4.1.1 MTL File: 'None' +# www.blender.org + +newmtl White.004 +Ns 250.000000 +Ka 1.000000 1.000000 1.000000 +Kd 1.000000 1.000000 1.000000 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.000000 +d 1.000000 +illum 2 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/shoulder_link.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/shoulder_link.obj new file mode 100644 index 000000000..beb8cdb93 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/shoulder_link.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4d304cba1afc3fc7266159a5e34c26385d30a1b64764d3f04100d1192698bf4f +size 424456 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_1_link.STL b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_1_link.STL new file mode 100644 index 000000000..cc0413db9 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_1_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0b97a5e3b2de535344f8e8d4176d77f0cdb3455b235f35430fa0413208856e66 +size 546484 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_1_link.mtl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_1_link.mtl new file mode 100644 index 000000000..8b4a00c2c --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_1_link.mtl @@ -0,0 +1,12 @@ +# Blender 4.1.1 MTL File: 'None' +# www.blender.org + +newmtl White.005 +Ns 250.000000 +Ka 1.000000 1.000000 1.000000 +Kd 1.000000 1.000000 1.000000 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.000000 +d 1.000000 +illum 2 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_1_link.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_1_link.obj new file mode 100644 index 000000000..c92ff7ba8 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_1_link.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:322c381b28eefca630d170cd434182434c063b6a36ebc1871152ccb490fa1864 +size 585957 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_2_link.STL b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_2_link.STL new file mode 100644 index 000000000..aa941bfe5 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_2_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:12271fb7d0ee78fb5642424cf5078be20f0476061e74203db964b674031ce879 +size 516984 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_2_link.mtl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_2_link.mtl new file mode 100644 index 000000000..d012aae9e --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_2_link.mtl @@ -0,0 +1,12 @@ +# Blender 4.1.1 MTL File: 'None' +# www.blender.org + +newmtl White.006 +Ns 250.000000 +Ka 1.000000 1.000000 1.000000 +Kd 1.000000 1.000000 1.000000 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.000000 +d 1.000000 +illum 2 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_2_link.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_2_link.obj new file mode 100644 index 000000000..a38660556 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/spherical_wrist_2_link.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:873e40ba691e3b15dcbd65ab9aad604c27b5747bbb674b1f38bfe3e0b2f648e1 +size 532462 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger.mtl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger.mtl new file mode 100644 index 000000000..65a2fec44 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger.mtl @@ -0,0 +1,12 @@ +# Blender 4.1.1 MTL File: 'part.blend' +# www.blender.org + +newmtl Material +Ns 250.000000 +Ka 1.000000 1.000000 1.000000 +Kd 0.034536 0.034536 0.034536 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.450000 +d 1.000000 +illum 2 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger.obj new file mode 100644 index 000000000..a1f64d567 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e4f6548123631dacb30a0a54516c522cf9aa6c556d9fbc2d42bc4938742c2032 +size 8371 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_1.mtl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_1.mtl new file mode 100644 index 000000000..0fcf20253 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_1.mtl @@ -0,0 +1,12 @@ +# Blender 4.1.1 MTL File: 'rafti_finger.blend' +# www.blender.org + +newmtl Material.001 +Ns 250.000000 +Ka 1.000000 1.000000 1.000000 +Kd 0.034536 0.034536 0.034536 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.450000 +d 1.000000 +illum 2 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_1.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_1.obj new file mode 100644 index 000000000..fdf6a470c --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7f1163f54bd7763f89c5a5ac2a2db71e6e7303d2676d60c3c34162234568e093 +size 682 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_2.mtl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_2.mtl new file mode 100644 index 000000000..0fcf20253 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_2.mtl @@ -0,0 +1,12 @@ +# Blender 4.1.1 MTL File: 'rafti_finger.blend' +# www.blender.org + +newmtl Material.001 +Ns 250.000000 +Ka 1.000000 1.000000 1.000000 +Kd 0.034536 0.034536 0.034536 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.450000 +d 1.000000 +illum 2 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_2.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_2.obj new file mode 100644 index 000000000..108f0754e --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_2.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:086bc1bda883756dea3c9a5306da2016547a812adb22e70811de0956eb043a98 +size 1465 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_3.mtl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_3.mtl new file mode 100644 index 000000000..0fcf20253 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_3.mtl @@ -0,0 +1,12 @@ +# Blender 4.1.1 MTL File: 'rafti_finger.blend' +# www.blender.org + +newmtl Material.001 +Ns 250.000000 +Ka 1.000000 1.000000 1.000000 +Kd 0.034536 0.034536 0.034536 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.450000 +d 1.000000 +illum 2 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_3.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_3.obj new file mode 100644 index 000000000..6110aa1fd --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_3.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4236029a257ad3f15ca5b32caa8cf8b40920dee83a1ae8ac9dadb4e8b006a40a +size 771 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_4.mtl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_4.mtl new file mode 100644 index 000000000..0fcf20253 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_4.mtl @@ -0,0 +1,12 @@ +# Blender 4.1.1 MTL File: 'rafti_finger.blend' +# www.blender.org + +newmtl Material.001 +Ns 250.000000 +Ka 1.000000 1.000000 1.000000 +Kd 0.034536 0.034536 0.034536 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.450000 +d 1.000000 +illum 2 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_4.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_4.obj new file mode 100644 index 000000000..a7dccfb5b --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/rafti_finger_collision_4.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5708a8d40a1848936f725031f090109eca1632b61e01d04d683fd33ce96ffd95 +size 3890 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/base.stl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/base.stl new file mode 100644 index 000000000..22a9c2b3b --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/base.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1c7b9f2bd92d705fc4e897c94e905973a3c05f406845e942229433deb7041453 +size 1712484 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/finger_link.stl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/finger_link.stl new file mode 100644 index 000000000..cbfd80ed7 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/finger_link.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d5ee95e62f8415bf5b6e503c831a958f5fc1990bf9b2865329ec38a28932727c +size 89084 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/finger_tip_link.stl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/finger_tip_link.stl new file mode 100644 index 000000000..9614b95e1 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/finger_tip_link.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:633793332c081641ce200df27a40643cc293b29956e3cb5cb29cb811c33ef1c7 +size 110484 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/inner_knuckle_link.stl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/inner_knuckle_link.stl new file mode 100644 index 000000000..01ec1e2a5 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/inner_knuckle_link.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4fb74a8a0d76c0e471cf19fd48bc676fd5b19123798e7a39eb6aa56869354283 +size 84884 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/knuckle_link.stl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/knuckle_link.stl new file mode 100644 index 000000000..0b3f90a5c --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/knuckle_link.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bbd6e868b1778bead60d2516c7ede581d7ad4e431744b1828757d6ea5129c112 +size 67084 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/pad.stl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/pad.stl new file mode 100644 index 000000000..231413751 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/pad.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4af1fa8d9bb285abecbf5dfc88e74cb30f059a7a074c2030dc35e9ee4316019d +size 15084 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/silicone_pad.stl b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/silicone_pad.stl new file mode 100644 index 000000000..6915244ae --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/robotiq_2f85/silicone_pad.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ca20c0fa61e6d3ce7b04bed25360e048e31dfd88ace04187aa6d9e8f4ca3fd0f +size 15084 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/cube_stack_scene.xml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/cube_stack_scene.xml new file mode 100644 index 000000000..90d694a5a --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/cube_stack_scene.xml @@ -0,0 +1,140 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/gen3_7dof.xml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/gen3_7dof.xml new file mode 100644 index 000000000..fbdfd6cc2 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/gen3_7dof.xml @@ -0,0 +1,731 @@ + + + diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/picknik_kinova_gen3.xacro b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/picknik_kinova_gen3.xacro new file mode 100644 index 000000000..7975fc62b --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/picknik_kinova_gen3.xacro @@ -0,0 +1,696 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 100000.0 + 100000.0 + + + + + 1e+5 + 1 + 0 + 0.2 + 0.002 + 0 + + + + + + + + + + + + + + + + + + + + + + + + 100000.0 + 100000.0 + + + + + 1e+5 + 1 + 0 + 0.2 + 0.002 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ${-2*pi} + ${2*pi} + + + 0.0 + + + + + + + -2.41 + 2.41 + + + 0.0 + + + + + + + ${-2*pi} + ${2*pi} + + + -3.14 + + + + + + + -2.66 + 2.66 + + + -2.51 + + + + + + + ${-2*pi} + ${2*pi} + + + 0.0 + + + + + + + -2.23 + 2.23 + + + 0.96 + + + + + + + ${-2*pi} + ${2*pi} + + + 1.57 + + + + + + + + 0.7929 + + + + + + + + + + + + + + + + + + + + + + + + + picknik_mujoco_ros/MujocoSystem + $(arg mujoco_model) + kinova_vla_test_sim + 10 + 60 + $(arg mujoco_viewer) + + + + diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/execute_color_stack_policy.xml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/execute_color_stack_policy.xml new file mode 100644 index 000000000..e691ee396 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/execute_color_stack_policy.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/package.xml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/package.xml new file mode 100644 index 000000000..17257f562 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/package.xml @@ -0,0 +1,30 @@ + + + kinova_vla_test_sim + 6.5.0 + + + MuJoCo simulation of a Kinova Gen3 (7-DoF) + Robotiq 2F-85 stacking + colored cubes, for testing VLA policy execution. + + + Dan Wahl + + BSD-3-Clause + + ament_cmake + + kinova_sim + kinova_gen3_base_config + joint_trajectory_admittance_controller + kortex_description + moveit_studio_kinova_pstop_manager + picknik_mujoco_ros + picknik_accessories + realsense2_description + velocity_force_controller + + + ament_cmake + + From 25d94232768a743e20be0edd316541970f0736c2 Mon Sep 17 00:00:00 2001 From: Dan Wahl Date: Wed, 22 Jul 2026 05:09:50 -0500 Subject: [PATCH 02/21] feat(kinova_vla_test_sim): serve VLA policies over docker with auto-launched bridge - docker/: Dockerfile.serve_policy + docker-compose.yaml + serve_policy.py, an HTTP inference server for lerobot pi0/pi0.5 checkpoints with real-time chunking support, plus unit tests (test_serve_policy.py). External port is driven by SERVE_POLICY_PORT; internal container port stays fixed at 8973. - script/get_action_chunk_adapter.py: ROS bridge between ExecutePolicy's /get_action_chunk service and the docker server, with unit tests under test/ wired into colcon test via CMakeLists.txt/package.xml. - launch/simulated_extras.launch.py: auto-launches the adapter alongside the sim Agent via config.yaml's simulated_hardware_launch_file, so it no longer needs to be run manually. - config/moveit/joint_limits.yaml: correct max_acceleration to match the vendor MoveIt config for this arm+gripper combo (kortex_moveit_config), rather than an uncited value. - objectives/execute_color_stack_policy.xml: lower policy_tracking_weight 300->30. Verified via e2e run with a pi0.5 checkpoint that 300 made the chunk executor's seam-anchor spline overshoot the acceleration limit right after each chunk boundary; 30 runs the full objective without aborting. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01Xkm9aPVJ33L5QhRYs8M3u6 --- .gitignore | 2 + .../kinova_vla_test_sim/CMakeLists.txt | 19 ++ .../kinova_vla_test_sim/config/config.yaml | 8 + .../config/moveit/joint_limits.yaml | 46 +++ .../docker/Dockerfile.serve_policy | 9 + .../kinova_vla_test_sim/docker/README.md | 30 ++ .../docker/docker-compose.yaml | 20 ++ .../docker/serve_policy.py | 251 ++++++++++++++ .../docker/test_serve_policy.py | 135 ++++++++ .../launch/simulated_extras.launch.py | 19 ++ .../objectives/execute_color_stack_policy.xml | 2 +- .../kinova_vla_test_sim/package.xml | 4 + .../script/get_action_chunk_adapter.py | 166 +++++++++ .../test/test_get_action_chunk_adapter.py | 318 ++++++++++++++++++ 14 files changed, 1028 insertions(+), 1 deletion(-) create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/moveit/joint_limits.yaml create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/Dockerfile.serve_policy create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/README.md create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/docker-compose.yaml create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/serve_policy.py create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/test_serve_policy.py create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/launch/simulated_extras.launch.py create mode 100755 src/moveit_pro_kinova_configs/kinova_vla_test_sim/script/get_action_chunk_adapter.py create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/test/test_get_action_chunk_adapter.py diff --git a/.gitignore b/.gitignore index f6ee3643d..fef744491 100644 --- a/.gitignore +++ b/.gitignore @@ -4,3 +4,5 @@ log/ .vscode/ MUJOCO_LOG.TXT .ccache/ +__pycache__/ +.pytest_cache/ diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/CMakeLists.txt b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/CMakeLists.txt index 2fd804624..76fa3498b 100644 --- a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/CMakeLists.txt +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/CMakeLists.txt @@ -7,13 +7,32 @@ install( DIRECTORY config description + launch objectives DESTINATION share/${PROJECT_NAME} ) +install(PROGRAMS + script/get_action_chunk_adapter.py + DESTINATION lib/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + set(_pytest_tests + test/test_get_action_chunk_adapter.py) + + foreach(_test_path ${_pytest_tests}) + get_filename_component(_test_name ${_test_path} NAME_WE) + ament_add_pytest_test(${_test_name} ${_test_path} + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT 60 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + endforeach() + ament_lint_auto_find_test_dependencies() endif() diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml index f542ae1e2..5d60060c0 100644 --- a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml @@ -10,6 +10,9 @@ hardware: simulated: True launch_control_node: True launch_robot_state_publisher: True + simulated_hardware_launch_file: + package: "kinova_vla_test_sim" + path: "launch/simulated_extras.launch.py" robot_description: urdf: @@ -32,6 +35,11 @@ hardware: - mujoco_model: "description/mujoco/cube_stack_scene.xml" - mujoco_viewer: false +moveit_params: + joint_limits: + package: "kinova_vla_test_sim" + path: "config/moveit/joint_limits.yaml" + objectives: objective_library_paths: colorstack_objectives: diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/moveit/joint_limits.yaml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/moveit/joint_limits.yaml new file mode 100644 index 000000000..13199dd35 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/moveit/joint_limits.yaml @@ -0,0 +1,46 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +# +# Matches external_dependencies/ros2_kortex/kortex_moveit_config/ +# kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/joint_limits.yaml (the vendor's +# own MoveIt config for this arm+gripper combo), rather than kinova_sim's conservative +# sim defaults, so ExecutePolicy's velocity/acceleration gate doesn't reject +# policy-commanded trajectories that are within hardware limits but above the sim +# defaults. +joint_limits: + joint_1: + has_velocity_limits: true + max_velocity: 1.3963 + has_acceleration_limits: true + max_acceleration: 8.6 + joint_2: + has_velocity_limits: true + max_velocity: 1.3963 + has_acceleration_limits: true + max_acceleration: 8.6 + joint_3: + has_velocity_limits: true + max_velocity: 1.3963 + has_acceleration_limits: true + max_acceleration: 8.6 + joint_4: + has_velocity_limits: true + max_velocity: 1.3963 + has_acceleration_limits: true + max_acceleration: 8.6 + joint_5: + has_velocity_limits: true + max_velocity: 1.2218 + has_acceleration_limits: true + max_acceleration: 8.6 + joint_6: + has_velocity_limits: true + max_velocity: 1.2218 + has_acceleration_limits: true + max_acceleration: 8.6 + joint_7: + has_velocity_limits: true + max_velocity: 1.2218 + has_acceleration_limits: true + max_acceleration: 8.6 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/Dockerfile.serve_policy b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/Dockerfile.serve_policy new file mode 100644 index 000000000..89f8a0d4b --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/Dockerfile.serve_policy @@ -0,0 +1,9 @@ +FROM python:3.12-slim + +RUN pip install --no-cache-dir "lerobot[pi,smolvla]==0.6.0" + +COPY serve_policy.py /app/serve_policy.py +WORKDIR /app + +EXPOSE 8973 +ENTRYPOINT ["python", "serve_policy.py"] diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/README.md b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/README.md new file mode 100644 index 000000000..e478b87e1 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/README.md @@ -0,0 +1,30 @@ +# Policy serving + +Runs a trained policy against the `Execute Color-Stack Policy` objective via MoveIt Pro's `ExecutePolicy` behavior and the `/get_action_chunk` service. + +## 1. Serve (model container) + +```bash +docker compose run --rm serve_policy \ + --checkpoint --policy-class pi05 --fps 10 +``` + +If `SERVE_POLICY_PORT` is set, also point the bridge's `INFER_URL` at +the same port (it defaults to `http://172.17.0.1:8973/infer`). + +## 2. Run (MoveIt Pro UI) + +The adapter starts automatically with Pro. To run manually without the whole stack: + +```bash +ros2 run kinova_vla_test_sim get_action_chunk_adapter.py +``` + +## Tests + +`../test/test_get_action_chunk_adapter.py` runs under `colcon test`. `test_serve_policy.py` needs `lerobot`/`torch`, so run it in model container: + +```bash +env -u LD_LIBRARY_PATH -m pip install pytest +env -u LD_LIBRARY_PATH -m pytest test_serve_policy.py +``` diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/docker-compose.yaml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/docker-compose.yaml new file mode 100644 index 000000000..7f80b0296 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/docker-compose.yaml @@ -0,0 +1,20 @@ +services: + serve_policy: + build: + context: . + dockerfile: Dockerfile.serve_policy + runtime: nvidia + deploy: + resources: + reservations: + devices: + - driver: nvidia + count: 1 + capabilities: [gpu, compute, utility] + ports: + - "${SERVE_POLICY_PORT:-8973}:8973" + environment: + - HF_HUB_OFFLINE=1 + volumes: + - /data/checkpoints:/data/checkpoints:ro + - /data/cache/huggingface:/root/.cache/huggingface:ro diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/serve_policy.py b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/serve_policy.py new file mode 100644 index 000000000..c3c7eb931 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/serve_policy.py @@ -0,0 +1,251 @@ +#!/usr/bin/env python3 +"""lerobot RTC inference server for MoveIt Pro ExecutePolicy (host side). + +Serves POST /infer and GET /health over HTTP. Generalized over any lerobot RTC +policy via --policy-class (pi05 | smolvla | ...); feeds dataset-native camera +keys and lets each checkpoint's baked rename_map map them onto the model's +slots, so IMG_KEY_MAP is policy-agnostic. + +Runs on the host; the in-container bridge node (get_action_chunk_adapter.py) +reaches it at http://172.17.0.1:/infer. + + env -u LD_LIBRARY_PATH serve_policy.py \ + --checkpoint --policy-class pi05 --fps 20 --port 8973 +""" +import argparse +import base64 +import json +import threading +import traceback +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer + +import cv2 +import numpy as np +import torch + +from lerobot.configs.types import RTCAttentionSchedule +from lerobot.policies.factory import get_policy_class, make_pre_post_processors +from lerobot.policies.rtc.configuration_rtc import RTCConfig + +# pi0.5 checkpoints save a processor pipeline that references +# 'relative_actions_processor', an alias lerobot does not always auto-register. +# Register it before loading, else make_pre_post_processors raises +# "Processor step 'relative_actions_processor' not found". +from lerobot.processor import ProcessorStepRegistry +from lerobot.processor.relative_action_processor import RelativeActionsProcessorStep + +try: + ProcessorStepRegistry.get("relative_actions_processor") +except Exception: + ProcessorStepRegistry.register("relative_actions_processor")( + RelativeActionsProcessorStep + ) + +# Request image key -> the checkpoint's dataset-native camera key. The training +# rename_map is baked into the saved processor, so make_pre_post_processors +# renames these to the model's slots itself; feed native keys and let it rename. +IMG_KEY_MAP = {"front": "scene", "wrist": "wrist", "overview": "overview"} + + +def decode_image(b64: str) -> torch.Tensor: + """base64 JPEG -> CHW float32 [0,1] RGB tensor.""" + buf = np.frombuffer(base64.b64decode(b64), dtype=np.uint8) + bgr = cv2.imdecode(buf, cv2.IMREAD_COLOR) # HxWx3 BGR uint8 + if bgr is None: + raise ValueError("cv2.imdecode failed on /infer image") + rgb = np.ascontiguousarray(bgr[:, :, ::-1]) + return torch.from_numpy(rgb).float().permute(2, 0, 1) / 255.0 + + +class PolicyServer: + def __init__( + self, + checkpoint: str, + device: str, + fps: float, + execution_horizon: int, + policy_class: str = "pi05", + state_dim: int = 0, + ): + self.device = device + self.fps = fps + self.state_dim = state_dim # >0 overrides config shape for warmup + self.lock = threading.Lock() # serialize: one model, stateful RTC processor + + self.policy = get_policy_class(policy_class).from_pretrained(checkpoint) + self.policy.to(device) + self.policy.eval() + + # RTC guidance configured once, server-side; applies when the Objective + # leaves guidance_horizon=0. A non-zero guidance_horizon value arrives per + # call as execution_horizon. + self.policy.config.rtc_config = RTCConfig( + enabled=True, + execution_horizon=execution_horizon, + prefix_attention_schedule=RTCAttentionSchedule.EXP, + ) + self.policy.init_rtc_processor() + + self.pre, self.post = make_pre_post_processors( + policy_cfg=self.policy.config, + pretrained_path=checkpoint, + preprocessor_overrides={"device_processor": {"device": self.device}}, + postprocessor_overrides={"device_processor": {"device": self.device}}, + ) + + @torch.no_grad() + def infer(self, payload: dict) -> dict: + obs = { + f"observation.images.{IMG_KEY_MAP[k]}": decode_image( + payload["images"][k] + ).to(self.device) + for k in IMG_KEY_MAP + } + obs["observation.state"] = torch.tensor( + payload["state"], dtype=torch.float32, device=self.device + ) + obs["task"] = payload["task"] + + kwargs = {} + prev = payload.get("prev_chunk_left_over") + if prev: + kwargs["prev_chunk_left_over"] = torch.tensor( + prev, dtype=torch.float32, device=self.device + ) + kwargs["inference_delay"] = int(payload.get("inference_delay", 0)) + if payload.get("execution_horizon") is not None: + kwargs["execution_horizon"] = int(payload["execution_horizon"]) + + self.policy.reset() + chunk = self.policy.predict_action_chunk( + self.pre(obs), **kwargs + ) # (1, T, A) normalized + raw = chunk.squeeze(0).detach().cpu().numpy() + steps = [self.post(chunk[:, i, :]) for i in range(chunk.shape[1])] + actions = ( + torch.stack(steps, dim=1).squeeze(0).detach().cpu().numpy() + ) # (T, A) absolute + return { + "action_chunk": actions.tolist(), + "action_chunk_raw": raw.tolist(), + "dt": 1.0 / self.fps, + } + + def warmup(self): + """Full-size warmup so the first real chunk does not pay for CUDA graph / cache setup.""" + h = w = 224 + blank = base64.b64encode( + cv2.imencode(".jpg", np.zeros((h, w, 3), np.uint8))[1] + ).decode("ascii") + state_dim = self.state_dim or int( + self.policy.config.input_features["observation.state"].shape[0] + ) + self.infer( + { + "images": {k: blank for k in IMG_KEY_MAP}, + "state": [0.0] * state_dim, + "task": "warmup", + } + ) + + +def make_handler(server: PolicyServer): + class Handler(BaseHTTPRequestHandler): + def _send(self, code, obj): + body = json.dumps(obj).encode() + self.send_response(code) + self.send_header("Content-Type", "application/json") + self.send_header("Content-Length", str(len(body))) + self.end_headers() + self.wfile.write(body) + + def do_GET(self): + if self.path == "/health": + self._send(200, {"status": "ok", "warm": True}) + else: + self._send(404, {"error": "not found"}) + + def do_POST(self): + if self.path != "/infer": + self._send(404, {"error": "not found"}) + return + try: + n = int(self.headers.get("Content-Length", 0)) + payload = json.loads(self.rfile.read(n)) + except (ValueError, json.JSONDecodeError) as exc: + self._send(400, {"error": f"bad request: {exc}"}) + return + try: + with server.lock: + result = server.infer(payload) + self._send(200, result) + except ( + Exception + ) as exc: # surfaced to the bridge as {"error": ...} -> success=false + traceback.print_exc() + self._send(200, {"error": f"{type(exc).__name__}: {exc}"}) + + def log_message(self, *args): + pass # quiet; call counting is done adapter-side + + return Handler + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--checkpoint", required=True) + ap.add_argument( + "--policy-class", + default="pi05", + help="lerobot policy class: pi05 | smolvla | ...", + ) + ap.add_argument( + "--state-dim", + type=int, + default=0, + help="override observation.state dim for warmup (0=use config)", + ) + ap.add_argument( + "--fps", type=float, default=20.0, help="training fps; response dt=1/fps" + ) + ap.add_argument("--port", type=int, default=8973) + ap.add_argument("--device", default="cuda") + ap.add_argument( + "--execution-horizon", type=int, default=12, help="server RTC default window" + ) + args = ap.parse_args() + + print( + f"[rtc-server] loading {args.policy_class} {args.checkpoint} on {args.device} ...", + flush=True, + ) + srv = PolicyServer( + args.checkpoint, + args.device, + args.fps, + args.execution_horizon, + policy_class=args.policy_class, + state_dim=args.state_dim, + ) + print("[rtc-server] warmup inference ...", flush=True) + try: + srv.warmup() + except ( + Exception + ) as exc: # warmup only pre-pays CUDA setup; never let it kill the server. + traceback.print_exc() + print( + f"[rtc-server] WARNING: warmup skipped ({type(exc).__name__}: {exc}); " + f"pass --state-dim for a merged checkpoint to warm up cleanly", + flush=True, + ) + httpd = ThreadingHTTPServer(("0.0.0.0", args.port), make_handler(srv)) + print( + f"[rtc-server] ready on 0.0.0.0:{args.port} (dt={1.0/args.fps:.4f}, ehz={args.execution_horizon})", + flush=True, + ) + httpd.serve_forever() + + +if __name__ == "__main__": + main() diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/test_serve_policy.py b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/test_serve_policy.py new file mode 100644 index 000000000..60dd9aa42 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/test_serve_policy.py @@ -0,0 +1,135 @@ +#!/usr/bin/env python3 +"""Tests for serve_policy.py: image decoding and the /infer, /health HTTP handlers. + +Runs in the same Python environment as serve_policy.py itself (lerobot/torch/cv2), +not the ROS workspace's pytest suite — see docker/README.md for how to run this. +""" + +import base64 +import http.client +import json +import threading +import unittest + +import cv2 +import numpy as np +import torch +from http.server import ThreadingHTTPServer + +from serve_policy import decode_image, make_handler + + +def encode_bgr_jpeg_b64(bgr: np.ndarray) -> str: + ok, buf = cv2.imencode(".jpg", bgr) + assert ok + return base64.b64encode(buf.tobytes()).decode("ascii") + + +class TestDecodeImage(unittest.TestCase): + """decode_image: base64 JPEG -> CHW float32 [0,1] RGB tensor.""" + + def test_invalid_base64_raises(self) -> None: + """Malformed image bytes fail loudly (ValueError) instead of returning garbage.""" + with self.assertRaises(ValueError): + decode_image(base64.b64encode(b"not a jpeg").decode("ascii")) + + def test_output_shape_and_dtype(self) -> None: + """A 4x2 BGR frame decodes to a (3, 4, 2) float32 tensor scaled to [0, 1].""" + bgr = np.zeros((4, 2, 3), dtype=np.uint8) + tensor = decode_image(encode_bgr_jpeg_b64(bgr)) + + self.assertEqual(tuple(tensor.shape), (3, 4, 2)) + self.assertEqual(tensor.dtype, torch.float32) + self.assertGreaterEqual(float(tensor.min()), 0.0) + self.assertLessEqual(float(tensor.max()), 1.0) + + def test_bgr_to_rgb_channel_order(self) -> None: + """A pure-blue BGR frame decodes with the red channel near zero (BGR -> RGB swap).""" + bgr = np.zeros((8, 8, 3), dtype=np.uint8) + bgr[:, :, 0] = 255 # BGR channel 0 = blue + tensor = decode_image(encode_bgr_jpeg_b64(bgr)) + + # channel 0 = red after the BGR->RGB swap, so it should stay dark despite the + # source being fully saturated on the blue channel; channel 2 = blue, saturated. + self.assertLess(float(tensor[0].mean()), 0.2) + self.assertGreater(float(tensor[2].mean()), 0.8) + + +class TestHttpHandler(unittest.TestCase): + """The /health and /infer HTTP endpoints, isolated from real policy inference.""" + + class FakeServer: + """Stands in for PolicyServer: same .lock/.infer(payload) contract, no model.""" + + def __init__(self, infer_result=None, infer_error=None) -> None: + self.lock = threading.Lock() + self._infer_result = infer_result + self._infer_error = infer_error + + def infer(self, payload: dict) -> dict: + if self._infer_error is not None: + raise self._infer_error + return self._infer_result + + def _start( + self, fake_server: "TestHttpHandler.FakeServer" + ) -> http.client.HTTPConnection: + httpd = ThreadingHTTPServer(("127.0.0.1", 0), make_handler(fake_server)) + threading.Thread(target=httpd.serve_forever, daemon=True).start() + self.addCleanup(httpd.shutdown) + return http.client.HTTPConnection("127.0.0.1", httpd.server_address[1]) + + def test_health_returns_ok(self) -> None: + """GET /health reports ok without touching the (possibly still-loading) server.""" + conn = self._start(self.FakeServer()) + conn.request("GET", "/health") + resp = conn.getresponse() + + self.assertEqual(resp.status, 200) + self.assertEqual(json.loads(resp.read())["status"], "ok") + + def test_unknown_path_is_404(self) -> None: + """A request to any path other than /health or /infer is rejected, not routed.""" + conn = self._start(self.FakeServer()) + conn.request("GET", "/unknown") + resp = conn.getresponse() + + self.assertEqual(resp.status, 404) + + def test_infer_success_returns_result(self) -> None: + """A valid POST /infer body is forwarded to server.infer() and its result returned.""" + conn = self._start( + self.FakeServer(infer_result={"action_chunk": [[0.1]], "dt": 0.05}) + ) + body = json.dumps({"task": "warmup"}).encode() + conn.request("POST", "/infer", body=body) + resp = conn.getresponse() + + self.assertEqual(resp.status, 200) + self.assertEqual(json.loads(resp.read())["dt"], 0.05) + + def test_infer_malformed_json_is_400(self) -> None: + """A body that isn't valid JSON is rejected before it ever reaches server.infer().""" + conn = self._start(self.FakeServer()) + conn.request("POST", "/infer", body=b"not json") + resp = conn.getresponse() + + self.assertEqual(resp.status, 400) + + def test_infer_exception_returns_200_with_error_field(self) -> None: + """server.infer() raising is caught and reported as {"error": ...}, not a 500. + + The bridge (get_action_chunk_adapter.py) treats any 2xx body containing an + "error" key as a failed chunk; a raw 500 would instead surface as an unhandled + requests.HTTPError from raise_for_status(). + """ + conn = self._start(self.FakeServer(infer_error=RuntimeError("model not warm"))) + conn.request("POST", "/infer", body=json.dumps({"task": "x"}).encode()) + resp = conn.getresponse() + + self.assertEqual(resp.status, 200) + self.assertIn("model not warm", json.loads(resp.read())["error"]) + + +if __name__ == "__main__": + unittest.main() diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/launch/simulated_extras.launch.py b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/launch/simulated_extras.launch.py new file mode 100644 index 000000000..36fd5be05 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/launch/simulated_extras.launch.py @@ -0,0 +1,19 @@ +#!/usr/bin/env python3 + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription( + [ + # /get_action_chunk bridge to the host inference server (docker/serve_policy.py); + # ExecutePolicy calls this service, so it must be up wherever the Agent runs. + Node( + package="kinova_vla_test_sim", + executable="get_action_chunk_adapter.py", + name="get_action_chunk_adapter", + output="log", + ), + ] + ) diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/execute_color_stack_policy.xml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/execute_color_stack_policy.xml index e691ee396..0c0629ed5 100644 --- a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/execute_color_stack_policy.xml +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/execute_color_stack_policy.xml @@ -26,7 +26,7 @@ committed_action_steps="12" num_interpolation_points="200" goal_path_tolerance="99.0" - policy_tracking_weight="300.0" + policy_tracking_weight="30.0" policy_uses_real_time_chunking="true" policy_call_timeout="10.0" image_topics="/scene_camera/color;/wrist_camera/color;/overview_camera/color" diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/package.xml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/package.xml index 17257f562..f02c892b6 100644 --- a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/package.xml +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/package.xml @@ -18,12 +18,16 @@ kinova_gen3_base_config joint_trajectory_admittance_controller kortex_description + moveit_pro_ml_msgs moveit_studio_kinova_pstop_manager picknik_mujoco_ros picknik_accessories realsense2_description velocity_force_controller + ament_lint_auto + ament_cmake_pytest + ament_cmake diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/script/get_action_chunk_adapter.py b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/script/get_action_chunk_adapter.py new file mode 100755 index 000000000..1a835db59 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/script/get_action_chunk_adapter.py @@ -0,0 +1,166 @@ +#!/usr/bin/env python3 +"""GetActionChunk adapter: serves ExecutePolicy from an inference process over HTTP. + +Policy-agnostic: runs inside the MoveIt Pro dev container, forwards observations +to the host inference server at INFER_URL and returns chunks. Run with the +overlay sourced so moveit_pro_ml_msgs resolves. +""" + +import base64 +import os + +import cv2 +import numpy as np +import requests + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64MultiArray, MultiArrayDimension +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +from moveit_pro_ml_msgs.srv import GetActionChunk + +INFER_URL = os.environ.get("INFER_URL", "http://172.17.0.1:8973/infer") + + +def encode_jpeg_b64(img) -> str: + """sensor_msgs/Image -> base64 JPEG. Accepts rgb8/bgr8/rgba8/bgra8 frames.""" + encoding = str(img.encoding).lower() + if encoding not in ("rgb8", "bgr8", "rgba8", "bgra8"): + raise ValueError(f"unsupported image encoding '{img.encoding}'") + channels = 4 if encoding in ("rgba8", "bgra8") else 3 + # step is the row stride in bytes; slice off any row padding before reshaping. + rows = np.frombuffer(bytes(img.data), dtype=np.uint8).reshape(img.height, img.step) + arr = rows[:, : img.width * channels].reshape(img.height, img.width, channels)[ + :, :, :3 + ] + # cv2.imencode expects BGR input, so bgr frames pass through and rgb frames flip once. + if encoding.startswith("rgb"): + arr = arr[:, :, ::-1] + ok, buf = cv2.imencode(".jpg", np.ascontiguousarray(arr)) + if not ok: + raise RuntimeError("cv2.imencode failed") + return base64.b64encode(buf.tobytes()).decode("ascii") + + +class GetActionChunkAdapter(Node): + def __init__(self) -> None: + super().__init__("get_action_chunk_adapter") + self.create_service(GetActionChunk, "/get_action_chunk", self._on_request) + self._calls = 0 + + def _on_request(self, request, response): + # The request carries the policy's full trained state: the arm group's + # joints plus the gripper joint appended last when the Objective configures one. + state = list(request.robot_state.position) + if len(request.images) != len(request.image_names): + response.success = False + response.message = ( + f"images ({len(request.images)}) and image_names " + f"({len(request.image_names)}) length mismatch" + ) + return response + try: + images = { + request.image_names[i]: encode_jpeg_b64(img) + for i, img in enumerate(request.images) + } + except (ValueError, RuntimeError) as exc: + response.success = False + response.message = f"image encode failed: {exc}" + return response + payload = { + "state": state, + "task": request.prompt, + "images": images, + "new_episode": bool(request.new_episode), + } + + # RTC carryover: forward the previous chunk's unexecuted tail and the overlap depth. + # A Float64MultiArray is a flat row-major buffer plus a layout, so reshape it back + # into (steps, action width) rows; it arrives empty on the first call and when RTC is off. + prev = request.previous_action_chunk + if prev.data and len(prev.layout.dim) == 2: + steps, width = prev.layout.dim[0].size, prev.layout.dim[1].size + payload["prev_chunk_left_over"] = ( + np.asarray(prev.data, dtype=float).reshape(steps, width).tolist() + ) + payload["inference_delay"] = int(request.frozen_prefix_steps) + # A non-zero guidance_horizon is the Objective overriding the server's RTC window. + if request.guidance_horizon > 0: + payload["execution_horizon"] = int(request.guidance_horizon) + + try: + resp = requests.post(INFER_URL, json=payload, timeout=30.0) + resp.raise_for_status() + data = resp.json() + except (requests.RequestException, ValueError) as exc: + response.success = False + response.message = f"/infer request failed: {exc}" + return response + if isinstance(data, dict) and data.get("error"): + response.success = False + response.message = f"/infer error: {data['error']}" + return response + chunk = data.get("action_chunk") if isinstance(data, dict) else None + if not chunk or "dt" not in data: + response.success = False + response.message = ( + "/infer response is missing a non-empty action_chunk or dt" + ) + return response + expected_dims = len(request.robot_state.name) + mismatched_width = next( + (len(step) for step in chunk if len(step) != expected_dims), None + ) + if mismatched_width is not None: + response.success = False + response.message = ( + f"/infer chunk width {mismatched_width} does not match the observed " + f"joint count {expected_dims}" + ) + return response + + # The chunk: absolute joint positions. The action columns line up with the request's + # state entries, so the request's joint names are also the chunk's joint names. + traj = JointTrajectory() + traj.joint_names = list(request.robot_state.name) + for step in chunk: + point = JointTrajectoryPoint() + point.positions = [float(v) for v in step] + traj.points.append(point) + response.success = True + response.chunk = traj + response.native_control_period = float(data["dt"]) + + # RTC echo: the normalized model output, one row per step, one column per action dimension. + raw = data.get("action_chunk_raw") + if raw: + arr = Float64MultiArray() + steps, width = len(raw), len(raw[0]) + arr.layout.data_offset = 0 + arr.layout.dim = [ + MultiArrayDimension(label="steps", size=steps, stride=steps * width), + MultiArrayDimension(label="dims", size=width, stride=width), + ] + arr.data = np.asarray(raw, dtype=float).ravel().tolist() + response.policy_action_chunk = arr + + self._calls += 1 + if self._calls % 10 == 0: + self.get_logger().info(f"get_action_chunk: served {self._calls} chunks") + return response + + +def main() -> None: + rclpy.init() + node = GetActionChunkAdapter() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/test/test_get_action_chunk_adapter.py b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/test/test_get_action_chunk_adapter.py new file mode 100644 index 000000000..db1eb908d --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/test/test_get_action_chunk_adapter.py @@ -0,0 +1,318 @@ +#!/usr/bin/env python3 +"""Tests for the GetActionChunk bridge: image encoding and request/response mapping.""" + +import importlib.util +import sys +import unittest +from pathlib import Path +from unittest.mock import MagicMock, patch + +import rclpy +import requests +from sensor_msgs.msg import Image, JointState +from std_msgs.msg import Float64MultiArray, MultiArrayDimension, MultiArrayLayout + +from moveit_pro_ml_msgs.srv import GetActionChunk + +# get_action_chunk_adapter.py is a standalone ROS executable (install(PROGRAMS ...) in +# CMakeLists.txt), not part of an importable Python package, so it's loaded by file path. +_SCRIPT_PATH = ( + Path(__file__).resolve().parent.parent / "script" / "get_action_chunk_adapter.py" +) +_spec = importlib.util.spec_from_file_location("get_action_chunk_adapter", _SCRIPT_PATH) +get_action_chunk_adapter = importlib.util.module_from_spec(_spec) +sys.modules["get_action_chunk_adapter"] = get_action_chunk_adapter +_spec.loader.exec_module(get_action_chunk_adapter) + +GetActionChunkAdapter = get_action_chunk_adapter.GetActionChunkAdapter +encode_jpeg_b64 = get_action_chunk_adapter.encode_jpeg_b64 + + +def make_image(encoding: str, height: int = 2, width: int = 2) -> Image: + """A flat-colored sensor_msgs/Image with no row padding (step == width * channels).""" + channels = 4 if encoding in ("rgba8", "bgra8") else 3 + img = Image() + img.encoding = encoding + img.height = height + img.width = width + img.step = width * channels + img.data = bytes([128] * (height * img.step)) + return img + + +def make_request(**overrides) -> GetActionChunk.Request: + request = GetActionChunk.Request() + request.robot_state = JointState(name=["j1", "j2"], position=[0.1, 0.2]) + request.images = [make_image("rgb8")] + request.image_names = ["front"] + request.prompt = "stack the blocks" + request.new_episode = False + for key, value in overrides.items(): + setattr(request, key, value) + return request + + +class TestEncodeJpegB64(unittest.TestCase): + """encode_jpeg_b64: sensor_msgs/Image -> base64 JPEG round trip.""" + + def test_unsupported_encoding_raises(self) -> None: + """A mono8 (or any non-{r,b}gb[a]8) image is rejected, not silently reinterpreted.""" + img = make_image("mono8") + with self.assertRaises(ValueError): + encode_jpeg_b64(img) + + def test_rgb8_and_bgr8_roundtrip_to_same_pixels(self) -> None: + """rgb8 and bgr8 inputs carrying the same visual color decode to matching JPEG bytes.""" + rgb = make_image("rgb8") + rgb.data = bytes([10, 20, 30] * (rgb.height * rgb.width)) + bgr = make_image("bgr8") + bgr.data = bytes([30, 20, 10] * (bgr.height * bgr.width)) + + rgb_b64 = encode_jpeg_b64(rgb) + bgr_b64 = encode_jpeg_b64(bgr) + + self.assertEqual(rgb_b64, bgr_b64) + + def test_rgba8_drops_alpha_channel(self) -> None: + """Encoding a 4-channel frame does not crash and yields a valid JPEG (3-channel).""" + img = make_image("rgba8") + img.data = bytes([10, 20, 30, 255] * (img.height * img.width)) + b64 = encode_jpeg_b64(img) + self.assertTrue(len(b64) > 0) + + def test_row_padding_is_stripped(self) -> None: + """step wider than width*channels (row padding) must not corrupt the decoded pixels.""" + img = make_image("rgb8") + pad = 4 + img.step = img.width * 3 + pad + img.data = bytes([10, 20, 30] * img.width + [0] * pad) * img.height + padded_b64 = encode_jpeg_b64(img) + + unpadded = make_image("rgb8") + unpadded.data = bytes([10, 20, 30] * (unpadded.height * unpadded.width)) + unpadded_b64 = encode_jpeg_b64(unpadded) + + self.assertEqual(padded_b64, unpadded_b64) + + +class TestOnRequest(unittest.TestCase): + """GetActionChunkAdapter._on_request: HTTP call shaping and response translation.""" + + @classmethod + def setUpClass(cls) -> None: + rclpy.init() + + @classmethod + def tearDownClass(cls) -> None: + rclpy.shutdown() + + def setUp(self) -> None: + self.node = GetActionChunkAdapter() + + def tearDown(self) -> None: + self.node.destroy_node() + + @patch("get_action_chunk_adapter.requests.post") + def test_successful_chunk_populates_trajectory(self, mock_post: MagicMock) -> None: + """A valid /infer response becomes a JointTrajectory with matching joint names.""" + mock_post.return_value.json.return_value = { + "action_chunk": [[0.5, 0.6], [0.7, 0.8]], + "dt": 0.05, + } + request = make_request() + response = self.node._on_request(request, GetActionChunk.Response()) + + self.assertTrue(response.success, response.message) + self.assertEqual(list(response.chunk.joint_names), ["j1", "j2"]) + self.assertEqual(len(response.chunk.points), 2) + self.assertEqual(list(response.chunk.points[0].positions), [0.5, 0.6]) + self.assertAlmostEqual(response.native_control_period, 0.05) + + @patch("get_action_chunk_adapter.requests.post") + def test_prompt_forwarded_as_task_field(self, mock_post: MagicMock) -> None: + """The fixed request.prompt field is sent to /infer under the 'task' key.""" + mock_post.return_value.json.return_value = { + "action_chunk": [[0.0, 0.0]], + "dt": 0.05, + } + request = make_request(prompt="pick the red cube") + self.node._on_request(request, GetActionChunk.Response()) + + sent_payload = mock_post.call_args.kwargs["json"] + self.assertEqual(sent_payload["task"], "pick the red cube") + + def test_unsupported_image_encoding_fails_without_http_call(self) -> None: + """An image the encoder can't handle fails locally; /infer is never called.""" + request = make_request(images=[make_image("mono8")]) + with patch("get_action_chunk_adapter.requests.post") as mock_post: + response = self.node._on_request(request, GetActionChunk.Response()) + mock_post.assert_not_called() + self.assertFalse(response.success) + self.assertIn("image encode failed", response.message) + + def test_images_and_image_names_length_mismatch_fails_without_http_call( + self, + ) -> None: + """A malformed request (arrays not lined up by index, per the .srv contract) + fails locally instead of raising IndexError out of the service callback.""" + request = make_request(images=[make_image("rgb8"), make_image("rgb8")]) + with patch("get_action_chunk_adapter.requests.post") as mock_post: + response = self.node._on_request(request, GetActionChunk.Response()) + mock_post.assert_not_called() + self.assertFalse(response.success) + self.assertIn("length mismatch", response.message) + + @patch("get_action_chunk_adapter.requests.post") + def test_http_error_surfaces_as_failure(self, mock_post: MagicMock) -> None: + """A network-level failure calling /infer is reported, not raised.""" + mock_post.side_effect = requests.ConnectionError("refused") + response = self.node._on_request(make_request(), GetActionChunk.Response()) + + self.assertFalse(response.success) + self.assertIn("/infer request failed", response.message) + + @patch("get_action_chunk_adapter.requests.post") + def test_server_error_field_surfaces_as_failure(self, mock_post: MagicMock) -> None: + """A server-reported {"error": ...} body fails the request rather than being ignored.""" + mock_post.return_value.json.return_value = {"error": "checkpoint not loaded"} + response = self.node._on_request(make_request(), GetActionChunk.Response()) + + self.assertFalse(response.success) + self.assertIn("checkpoint not loaded", response.message) + + @patch("get_action_chunk_adapter.requests.post") + def test_missing_dt_fails(self, mock_post: MagicMock) -> None: + """A response with a chunk but no dt is rejected rather than defaulting silently.""" + mock_post.return_value.json.return_value = {"action_chunk": [[0.0, 0.0]]} + response = self.node._on_request(make_request(), GetActionChunk.Response()) + + self.assertFalse(response.success) + self.assertIn("missing", response.message) + + @patch("get_action_chunk_adapter.requests.post") + def test_chunk_width_mismatch_fails(self, mock_post: MagicMock) -> None: + """A chunk whose column count doesn't match the requested joint count is rejected.""" + mock_post.return_value.json.return_value = { + "action_chunk": [[0.0, 0.0, 0.0]], # 3 columns, request has 2 joints + "dt": 0.05, + } + response = self.node._on_request(make_request(), GetActionChunk.Response()) + + self.assertFalse(response.success) + self.assertIn("does not match", response.message) + + @patch("get_action_chunk_adapter.requests.post") + def test_chunk_width_mismatch_reports_the_offending_row( + self, mock_post: MagicMock + ) -> None: + """The error names the row that actually mismatched, not always row 0.""" + mock_post.return_value.json.return_value = { + # row 0 matches the request's 2 joints; row 1 is the actual offender. + "action_chunk": [[0.0, 0.0], [0.0, 0.0, 0.0]], + "dt": 0.05, + } + response = self.node._on_request(make_request(), GetActionChunk.Response()) + + self.assertFalse(response.success) + self.assertIn("chunk width 3", response.message) + + @patch("get_action_chunk_adapter.requests.post") + def test_previous_chunk_reshaped_and_forwarded(self, mock_post: MagicMock) -> None: + """A populated previous_action_chunk is reshaped from its flat layout before sending.""" + mock_post.return_value.json.return_value = { + "action_chunk": [[0.0, 0.0]], + "dt": 0.05, + } + prev = Float64MultiArray( + layout=MultiArrayLayout( + dim=[ + MultiArrayDimension(label="steps", size=2, stride=4), + MultiArrayDimension(label="dims", size=2, stride=2), + ] + ), + data=[1.0, 2.0, 3.0, 4.0], + ) + request = make_request(previous_action_chunk=prev, frozen_prefix_steps=3) + self.node._on_request(request, GetActionChunk.Response()) + + sent_payload = mock_post.call_args.kwargs["json"] + self.assertEqual(sent_payload["prev_chunk_left_over"], [[1.0, 2.0], [3.0, 4.0]]) + self.assertEqual(sent_payload["inference_delay"], 3) + + @patch("get_action_chunk_adapter.requests.post") + def test_empty_previous_chunk_omits_rtc_fields(self, mock_post: MagicMock) -> None: + """The first call of an episode (empty previous_action_chunk) sends no RTC carryover.""" + mock_post.return_value.json.return_value = { + "action_chunk": [[0.0, 0.0]], + "dt": 0.05, + } + self.node._on_request(make_request(), GetActionChunk.Response()) + + sent_payload = mock_post.call_args.kwargs["json"] + self.assertNotIn("prev_chunk_left_over", sent_payload) + self.assertNotIn("inference_delay", sent_payload) + + @patch("get_action_chunk_adapter.requests.post") + def test_zero_guidance_horizon_omits_execution_horizon( + self, mock_post: MagicMock + ) -> None: + """guidance_horizon=0 defers to the server's own RTC default, per the .srv contract.""" + mock_post.return_value.json.return_value = { + "action_chunk": [[0.0, 0.0]], + "dt": 0.05, + } + self.node._on_request( + make_request(guidance_horizon=0), GetActionChunk.Response() + ) + + sent_payload = mock_post.call_args.kwargs["json"] + self.assertNotIn("execution_horizon", sent_payload) + + @patch("get_action_chunk_adapter.requests.post") + def test_nonzero_guidance_horizon_overrides_execution_horizon( + self, mock_post: MagicMock + ) -> None: + """A nonzero guidance_horizon from the Objective is forwarded as execution_horizon.""" + mock_post.return_value.json.return_value = { + "action_chunk": [[0.0, 0.0]], + "dt": 0.05, + } + self.node._on_request( + make_request(guidance_horizon=7), GetActionChunk.Response() + ) + + sent_payload = mock_post.call_args.kwargs["json"] + self.assertEqual(sent_payload["execution_horizon"], 7) + + @patch("get_action_chunk_adapter.requests.post") + def test_action_chunk_raw_echoed_as_policy_action_chunk( + self, mock_post: MagicMock + ) -> None: + """When the server echoes action_chunk_raw, it is reflected back as policy_action_chunk.""" + mock_post.return_value.json.return_value = { + "action_chunk": [[0.5, 0.6], [0.7, 0.8]], + "action_chunk_raw": [[-0.1, 0.1], [0.2, -0.2]], + "dt": 0.05, + } + response = self.node._on_request(make_request(), GetActionChunk.Response()) + + arr = response.policy_action_chunk + self.assertEqual([d.size for d in arr.layout.dim], [2, 2]) + self.assertEqual(list(arr.data), [-0.1, 0.1, 0.2, -0.2]) + + @patch("get_action_chunk_adapter.requests.post") + def test_no_action_chunk_raw_leaves_policy_action_chunk_empty( + self, mock_post: MagicMock + ) -> None: + """A policy without RTC support (no action_chunk_raw) leaves policy_action_chunk unset.""" + mock_post.return_value.json.return_value = { + "action_chunk": [[0.5, 0.6]], + "dt": 0.05, + } + response = self.node._on_request(make_request(), GetActionChunk.Response()) + + self.assertEqual(list(response.policy_action_chunk.data), []) + + +if __name__ == "__main__": + unittest.main() From cda92aa3a7502308aa811049763466821569f063 Mon Sep 17 00:00:00 2001 From: Dan Wahl Date: Wed, 22 Jul 2026 05:46:39 -0500 Subject: [PATCH 03/21] fix(kinova_vla_test_sim): home+open-gripper before rollout, standard RTC defaults, drop dead RealSense mesh - objectives/execute_color_stack_policy.xml: move to the Home waypoint and open the gripper before switching controllers and running ExecutePolicy, so a rollout always starts from a known state. - Default committed_action_steps/guidance_horizon to 20 (matching the standard RTC execution horizon) and serve_policy.py's --fps/--execution-horizon defaults to 10/20 to match; verified end to end that the resulting total_action_steps=250 (25s) budget is sufficient for a full stack. - description/picknik_kinova_gen3.xacro: drop the unconditional RealSense D415 mesh instantiation at external_camera_link ("scene_camera") and the unused wrist_realsense arg. Neither was wired to anything: the real /scene_camera and /wrist_camera image topics come from mesh-free native MuJoCo tags in the hand-authored MJCF scene files, not this URDF. The mesh only ever showed up as an unexplained floating object in RViz/planning-scene views. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01Xkm9aPVJ33L5QhRYs8M3u6 --- .../description/picknik_kinova_gen3.xacro | 2 -- .../kinova_vla_test_sim/docker/README.md | 2 +- .../kinova_vla_test_sim/docker/serve_policy.py | 6 +++--- .../objectives/execute_color_stack_policy.xml | 12 +++++++++--- 4 files changed, 13 insertions(+), 9 deletions(-) diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/picknik_kinova_gen3.xacro b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/picknik_kinova_gen3.xacro index 7975fc62b..5e13a718f 100644 --- a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/picknik_kinova_gen3.xacro +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/picknik_kinova_gen3.xacro @@ -19,13 +19,11 @@ - - diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/README.md b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/README.md index e478b87e1..b7fc5e891 100644 --- a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/README.md +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/README.md @@ -6,7 +6,7 @@ Runs a trained policy against the `Execute Color-Stack Policy` objective via Mov ```bash docker compose run --rm serve_policy \ - --checkpoint --policy-class pi05 --fps 10 + --checkpoint --policy-class pi05 ``` If `SERVE_POLICY_PORT` is set, also point the bridge's `INFER_URL` at diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/serve_policy.py b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/serve_policy.py index c3c7eb931..0bc41f0a0 100644 --- a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/serve_policy.py +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/serve_policy.py @@ -10,7 +10,7 @@ reaches it at http://172.17.0.1:/infer. env -u LD_LIBRARY_PATH serve_policy.py \ - --checkpoint --policy-class pi05 --fps 20 --port 8973 + --checkpoint --policy-class pi05 """ import argparse import base64 @@ -206,12 +206,12 @@ def main(): help="override observation.state dim for warmup (0=use config)", ) ap.add_argument( - "--fps", type=float, default=20.0, help="training fps; response dt=1/fps" + "--fps", type=float, default=10.0, help="training fps; response dt=1/fps" ) ap.add_argument("--port", type=int, default=8973) ap.add_argument("--device", default="cuda") ap.add_argument( - "--execution-horizon", type=int, default=12, help="server RTC default window" + "--execution-horizon", type=int, default=20, help="server RTC default window" ) args = ap.parse_args() diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/execute_color_stack_policy.xml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/execute_color_stack_policy.xml index 0c0629ed5..8518a7a2a 100644 --- a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/execute_color_stack_policy.xml +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/execute_color_stack_policy.xml @@ -6,6 +6,12 @@ _favorite="true" > + + From d6b99d6afbed557b27a6ccdd792bd059acdd2e4e Mon Sep 17 00:00:00 2001 From: Dan Wahl Date: Thu, 23 Jul 2026 20:01:31 -0500 Subject: [PATCH 04/21] fix(kinova_vla_test_sim): fork local SRDF to drop dangling camera/gripper warnings, fix serve_policy docs The xacro dropped the RealSense mesh/scene_camera link but the SRDF (shared verbatim from kinova_sim) still referenced it in disable_collisions entries, and the gripper's non-mimic finger joints were never marked passive or given group_state values. Fork a local SRDF so this config's collision matrix and group_state actually match its own URDF, and drop the now-unused external_camera/wrist_realsense urdf_params. Also document --service-ports on the serve_policy run command; docker compose run does not publish a service's ports by default, so omitting it left the bridge unable to reach the inference server. Co-Authored-By: Claude Sonnet 5 --- .../kinova_vla_test_sim/config/config.yaml | 6 +- .../config/moveit/picknik_kinova_gen3.srdf | 178 ++++++++++++++++++ .../description/picknik_kinova_gen3.xacro | 12 -- .../kinova_vla_test_sim/docker/README.md | 2 +- 4 files changed, 181 insertions(+), 17 deletions(-) create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/moveit/picknik_kinova_gen3.srdf diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml index 5d60060c0..1bcb35ccf 100644 --- a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml @@ -19,8 +19,8 @@ hardware: package: "kinova_vla_test_sim" path: "description/picknik_kinova_gen3.xacro" srdf: - package: "kinova_sim" - path: "config/moveit/picknik_kinova_gen3_base.srdf" + package: "kinova_vla_test_sim" + path: "config/moveit/picknik_kinova_gen3.srdf" urdf_params: - mujoco_keyframe: "default" - arm: "gen3" @@ -29,8 +29,6 @@ hardware: - gripper: "robotiq_2f_85" - use_fake_hardware: "%>> hardware.simulated" - fake_sensor_commands: "false" - - external_camera: "true" - - wrist_realsense: "true" - use_internal_bus_gripper_comm: "false" - mujoco_model: "description/mujoco/cube_stack_scene.xml" - mujoco_viewer: false diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/moveit/picknik_kinova_gen3.srdf b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/moveit/picknik_kinova_gen3.srdf new file mode 100644 index 000000000..0585e62ad --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/moveit/picknik_kinova_gen3.srdf @@ -0,0 +1,178 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/picknik_kinova_gen3.xacro b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/picknik_kinova_gen3.xacro index 5e13a718f..b1ac103ea 100644 --- a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/picknik_kinova_gen3.xacro +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/picknik_kinova_gen3.xacro @@ -564,18 +564,6 @@ - - - - - - - - - - - - diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/README.md b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/README.md index b7fc5e891..75f86eb02 100644 --- a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/README.md +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/docker/README.md @@ -5,7 +5,7 @@ Runs a trained policy against the `Execute Color-Stack Policy` objective via Mov ## 1. Serve (model container) ```bash -docker compose run --rm serve_policy \ +docker compose run --rm --service-ports serve_policy \ --checkpoint --policy-class pi05 ``` From a7aa97dfbe3ab6d9ff70e48cd3f84b3b5e9dc4c7 Mon Sep 17 00:00:00 2001 From: Dan Wahl Date: Sat, 25 Jul 2026 22:22:04 -0500 Subject: [PATCH 05/21] feat(kinova_vla_test_sim): add eval-layout keyframes and a reset objective Rename cube_stack_scene.xml -> scene.xml and split its keyframes out into scene.xml/keyframes.xml (included via ): `default` plus one keyframe per layout (eval_0..eval_149) from the color-cube-stack eval dataset's meta/eval_layouts.jsonl, so the sim can be reset directly to a recorded cube arrangement via ResetMujocoKeyframe. config.yaml needs its own mujoco_model override even though the value now matches the xacro arg's default -- kinova_sim's config.yaml sets its own mujoco_model urdf_param, and dropping the child override lets that parent value win instead of the xacro default, so agent_robot.app tried to load a kinova_sim-only mujoco file that doesn't exist in this package. Add objectives/reset_to_eval_layout.xml as a minimal example (hardcoded to eval_0) of resetting to one of these layouts; a follow-up can parameterize keyframe_name at runtime to loop over all of them. Co-Authored-By: Claude Sonnet 5 --- .../kinova_vla_test_sim/config/config.yaml | 2 +- .../description/mujoco/keyframes.xml | 157 ++++++++++++++++++ .../{cube_stack_scene.xml => scene.xml} | 10 +- .../objectives/reset_to_eval_layout.xml | 36 ++++ 4 files changed, 196 insertions(+), 9 deletions(-) create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/keyframes.xml rename src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/{cube_stack_scene.xml => scene.xml} (84%) create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/reset_to_eval_layout.xml diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml index 1bcb35ccf..2e3d3bd9f 100644 --- a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml @@ -30,7 +30,7 @@ hardware: - use_fake_hardware: "%>> hardware.simulated" - fake_sensor_commands: "false" - use_internal_bus_gripper_comm: "false" - - mujoco_model: "description/mujoco/cube_stack_scene.xml" + - mujoco_model: "description/mujoco/scene.xml" - mujoco_viewer: false moveit_params: diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/keyframes.xml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/keyframes.xml new file mode 100644 index 000000000..ff39c13fd --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/keyframes.xml @@ -0,0 +1,157 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/cube_stack_scene.xml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/scene.xml similarity index 84% rename from src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/cube_stack_scene.xml rename to src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/scene.xml index 90d694a5a..d66509633 100644 --- a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/cube_stack_scene.xml +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/scene.xml @@ -128,13 +128,7 @@ - - - - + diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/reset_to_eval_layout.xml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/reset_to_eval_layout.xml new file mode 100644 index 000000000..4acb6d518 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/objectives/reset_to_eval_layout.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + From 284b9cbf976bdd616f8e7ee130bca5c097c859a0 Mon Sep 17 00:00:00 2001 From: Dan Wahl Date: Sat, 25 Jul 2026 22:45:48 -0500 Subject: [PATCH 06/21] fix(kinova_vla_test_sim): convex-decompose bracelet mesh for MJWarp render Ports the same fix from end-to-end-colorstack's kinova_cube_stack_sim (identical mesh, geom placement, and wrist_camera setup verified before porting). MJWarp's ray tracer decides "is the camera inside this geometry" per triangle, which only works for convex shapes; wrist_camera sits inside the non-convex bracelet_with_vision_link mesh, so every ray hits the nearest interior triangle instead of passing through. Swap in a 19-hull CoACD convex decomposition (render-only geoms, group 2, same compiled placement as the original) and hide the original mesh (group 5). Verified against CPU MuJoCo's native renderer. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01HMkrP7XWDDkDUNVmQrHzEf --- .../bracelet_convex_0.obj | 3 ++ .../bracelet_convex_1.obj | 3 ++ .../bracelet_convex_10.obj | 3 ++ .../bracelet_convex_11.obj | 3 ++ .../bracelet_convex_12.obj | 3 ++ .../bracelet_convex_13.obj | 3 ++ .../bracelet_convex_14.obj | 3 ++ .../bracelet_convex_15.obj | 3 ++ .../bracelet_convex_16.obj | 3 ++ .../bracelet_convex_17.obj | 3 ++ .../bracelet_convex_18.obj | 3 ++ .../bracelet_convex_2.obj | 3 ++ .../bracelet_convex_3.obj | 3 ++ .../bracelet_convex_4.obj | 3 ++ .../bracelet_convex_5.obj | 3 ++ .../bracelet_convex_6.obj | 3 ++ .../bracelet_convex_7.obj | 3 ++ .../bracelet_convex_8.obj | 3 ++ .../bracelet_convex_9.obj | 3 ++ .../description/mujoco/gen3_7dof.xml | 42 +++++++++++++++++++ 20 files changed, 99 insertions(+) create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_0.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_1.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_10.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_11.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_12.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_13.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_14.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_15.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_16.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_17.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_18.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_2.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_3.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_4.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_5.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_6.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_7.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_8.obj create mode 100644 src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_9.obj diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_0.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_0.obj new file mode 100644 index 000000000..696d39d25 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_0.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:200c5946064cb5c5219057c7af7f5a3a7c638ce5058d8f62ea434a00ea45a33a +size 95754 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_1.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_1.obj new file mode 100644 index 000000000..7fa43cd2b --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_1.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:120e230afb4145c9cdcd19f21086456508e7b6b83800511555a3bea1cdc84362 +size 5220 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_10.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_10.obj new file mode 100644 index 000000000..f913d1b8c --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_10.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8e24cf26a6a62031d3ccb3602949c21f89b5249b19c78d974ef4fc348ff85f52 +size 14375 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_11.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_11.obj new file mode 100644 index 000000000..6d8321db5 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_11.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2e7a61525c6148600ce9d0269c75fbb32e7224426407b13cede83b12bc8bedc8 +size 21037 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_12.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_12.obj new file mode 100644 index 000000000..86702f43a --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_12.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:efa7b3b5c50fe5841425048071dbb74489a40a88ca1b42bc4ef13f19dd1c1600 +size 18640 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_13.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_13.obj new file mode 100644 index 000000000..b7703742d --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_13.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8391b80722e39ed690f938260c7e85dbf551d4c2ac011d9e953c918339ed650e +size 1428 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_14.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_14.obj new file mode 100644 index 000000000..28307b6a8 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_14.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c718a92fa738801de98ff2c6e1c056abb253127759c1e04a785fffb10edec6f7 +size 23919 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_15.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_15.obj new file mode 100644 index 000000000..8951a0e5e --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_15.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3502178cd329fa8cddc20d18c53326d7910ce395460b0b9a92b1c95817df26b5 +size 7435 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_16.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_16.obj new file mode 100644 index 000000000..66e1ad113 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_16.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c710c70d5393bed97cb30627b2095cc56acb628837c94d219e94115624ee454a +size 17957 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_17.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_17.obj new file mode 100644 index 000000000..95ce5a363 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_17.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c131978369b705f13e1add72c239364b9882e3426019db72ff4746d7a4d43c51 +size 23426 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_18.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_18.obj new file mode 100644 index 000000000..8675b08f0 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_18.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1c2d8e8e15a8ea372d59d72382f8fc0e243dca814e17e9f02b90aaa8096f5dfd +size 9073 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_2.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_2.obj new file mode 100644 index 000000000..486ceccf0 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_2.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ee058b877e20e1fc6e48a0adb7a5c2dbaf75f7ac191e829923064f3e326f0453 +size 5434 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_3.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_3.obj new file mode 100644 index 000000000..6c2383041 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_3.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:664d46c25e091ddbfacb31102be55e1743bab1ab3829b40ccb3439ca6ab5f88a +size 37408 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_4.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_4.obj new file mode 100644 index 000000000..3abf22743 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_4.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0b1a1689e7a4509860147bed4ba130a9ce20aa3f3fd56022feec4e22805ae5e1 +size 18107 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_5.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_5.obj new file mode 100644 index 000000000..d89e99e4e --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_5.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a06f880f48a046fb78cfe3e45f3136691374371da4c82810349216c466abad11 +size 30633 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_6.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_6.obj new file mode 100644 index 000000000..c6c80a932 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_6.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:80160c18c3ea6370906c2555639ff2c9ecdbcf968cd3196504544d174e974efa +size 21843 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_7.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_7.obj new file mode 100644 index 000000000..08730b1c7 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_7.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3ea628bef2b03f7c24f74dad186e5e4f8d975aeacf9d9a778a0ee84d46c13ea9 +size 26696 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_8.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_8.obj new file mode 100644 index 000000000..d4f0d6782 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_8.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a41cc720f7916d370207b2893077b5f5025e2b3672e046fb43e8d2756b613c33 +size 23518 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_9.obj b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_9.obj new file mode 100644 index 000000000..f2a05c8ae --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/assets/kinova/bracelet_with_vision_link_convex/bracelet_convex_9.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3a67149b83d2c80a14cdd4fff7c3110afb043c948d478ea468aec05e23eea155 +size 14182 diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/gen3_7dof.xml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/gen3_7dof.xml index fbdfd6cc2..43403a2f7 100644 --- a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/gen3_7dof.xml +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/gen3_7dof.xml @@ -143,6 +143,26 @@ name="bracelet_with_vision_link" file="kinova/bracelet_with_vision_link.obj" /> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Date: Mon, 27 Jul 2026 12:07:34 -0500 Subject: [PATCH 07/21] fix(kinova_vla_test_sim): set impratio=20 so a pinched cube cannot slide In a top-down pinch the cube's whole weight is carried by frictional contact force, and at the default impratio=1 the friction constraint is too compliant to hold it: the cube slides through the pads for the entire carry and is still sliding at release. Median cube drift in the PAD frame over 32 held-out layouts, max / creep: oracle / GPU 12.7 / 6.2mm -> 2.94 / 0.27mm oracle / CPU 1.7 / 0.5mm -> 2.93 / 0.22mm pi0.5 / GPU 8.5 / 4.7mm -> 2.66 / 0.78mm Load-bearing for policy eval, not just the oracle, and it fails SILENTLY: a sinking cube still clears the 5cm lift gate, so pick barely moves (76.7 -> 80.0%) while stack halves (23.3 -> 35.0% at impratio=20, a within-condition A/B). The cube is released ~15mm below where the policy thinks it is, against an 18mm stack tolerance -- which reads as a bad policy, not a physics setting. Sweeping impratio knees at ~10 and plateaus after, so 20 sits past the knee with margin. Ruled out as causes: solver tolerance and iteration count, narrowphase manifold size, and grasp height (+/-12mm moves slip <0.6mm). Brings this cell in line with end-to-end-colorstack's kinova_cube_stack_sim, which carries the full derivation. It was the only functional drift between the two: the scene XMLs are already physics-identical, and the gripper damping and actuator kv/forcerange had been ported previously. Verified: scene.xml loads with impratio=20.0, nq=36. Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01JYRSviSypggS2eCjAj9BxG --- .../kinova_vla_test_sim/description/mujoco/gen3_7dof.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/gen3_7dof.xml b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/gen3_7dof.xml index 43403a2f7..28c841f84 100644 --- a/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/gen3_7dof.xml +++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim/description/mujoco/gen3_7dof.xml @@ -1,6 +1,7 @@ -