Mujoco 6.0 (#347)

* updates to mujoco config

* Update deprecated param (#336)

* Mujoco scene cosmetic improvements (#337)

* update scene camera position

* rotate scene 90 deg, add adapter to linear rail

---------

Co-authored-by: Michael Wrock <mike.wrock@gmail.com>

* merging branches

* updates

* Changed integration tests

* increased timeout

* increased timeout

* Subtree and other updates

* Apply suggestions from code review

* formatting

* updated vfc start waypoint

* add VFC stop conditions back

* Teleop objective refactor (#314)

* refactored teleoperate so that it does not use subtrees, and make the subtrees that were used now useable elsewhere

---------

Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai>

* attempt to fix ci

* removed unreleased objectives

* kepts force_relaxation

* formatting

* fix typo

* updated diagram

* updated diagram

* updated objectives

* update

* updating objectives

* renaming config

---------

Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai>
Co-authored-by: Mario Prats <mario.prats@picknik.ai>

src/picknik_ur_mujoco_config/CMakeLists.txt → src/arm_on_rail_sim/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.22)
-project(picknik_ur_mujoco_config)
+project(arm_on_rail_sim)
 
 find_package(ament_cmake REQUIRED)
 

src/picknik_ur_mujoco_config/README.md → src/arm_on_rail_sim/README.md

@@ -1,4 +1,4 @@
-# picknik_ur_mujoco_config
+# arm_on_rail_sim
 
 A MoveIt Pro MuJoCo simulation for PickNik's Universal Robots (UR) arms.
 

src/picknik_ur_mujoco_config/config/config.yaml → src/arm_on_rail_sim/config/config.yaml

@@ -10,23 +10,23 @@ hardware:
   # [Required]
   robot_description:
     urdf:
-      package: "picknik_ur_mujoco_config"
+      package: "arm_on_rail_sim"
       path: "description/picknik_ur.xacro"
     srdf:
-      package: "picknik_ur_mujoco_config"
+      package: "arm_on_rail_sim"
       path: "config/moveit/picknik_ur.srdf"
     urdf_params:
       - mujoco_model: "description/scene.xml"
 
 moveit_params:
   servo:
-    package: "picknik_ur_mujoco_config"
+    package: "arm_on_rail_sim"
     path: "config/moveit/ur_servo.yaml"
   joint_limits:
-    package: "picknik_ur_mujoco_config"
+    package: "arm_on_rail_sim"
     path: "config/moveit/joint_limits.yaml"
   servo_joint_limits:
-    package: "picknik_ur_mujoco_config"
+    package: "arm_on_rail_sim"
     path: "config/moveit/hard_joint_limits.yaml"
 
 # Configuration for loading behaviors and objectives.
@@ -46,20 +46,20 @@ objectives:
   # [Required]
   objective_library_paths:
     sim_objectives:
-      package_name: "picknik_ur_mujoco_config"
+      package_name: "arm_on_rail_sim"
       relative_path: "objectives"
   # Specify the location of the saved waypoints file.
   # [Required]
   waypoints_file:
-    package_name: "picknik_ur_mujoco_config"
+    package_name: "arm_on_rail_sim"
     relative_path: "waypoints/ur_waypoints.yaml"
 
 
 # Configuration for launching ros2_control processes.
 # [Required, if using ros2_control]
 ros2_control:
   config:
-    package: "picknik_ur_mujoco_config"
+    package: "arm_on_rail_sim"
     path: "config/control/picknik_ur.ros2_control.yaml"
   # MoveIt Pro will load and activate these controllers at start up to ensure they are available.
   # If not specified, it is up to the user to ensure the appropriate controllers are active and available

src/picknik_ur_mujoco_config/config/moveit/picknik_ur.srdf → src/arm_on_rail_sim/config/moveit/picknik_ur.srdf

@@ -31,12 +31,12 @@
     </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="open" group="gripper">
-        <joint name="robotiq_85_left_knuckle_joint" value="0"/>
-        <joint name="robotiq_85_right_knuckle_joint" value="0"/>
+        <joint name="robotiq_85_left_knuckle_joint" value="0.05"/>
+        <joint name="robotiq_85_right_knuckle_joint" value="0.05"/>
     </group_state>
     <group_state name="close" group="gripper">
-        <joint name="robotiq_85_left_knuckle_joint" value="0.7929"/>
-        <joint name="robotiq_85_right_knuckle_joint" value="-0.7929"/>
+        <joint name="robotiq_85_left_knuckle_joint" value="0.79"/>
+        <joint name="robotiq_85_right_knuckle_joint" value="-0.79"/>
     </group_state>
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
     <end_effector name="moveit_ee" parent_link="tool0" group="gripper"/>

src/picknik_ur_mujoco_config/description/picknik_ur.xacro → src/arm_on_rail_sim/description/picknik_ur.xacro

@@ -981,7 +981,7 @@
     <hardware>
       <plugin>picknik_mujoco_ros/MujocoSystem</plugin>
       <param name="mujoco_model">$(arg mujoco_model)</param>
-      <param name="mujoco_model_package">picknik_ur_mujoco_config</param>
+      <param name="mujoco_model_package">arm_on_rail_sim</param>
       <param name="render_publish_rate">10</param>
       <param name="tf_publish_rate">60</param>
       <param name="lidar_publish_rate">10</param>

src/picknik_ur_mujoco_config/description/scene.xml → src/arm_on_rail_sim/description/scene.xml

@@ -51,7 +51,7 @@
         <body name="block2" pos="0.0 0.75 0.575">
             <freejoint name="block2"/>
             <geom class="visual" mesh="cube" pos="0 0 -0.03"/>
-            <geom class="collision" type="box" size="0.0325 0.0325 0.0325" pos="0 0 0"/>
+            <geom class="collision" type="box" size="0.025 0.025 0.025" pos="0 0 0"/>
         </body>
         <body name="block3" pos="-0.2 0.75 0.575">
             <freejoint name="block3"/>
@@ -78,7 +78,7 @@
       -0.2 0.75 0.51 1 0 0 0
     "
     ctrl="
-      0 0 -1.5707 1.5707 -1.8 -1.5707 0 0
+      0 0 -1.0 1.0 -1.57 -1.57 0 0
     "
     />
   </keyframe>

src/picknik_ur_mujoco_config/objectives/apriltag_pick_object.xml → src/arm_on_rail_sim/objectives/apriltag_pick_object.xml

@@ -3,6 +3,8 @@
   <!--//////////-->
   <BehaviorTree ID="Pick April Tag Labeled Object" _description="Picks up an object that has an AprilTag marker." _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree ID="Look at table" _collapsed="true"/>
+      <Action ID="ClearSnapshot"/>
       <SubTree ID="Wrist camera snapshot" _collapsed="true"/>
       <Control ID="Sequence" name="Setup">
         <SubTree ID="Open Gripper"/>
@@ -13,10 +15,11 @@
       <Action ID="DetectAprilTags" image="{image}" camera_info="{camera_info}" parameters="{parameters}" detections="{detections}"/>
       <Action ID="GetDetectionPose"/>
       <Action ID="TransformPoseFrame" input_pose="{detection_pose}"/>
+      <Action ID="TransformPose" output_pose="{offset_pose}" input_pose="{output_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;0;-0.01"/>
       <Action ID="LoadObjectiveParameters" config_file_name="pick_object_config.yaml"/>
       <Action ID="InitializeMTCTask" task_id="pick_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{pick_object_task}"/>
       <Action ID="SetupMTCCurrentState" task="{pick_object_task}"/>
-      <Action ID="SetupMTCPickObject" grasp_pose="{output_pose}" task="{pick_object_task}" parameters="{parameters}"/>
+      <Action ID="SetupMTCPickObject" grasp_pose="{offset_pose}" task="{pick_object_task}" parameters="{parameters}"/>
       <Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/>
       <Action ID="ExecuteMTCTask" solution="{pick_object_solution}"/>
     </Control>

src/picknik_ur_mujoco_config/objectives/load_mesh_as_green_poincloud.xml → src/arm_on_rail_sim/objectives/load_mesh_as_green_poincloud.xml

@@ -3,7 +3,7 @@
   <!--//////////-->
   <BehaviorTree ID="Load Mesh as Green Pointcloud" _description="" _favorite="false" _subtreeOnly="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="LoadPointCloudFromFile" color="0;255;0" file_path="~/user_ws/install/picknik_ur_mujoco_config/share/picknik_ur_mujoco_config/description/assets/Cube.stl" point_cloud="{point_cloud}"/>
+      <Action ID="LoadPointCloudFromFile" color="0;255;0" file_path="~/user_ws/install/arm_on_rail_sim/share/arm_on_rail_sim/description/assets/Cube.stl" point_cloud="{point_cloud}"/>
       <Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" output_cloud="{point_cloud}"/>
       <Action ID="TransformPointCloud" input_cloud="{point_cloud}" transform_pose="{initial_pose}"/>
     </Control>

src/picknik_ur_mujoco_config/objectives/load_mesh_as_red_pointcloud.xml → src/arm_on_rail_sim/objectives/load_mesh_as_red_pointcloud.xml

@@ -3,7 +3,7 @@
   <!--//////////-->
   <BehaviorTree ID="Load Mesh as Red Pointcloud" _description="" _favorite="false" _subtreeOnly="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="LoadPointCloudFromFile" color="255;0;0" file_path="~/user_ws/install/picknik_ur_mujoco_config/share/picknik_ur_mujoco_config/description/assets/Cube.stl" point_cloud="{point_cloud}"/>
+      <Action ID="LoadPointCloudFromFile" color="255;0;0" file_path="~/user_ws/install/arm_on_rail_sim/share/arm_on_rail_sim/description/assets/Cube.stl" point_cloud="{point_cloud}"/>
       <Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" output_cloud="{point_cloud}"/>
       <Action ID="TransformPointCloud" input_cloud="{point_cloud}" transform_pose="{initial_pose}"/>
     </Control>

src/arm_on_rail_sim/objectives/pick_object.xml

@@ -0,0 +1,17 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Pick object">
+  <!--//////////-->
+  <BehaviorTree ID="Pick object" _description="" _favorite="false" _subtreeOnly="true">
+    <Control ID="Sequence" name="root">
+      <SubTree ID="Open Gripper"/>
+      <Control ID="Sequence">
+        <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
+        <Action ID="CreateStampedPose" orientation_xyzw="0;1;0;0" position_xyz="0.01;.75;0.525"/>
+        <!--Create a vector of poses to match the input of AdjustPoseWithIMarker-->
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{initial_poses}"/>
+        <Action ID="AdjustPoseWithIMarker" prompts="Adjust IMarker to desired pose" initial_poses="{initial_poses}" adjusted_poses="{pose_vector}"/>
+      </Control>
+      <SubTree ID="Pick first object in vector" pose_vector="{pose_vector}" _collapsed="false"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/arm_on_rail_sim/objectives/pick_object_config.yaml

@@ -0,0 +1,9 @@
+SetupMTCPickObject:
+  approach_distance: 0.05
+  arm_group_name: manipulator
+  end_effector_closed_pose_name: close
+  end_effector_group_name: gripper
+  end_effector_name: moveit_ee
+  hand_frame_name: grasp_link
+  lift_distance: 0.05
+  world_frame_name: world

src/arm_on_rail_sim/objectives/place_object.xml

@@ -0,0 +1,20 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Place object">
+  <!--//////////-->
+  <BehaviorTree ID="Place object" _description="" _favorite="true">
+    <Control ID="Sequence" name="root">
+      <Control ID="Sequence">
+        <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
+        <Action ID="CreateStampedPose" orientation_xyzw="0;1;0;0" position_xyz="0.01;.75;0.525"/>
+        <!--Create a vector of poses to match the input of AdjustPoseWithIMarker-->
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{initial_poses}"/>
+        <Action ID="AdjustPoseWithIMarker" adjusted_poses="{corrected_poses}" prompts="Adjust place position"/>
+      </Control>
+      <!--Use the ForEach decorator to get a pose from our vector of poses-->
+      <SubTree ID="Place first object in vector" pose_vector="{corrected_poses}" _collapsed="false"/>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Place object"/>
+  </TreeNodesModel>
+</root>

src/picknik_ur_mujoco_config/objectives/place_object_config.yaml → src/arm_on_rail_sim/objectives/place_object_config.yaml

@@ -7,5 +7,5 @@ SetupMTCPickObject:
   hand_frame_name: "grasp_link"
   end_effector_closed_pose_name: "open"
 
-  approach_distance: 0.1
-  lift_distance: 0.1
+  approach_distance: 0.05
+  lift_distance: 0.05

src/picknik_ur_mujoco_config/objectives/register_cad_part.xml → src/arm_on_rail_sim/objectives/register_cad_part.xml

@@ -3,16 +3,17 @@
   <!--//////////-->
   <BehaviorTree ID="Register CAD Part" _description="" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="CreateStampedPose" position_xyz="0.02;.75;0.48" orientation_xyzw="0;0;0;1"/>
-      <SubTree ID="Load Mesh as Red Pointcloud" _collapsed="false" output_cloud="{red_cloud}" initial_pose="{stamped_pose}"/>
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Look at table" _collapsed="true"/>
+      <Action ID="CreateStampedPose" position_xyz="0.02;.75;0.58" orientation_xyzw="0;0;0;1" name="Initial Guess"/>
+      <SubTree ID="Load Mesh as Red Pointcloud" _collapsed="false" output_cloud="{red_cloud}" initial_pose="{stamped_pose}" point_cloud="{red_cloud}"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{red_cloud}"/>
       <Action ID="GetPointCloud" topic_name="/wrist_camera/points" message_out="{wrist_point_cloud}"/>
       <Action ID="TransformPointCloudFrame" input_cloud="{wrist_point_cloud}" output_cloud="{wrist_point_cloud}"/>
-      <Action ID="CropPointsInBox" crop_box_centroid_pose="{stamped_pose}" point_cloud="{wrist_point_cloud}" point_cloud_cropped="{wrist_point_cloud}" crop_box_size="0.4;0.4;0.7"/>
       <Action ID="SendPointCloudToUI" point_cloud="{wrist_point_cloud}"/>
-      <Action ID="RegisterPointClouds" target_point_cloud="{wrist_point_cloud}" base_point_cloud="{red_cloud}" max_correspondence_distance="0.005" max_iterations="100"/>
-      <SubTree ID="Load Mesh as Green Pointcloud" _collapsed="false" output_cloud="{green_cloud}" initial_pose="{stamped_pose}"/>
+      <Action ID="RegisterPointClouds" target_point_cloud="{wrist_point_cloud}" base_point_cloud="{red_cloud}" max_correspondence_distance="0.5" max_iterations="100"/>
+      <SubTree ID="Load Mesh as Green Pointcloud" _collapsed="false" output_cloud="{green_cloud}" initial_pose="{stamped_pose}" point_cloud="{green_cloud}"/>
       <Action ID="TransformPointCloud" input_cloud="{green_cloud}" transform_pose="{target_pose}" output_cloud="{output_cloud_aligned}"/>
-      <Action ID="SendPointCloudToUI" point_cloud="{red_cloud}"/>
       <Action ID="SendPointCloudToUI" point_cloud="{output_cloud_aligned}"/>
     </Control>
   </BehaviorTree>

src/picknik_ur_mujoco_config/package.xml → src/arm_on_rail_sim/package.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <package format="3">
-  <name>picknik_ur_mujoco_config</name>
+  <name>arm_on_rail_sim</name>
   <version>6.0.0</version>
 
   <description>MuJoCo simulation configuration package for Picknik's UR robot on a linear rail</description>

src/picknik_ur_studio_integration_testing/test/launch/test_basic_ur5e_objectives.test.py

@@ -43,7 +43,7 @@ def generate_test_description():
         gtest_name="test_basic_ur5e_objectives",
         env_vars={
             # Load site config for simulated UR-5e
-            "STUDIO_CONFIG_PACKAGE": "picknik_ur_mujoco_config",
+            "STUDIO_CONFIG_PACKAGE": "arm_on_rail_sim",
             "MOCK_HARDWARE": "true",
         },
     )