Bring it

.gitmodules

@@ -1,3 +1,6 @@
 [submodule "src/picknik_accessories"]
 	path = src/picknik_accessories
 	url = https://github.com/PickNikRobotics/picknik_accessories.git
+[submodule "src/external_dependencies/ridgeback/clearpath_mecanum_drive_controller"]
+	path = src/external_dependencies/ridgeback/clearpath_mecanum_drive_controller
+	url = https://github.com/clearpathrobotics/clearpath_mecanum_drive_controller.git

src/moveit_pro_ur_configs/bring_it_sim/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.22)
+project(bring_it_sim)
+
+find_package(ament_cmake REQUIRED)
+find_package(picknik_accessories REQUIRED)
+
+install(
+  DIRECTORY
+    config
+    description
+    launch
+    maps
+    objectives
+    params
+    waypoints
+  DESTINATION
+    share/${PROJECT_NAME}
+)
+
+install(PROGRAMS
+  script/odometry_joint_state_publisher.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/moveit_pro_ur_configs/bring_it_sim/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/moveit_pro_ur_configs/bring_it_sim/README.md

@@ -0,0 +1,5 @@
+# bring_it_sim
+
+A MoveIt Pro MuJoCo simulation for PickNik's Universal Robots (UR) arms.
+
+For detailed documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/)

src/moveit_pro_ur_configs/bring_it_sim/config/config.yaml

@@ -0,0 +1,88 @@
+#
+#  This contains information for a unique instance of a robot.
+#
+
+# Name of the package to specialize
+based_on_package: "ur5e_ridgeback_mujoco_config"
+hardware:
+  # Parameters used to configure the robot description through XACRO.
+  # A URDF and SRDF are both required.
+  # [Required]
+  robot_description:
+    urdf:
+      package: "bring_it_sim"
+      path: "description/ur5e_ridgeback.xacro"
+    srdf:
+      package: "bring_it_sim"
+      path: "config/moveit/picknik_ur.srdf"
+    urdf_params:
+      - joint_limits_parameters_file:
+          package: "ur5e_ridgeback_mujoco_config"
+          path: "config/moveit/joint_limits.yaml"
+      - mujoco_model: "description/bar_scene.xml"
+
+  # Specify any additional launch files for running the robot in simulation mode.
+  # Used when hardware.simulated is True.
+  # [Optional, defaults to a blank launch file if not specified]
+  simulated_robot_driver_persist_launch_file:
+    package: "bring_it_sim"
+    path: "launch/sim/robot_drivers_to_persist_sim.launch.py"
+
+moveit_params:
+  servo:
+    package: "bring_it_sim"
+    path: "config/moveit/ur_servo.yaml"
+  joint_limits:
+    package: "bring_it_sim"
+    path: "config/moveit/joint_limits.yaml"
+  servo_joint_limits:
+    package: "bring_it_sim"
+    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::NavBehaviorsLoader"
+      - "moveit_studio::behaviors::ServoBehaviorsLoader"
+      - "moveit_studio::behaviors::VisionBehaviorsLoader"
+  # Specify source folder for objectives
+  # [Required]
+  objective_library_paths:
+    bring_it_sim_objectives:
+      package_name: "bring_it_sim"
+      relative_path: "objectives"
+
+  # Specify the location of the saved waypoints file.
+  # [Required]
+  waypoints_file:
+    package_name: "bring_it_sim"
+    relative_path: "waypoints/ur_waypoints.yaml"
+
+# Configuration for launching ros2_control processes.
+# [Required, if using ros2_control]
+ros2_control:
+  config:
+    package: "bring_it_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
+  # for running the application.
+  # [Optional, default=[]]
+  controllers_active_at_startup:
+    - "force_torque_sensor_broadcaster"
+    - "robotiq_gripper_controller"
+    - "joint_state_broadcaster"
+    - "servo_controller"
+    - "platform_velocity_controller"
+  # Load but do not start these controllers so they can be activated later if needed.
+  controllers_inactive_at_startup:
+    - "joint_trajectory_controller"

src/moveit_pro_ur_configs/bring_it_sim/config/control/picknik_ur.ros2_control.yaml

@@ -0,0 +1,210 @@
+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
+    platform_velocity_controller:
+      type: clearpath_mecanum_drive_controller/MecanumDriveController
+
+platform_velocity_controller:
+  ros__parameters:
+    use_sim_time: False
+    interface_name: velocity
+
+    command_joint_names: ["front_left_wheel", "rear_left_wheel", "rear_right_wheel", "front_right_wheel"]
+
+    front_left_wheel_command_joint_name: "front_left_wheel"
+    front_right_wheel_command_joint_name: "front_right_wheel"
+    rear_right_wheel_command_joint_name: "rear_right_wheel"
+    rear_left_wheel_command_joint_name: "rear_left_wheel"
+
+    kinematics.base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 }
+    kinematics.wheels_radius: 0.05
+    kinematics.sum_of_robot_center_projection_on_X_Y_axis: 0.4704
+    wheel_separation_multiplier: 1.0
+    wheel_radius_multiplier: 1.0
+
+    publish_rate: 50.0
+    reference_timeout: 0.1
+    use_stamped_vel: False
+
+    pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03]
+    twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03]
+
+    cmd_vel_timeout: 0.5
+
+    base_frame_id: "ridgeback_base_link"
+    odom_frame_id: "odom"
+    enable_odom_tf: true
+
+    # Velocity and acceleration limits
+    # Whenever a min_* is unspecified, default to -max_*
+    linear.x.has_velocity_limits: true
+    linear.x.has_acceleration_limits: true
+    linear.x.has_jerk_limits: false
+    linear.x.max_velocity: 1.3
+    linear.x.min_velocity: -1.3
+    linear.x.max_acceleration: 1.0
+    linear.x.min_acceleration: -1.0
+    linear.x.max_jerk: 0.0
+    linear.x.min_jerk: 0.0
+
+    linear.y.has_velocity_limits: true
+    linear.y.has_acceleration_limits: true
+    linear.y.has_jerk_limits: false
+    linear.y.max_velocity: 1.3
+    linear.y.min_velocity: -1.3
+    linear.y.max_acceleration: 1.0
+    linear.y.min_acceleration: -1.0
+    linear.y.max_jerk: 0.0
+    linear.y.min_jerk: 0.0
+
+    angular.z.has_velocity_limits: true
+    angular.z.has_acceleration_limits: true
+    angular.z.has_jerk_limits: false
+    angular.z.max_velocity: 4.0
+    angular.z.min_velocity: -4.0
+    angular.z.max_acceleration: 2.0
+    angular.z.min_acceleration: -2.0
+    angular.z.max_jerk: 0.0
+    angular.z.min_jerk: 0.0
+
+joint_state_broadcaster:
+  ros__parameters:
+    use_local_topics: false
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+      - robotiq_85_left_knuckle_joint
+      - front_rocker
+      - front_left_wheel
+      - front_right_wheel
+      - rear_left_wheel
+      - rear_right_wheel
+    interfaces:
+      - position
+      - velocity
+
+joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - 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:
+      - 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
+      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:
+      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:
+      - 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:
+      - 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
+      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:
+      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: robotiq_ft_sensor