Kinova block stack config for VLA testing

.gitignore

@@ -4,3 +4,6 @@ log/
 .vscode/
 MUJOCO_LOG.TXT
 .ccache/
+__pycache__/
+.pytest_cache/
+.venv/

docker-compose.yaml

@@ -10,7 +10,11 @@ services:
   base: {}
 
   # Starts the MoveIt Pro Agent and the Bridge between the Agent and the Web UI.
-  agent_bridge: {}
+  agent_bridge:
+    environment:
+      # /joint_states carries all fifteen joints; the recorded `observation.state`
+      # must be the eight the policies use. See script/joint_command_bridge.py.
+      - MOVEIT_PRO_TRAIN_JOINT_STATES_TOPIC=/observed_joint_states
 
   # Starts the robot drivers.
   drivers:
@@ -23,6 +27,10 @@ services:
 
   # Developer specific configuration when running `moveit_pro dev`.
   dev:
+    environment:
+      # /joint_states carries all fifteen joints; the recorded `observation.state`
+      # must be the eight the policies use. See script/joint_command_bridge.py.
+      - MOVEIT_PRO_TRAIN_JOINT_STATES_TOPIC=/observed_joint_states
     volumes:
       # Allow access to host hardware e.g. RealSense cameras
       - /dev:/dev

src/moveit_pro_kinova_configs/kinova_vla_test_sim/CMakeLists.txt

@@ -0,0 +1,41 @@
+cmake_minimum_required(VERSION 3.22)
+project(kinova_vla_test_sim)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY
+    config
+    description
+    launch
+    objectives
+  DESTINATION
+    share/${PROJECT_NAME}
+)
+
+install(PROGRAMS
+  script/get_action_chunk_adapter.py
+  script/joint_command_bridge.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
+    test/test_joint_command_bridge.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()
+
+ament_package()

src/moveit_pro_kinova_configs/kinova_vla_test_sim/README.md

@@ -0,0 +1,8 @@
+# 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.
+
+- `train/`: how the pi0.5 checkpoint is trained, as a config file plus a pinned environment
+- `docker/`: how that checkpoint is served to the `Execute Color-Stack Policy` objective
+
+For detailed documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/)

src/moveit_pro_kinova_configs/kinova_vla_test_sim/config/config.yaml

@@ -0,0 +1,48 @@
+###############################################################
+#
+#  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
+  simulated_hardware_launch_file:
+    package: "kinova_vla_test_sim"
+    path: "launch/simulated_extras.launch.py"
+
+  robot_description:
+    urdf:
+      package: "kinova_vla_test_sim"
+      path: "description/picknik_kinova_gen3.xacro"
+    srdf:
+      package: "kinova_vla_test_sim"
+      path: "config/moveit/picknik_kinova_gen3.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"
+      - use_internal_bus_gripper_comm: "false"
+      - mujoco_model: "description/mujoco/scene.xml"
+      - mujoco_viewer: false
+
+moveit_params:
+  joint_limits:
+    package: "kinova_vla_test_sim"
+    path: "config/moveit/joint_limits.yaml"
+
+objectives:
+  behavior_loader_plugins:
+    kinova_vla_test_sim:
+      - "kinova_vla_test_sim_behaviors::KinovaVlaTestSimBehaviorsLoader"
+  objective_library_paths:
+    colorstack_objectives:
+      package_name: "kinova_vla_test_sim"
+      relative_path: "objectives"

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