PickNik ur mujoco base config (#284)

Co-authored-by: Nathan Brooks <[email protected]> Co-authored-by: Sebastian Jahr <[email protected]> Co-authored-by: David Yackzan <[email protected]>
PickNikRobotics · Jul 9, 2024 · e76d2a3 · e76d2a3
1 parent d82234b
commit e76d2a3
Show file tree

Hide file tree

Showing 73 changed files with 841,469 additions and 121 deletions.
diff --git a/src/picknik_ur_base_config/config/moveit/picknik_ur.srdf b/src/picknik_ur_base_config/config/moveit/picknik_ur.srdf
@@ -53,7 +53,6 @@
     <passive_joint name="robotiq_85_left_inner_knuckle_joint"/>
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="base_link_inertia" link2="shoulder_link" reason="Adjacent"/>
-    <disable_collisions link1="base_link_inertia" link2="environment" reason="Adjacent"/>
     <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent"/>
     <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent"/>
     <disable_collisions link1="forearm_link" link2="wrist_2_link" reason="Never"/>
@@ -183,7 +182,6 @@
     <disable_collisions link1="robotiq_85_right_knuckle_link" link2="wrist_2_link" reason="Never"/>
     <disable_collisions link1="robotiq_85_right_knuckle_link" link2="wrist_3_link" reason="Never"/>
     <disable_collisions link1="robotiq_85_right_knuckle_link" link2="wrist_mounted_camera_link" reason="Never"/>
-    <disable_collisions link1="shoulder_link" link2="environment" reason="Never"/>
     <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent"/>
     <disable_collisions link1="tool_changer_link" link2="ur_to_robotiq_link" reason="Never"/>
     <disable_collisions link1="tool_changer_link" link2="wrist_1_link" reason="Never"/>

diff --git a/src/picknik_ur_base_config/description/environment.xacro b/src/picknik_ur_base_config/description/environment.xacro
diff --git a/src/picknik_ur_base_config/description/picknik_ur.xacro b/src/picknik_ur_base_config/description/picknik_ur.xacro
@@ -5,6 +5,7 @@
   <xacro:arg name="external_camera" default="false"/>
   <xacro:arg name="has_tool_changer" default="true"/>
   <xacro:arg name="use_pinch_links" default="true"/>
+  <xacro:arg name="generate_ros2_control_tag" default="true"/>
   <xacro:arg name="name" default=""/>
   <xacro:arg name="joint_limits_parameters_file" default=""/>
   <xacro:arg name="kinematics_parameters_file" default=""/>
@@ -16,22 +17,17 @@
   <xacro:arg name="tool_voltage" default="0"/>
   <xacro:arg name="tool_device_name" default="/dev/ttyUSB0"/>
   <xacro:arg name="initial_positions_file" default="$(find picknik_ur_base_config)/config/initial_positions.yaml"/>
-  <xacro:arg name="environment_xacro" default="$(find picknik_ur_base_config)/description/environment.xacro"/>
 
   <!-- Import UR and environment macros -->
   <xacro:include filename="$(find picknik_ur_base_config)/description/picknik_ur_macro.xacro"/>
   <xacro:include filename="$(find picknik_ur_base_config)/description/picknik_ur_attachments_macro.xacro"/>
-  <xacro:include filename="$(arg environment_xacro)"/>
 
   <!-- Robot: contains robot description from the world frame to the tool frame -->
-  <xacro:picknik_ur parent="world"  child="tool0" initial_positions_file="$(arg initial_positions_file)" >
+  <xacro:picknik_ur parent="world"  child="tool0" initial_positions_file="$(arg initial_positions_file)" generate_ros2_control_tag="$(arg generate_ros2_control_tag)" >
     <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:picknik_ur>
 
   <!-- Gripper and UR adapter and realsense camera -->
   <xacro:picknik_ur_attachments parent="tool0" child="grasp_link" has_tool_changer="$(arg has_tool_changer)"/>
 
-  <!-- Environment: contains scene geometry and external sensors, e.g. cameras -->
-  <xacro:environment parent="world"/>
-
 </robot>
diff --git a/src/picknik_ur_base_config/description/picknik_ur_macro.xacro b/src/picknik_ur_base_config/description/picknik_ur_macro.xacro
@@ -1,11 +1,12 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-    <xacro:macro name="picknik_ur" params="parent child initial_positions_file *origin">
+    <xacro:macro name="picknik_ur" params="parent child initial_positions_file generate_ros2_control_tag *origin">
         <!-- parameters -->
         <xacro:arg name="use_fake_hardware" default="false"/>
         <xacro:arg name="external_camera" default="false"/>
         <xacro:arg name="has_tool_changer" default="true"/>
         <xacro:arg name="use_pinch_links" default="true"/>
+        <xacro:arg name="generate_ros2_control_tag" default="true"/>
         <xacro:arg name="name" default=""/>
         <xacro:arg name="joint_limits_parameters_file" default=""/>
         <xacro:arg name="kinematics_parameters_file" default=""/>
@@ -52,6 +53,7 @@
                 headless_mode="$(arg headless_mode)"
                 robot_ip="$(arg robot_ip)"
                 use_tool_communication="$(arg use_tool_communication)"
+                generate_ros2_control_tag="$(arg generate_ros2_control_tag)"
                 tool_voltage="$(arg tool_voltage)"
                 tool_device_name="$(arg tool_device_name)"
                 script_filename="$(find ur_robot_driver)/resources/ros_control.urscript"
@@ -77,6 +79,5 @@
             <xacro:forearm_pinch_link/>
             <xacro:wrist_3_pinch_link/>
         </xacro:if>
-
     </xacro:macro>
 </robot>
diff --git a/src/picknik_ur_gazebo_config/description/picknik_ur_gazebo.xacro b/src/picknik_ur_gazebo_config/description/picknik_ur_gazebo.xacro
@@ -5,6 +5,7 @@
   <xacro:arg name="external_camera" default="false"/>
   <xacro:arg name="has_tool_changer" default="true"/>
   <xacro:arg name="use_pinch_links" default="true"/>
+  <xacro:arg name="generate_ros2_control_tag" default="true"/>
   <xacro:arg name="name" default=""/>
   <xacro:arg name="joint_limits_parameters_file" default=""/>
   <xacro:arg name="kinematics_parameters_file" default=""/>
@@ -27,7 +28,7 @@
   <xacro:include filename="$(arg environment_xacro)"/>
 
   <!-- Robot: contains robot description from the world frame to the tool frame -->
-  <xacro:picknik_ur parent="world"  child="tool0" initial_positions_file="$(arg initial_positions_file)">
+  <xacro:picknik_ur parent="world"  child="tool0" initial_positions_file="$(arg initial_positions_file)" generate_ros2_control_tag="$(arg generate_ros2_control_tag)">
     <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:picknik_ur>
 

diff --git a/src/picknik_ur_mock_hw_config/description/picknik_ur_machine_tending.xacro b/src/picknik_ur_mock_hw_config/description/picknik_ur_machine_tending.xacro
@@ -5,6 +5,7 @@
     <xacro:arg name="external_camera" default="false"/>
     <xacro:arg name="has_tool_changer" default="true"/>
     <xacro:arg name="use_pinch_links" default="true"/>
+    <xacro:arg name="generate_ros2_control_tag" default="true"/>
     <xacro:arg name="name" default=""/>
     <xacro:arg name="joint_limits_parameters_file" default=""/>
     <xacro:arg name="kinematics_parameters_file" default=""/>
@@ -24,7 +25,7 @@
     <xacro:include filename="$(arg environment_xacro)"/>
 
     <!-- Robot: contains robot description from the world frame to the tool frame -->
-    <xacro:picknik_ur parent="world"  child="tool0" initial_positions_file="$(arg initial_positions_file)">
+    <xacro:picknik_ur parent="world"  child="tool0" initial_positions_file="$(arg initial_positions_file)" generate_ros2_control_tag="$(arg generate_ros2_control_tag)">
       <origin xyz="0 0 0" rpy="0 0 ${pi/2}" />
     </xacro:picknik_ur>
 

diff --git a/src/picknik_ur_mujoco_config/CMakeLists.txt b/src/picknik_ur_mujoco_config/CMakeLists.txt
@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.22)
+project(picknik_ur_mujoco_config)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY
+    config
+    description
+    launch
+    objectives
+    waypoints
+  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/picknik_ur_mujoco_config/LICENSE b/src/picknik_ur_mujoco_config/LICENSE
@@ -0,0 +1,25 @@
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+
+   * Neither the name of the copyright holder nor the names of its
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
diff --git a/src/picknik_ur_mujoco_config/README.md b/src/picknik_ur_mujoco_config/README.md
@@ -0,0 +1,5 @@
+# picknik_ur_mujoco_config
+
+A MoveIt Pro MuJoCo simulation for PickNik's Universal Robots (UR) arms.
+
+For detailed documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/)
diff --git a/src/picknik_ur_mujoco_config/config/config.yaml b/src/picknik_ur_mujoco_config/config/config.yaml
@@ -0,0 +1,87 @@
+#
+#  This contains information for a unique instance of a robot.
+#
+
+# Name of the package to specialize
+based_on_package: "picknik_ur_base_config"
+hardware:
+  # Parameters used to configure the robot description through XACRO.
+  # A URDF and SRDF are both required.
+  # [Required]
+  robot_description:
+    urdf:
+      package: "picknik_ur_mujoco_config"
+      path: "description/picknik_ur.xacro"
+    srdf:
+      package: "picknik_ur_mujoco_config"
+      path: "config/moveit/picknik_ur.srdf"
+    urdf_params:
+      - mujoco_model: "description/scene.xml"
+
+moveit_params:
+  servo:
+    package: "picknik_ur_mujoco_config"
+    path: "config/moveit/ur_servo.yaml"
+  joint_limits:
+    package: "picknik_ur_mujoco_config"
+    path: "config/moveit/joint_limits.yaml"
+  servo_joint_limits:
+    package: "picknik_ur_mujoco_config"
+    path: "config/moveit/hard_joint_limits.yaml"
+
+# Configuration for loading behaviors and objectives.
+# [Required]
+objectives:
+  # List of plugins for loading custom behaviors.
+  # [Required]
+  behavior_loader_plugins:
+    # This plugin will load the core MoveIt Pro Behaviors.
+    # Add additional plugin loaders as needed.
+    core:
+      - "moveit_studio::behaviors::CoreBehaviorsLoader"
+      - "moveit_studio::behaviors::MTCCoreBehaviorsLoader"
+      - "moveit_studio::behaviors::ServoBehaviorsLoader"
+      - "moveit_studio::behaviors::VisionBehaviorsLoader"
+  # Specify source folder for objectives
+  # [Required]
+  objective_library_paths:
+    sim_objectives:
+      package_name: "picknik_ur_mujoco_config"
+      relative_path: "objectives"
+  # Specify the location of the saved waypoints file.
+  # [Required]
+  waypoints_file:
+    package_name: "picknik_ur_mujoco_config"
+    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"
+    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
+  # for running the application.
+  # [Optional, default=[]]
+  controllers_active_at_startup:
+    - "force_torque_sensor_broadcaster"
+    - "robotiq_gripper_controller"
+    - "joint_state_broadcaster"
+    - "servo_controller"
+  # Load but do not start these controllers so they can be activated later if needed.
+  controllers_inactive_at_startup:
+    - "joint_trajectory_controller"
+  # Any controllers here will not be spawned by MoveIt Pro.
+  # [Optional, default=[]]
+  controllers_not_managed: []
+  # Optionally configure remapping rules to let multiple controllers receive commands on the same topic.
+  # [Optional, default=[]]
+  controller_shared_topics: []
+
+# Octomap manager configuration parameters
+octomap_manager:
+  # Input point cloud topic name. The *output* point cloud topic published by
+  # the Octomap manager node is defined in sensors_3d.yaml.
+  input_point_cloud_topic: "/wrist_camera/points"