Add mobile ur config (#304)

---
Co-authored-by: Mario Prats <[email protected]>

.gitmodules

@@ -13,3 +13,11 @@
 	path = src/external_dependencies/gz_ros2_control
 	url = https://github.com/PickNikRobotics/gz_ros2_control.git
 	branch = mimic-joint-feature-humble
+[submodule "src/external_dependencies/LMS1xx"]
+	path = src/external_dependencies/LMS1xx
+	url = https://github.com/clearpathrobotics/LMS1xx.git
+	branch = humble-devel
+[submodule "src/external_dependencies/ridgeback"]
+	path = src/external_dependencies/ridgeback
+	url = https://github.com/sjahr/ridgeback.git
+	branch = ros2

src/picknik_ur_mobile_config/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.22)
+project(picknik_ur_mobile_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()

src/picknik_ur_mobile_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.

src/picknik_ur_mobile_config/README.md

@@ -0,0 +1,5 @@
+# picknik_ur_mobile_config
+
+A MoveIt Pro MuJoCo simulation of a UR5 arm on a mobile Ridgeback base.
+
+For detailed documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/)

src/picknik_ur_mobile_config/config/config.yaml

@@ -0,0 +1,89 @@
+#
+#  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_mobile_config"
+      path: "description/picknik_ur.xacro"
+    srdf:
+      package: "picknik_ur_mobile_config"
+      path: "config/moveit/picknik_ur.srdf"
+    urdf_params:
+      - joint_limits_parameters_file:
+          package: "picknik_ur_mobile_config"
+          path: "config/moveit/joint_limits.yaml"
+
+moveit_params:
+  servo:
+    package: "picknik_ur_mobile_config"
+    path: "config/moveit/ur_servo.yaml"
+  joint_limits:
+    package: "picknik_ur_mobile_config"
+    path: "config/moveit/joint_limits.yaml"
+  servo_joint_limits:
+    package: "picknik_ur_mobile_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_mobile_config"
+      relative_path: "objectives"
+  # Specify the location of the saved waypoints file.
+  # [Required]
+  waypoints_file:
+    package_name: "picknik_ur_mobile_config"
+    relative_path: "waypoints/ur_waypoints.yaml"
+
+
+# Configuration for launching ros2_control processes.
+# [Required, if using ros2_control]
+ros2_control:
+  config:
+    package: "picknik_ur_mobile_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"

src/picknik_ur_mobile_config/config/control/picknik_ur.ros2_control.yaml

@@ -0,0 +1,175 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 600  # Hz
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+    robotiq_gripper_controller:
+      type: position_controllers/GripperActionController
+    servo_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    force_torque_sensor_broadcaster:
+      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
+
+joint_state_broadcaster:
+  ros__parameters:
+    use_local_topics: false
+    joints:
+      - linear_x_joint
+      - linear_y_joint
+      - rotational_yaw_joint
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+      - robotiq_85_left_knuckle_joint
+    interfaces:
+      - position
+      - velocity
+      - effort
+
+joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - linear_x_joint
+      - linear_y_joint
+      - rotational_yaw_joint
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    command_joints:
+      - linear_x_joint
+      - linear_y_joint
+      - rotational_yaw_joint
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.0
+      goal_time: 0.0
+      linear_x_joint:
+        goal: 0.05
+      linear_y_joint:
+        goal: 0.05
+      rotational_yaw_joint:
+        goal: 0.05
+      shoulder_pan_joint:
+        goal: 0.05
+      shoulder_lift_joint:
+        goal: 0.05
+      elbow_joint:
+        goal: 0.05
+      wrist_1_joint:
+        goal: 0.05
+      wrist_2_joint:
+        goal: 0.05
+      wrist_3_joint:
+        goal: 0.05
+    acceleration_limits:
+      linear_x_joint: 10.0
+      linear_y_joint: 10.0
+      rotational_yaw_joint: 10.0
+      shoulder_pan_joint: 10.0
+      shoulder_lift_joint: 10.0
+      elbow_joint: 10.0
+      wrist_1_joint: 10.0
+      wrist_2_joint: 10.0
+      wrist_3_joint: 10.0
+
+robotiq_gripper_controller:
+  ros__parameters:
+    default: true
+    joint: robotiq_85_left_knuckle_joint
+    allow_stalling: true
+servo_controller:
+  ros__parameters:
+    joints:
+      - linear_x_joint
+      - linear_y_joint
+      - rotational_yaw_joint
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    command_joints:
+      - linear_x_joint
+      - linear_y_joint
+      - rotational_yaw_joint
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.0
+      goal_time: 0.0
+      linear_x_joint:
+        goal: 0.05
+      linear_y_joint:
+        goal: 0.05
+      rotational_yaw_joint:
+        goal: 0.05
+      shoulder_pan_joint:
+        goal: 0.05
+      shoulder_lift_joint:
+        goal: 0.05
+      elbow_joint:
+        goal: 0.05
+      wrist_1_joint:
+        goal: 0.05
+      wrist_2_joint:
+        goal: 0.05
+      wrist_3_joint:
+        goal: 0.05
+    acceleration_limits:
+      linear_x_joint: 10.0
+      linear_y_joint: 10.0
+      rotational_yaw_joint: 10.0
+      shoulder_pan_joint: 10.0
+      shoulder_lift_joint: 10.0
+      elbow_joint: 10.0
+      wrist_1_joint: 10.0
+      wrist_2_joint: 10.0
+      wrist_3_joint: 10.0
+
+force_torque_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: robotiq_ft_sensor
+    state_interface_names:
+      - force.x
+      - force.y
+      - force.z
+      - torque.x
+      - torque.y
+      - torque.z
+    frame_id: fts_link

src/picknik_ur_mobile_config/config/initial_positions.yaml

@@ -0,0 +1,9 @@
+linear_x_joint: 1.5
+linear_y_joint: 1.5
+rotational_yaw_joint: 0.0
+shoulder_pan_joint: 0.0
+shoulder_lift_joint: -1.41
+elbow_joint: -0.7
+wrist_1_joint: -2.12
+wrist_2_joint: 1.67
+wrist_3_joint: 0.0