diff --git a/.gitmodules b/.gitmodules index c0393fa4..1f7452a3 100644 --- a/.gitmodules +++ b/.gitmodules @@ -3,4 +3,4 @@ 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 = git@github.com:clearpathrobotics/clearpath_mecanum_drive_controller.git + url = https://github.com/clearpathrobotics/clearpath_mecanum_drive_controller.git diff --git a/src/moveit_pro_ur_configs/bring_it_sim/description/assets/cube.stl b/src/moveit_pro_ur_configs/bring_it_sim/description/assets/cube.stl deleted file mode 100644 index 44963edd..00000000 Binary files a/src/moveit_pro_ur_configs/bring_it_sim/description/assets/cube.stl and /dev/null differ diff --git a/src/moveit_pro_ur_configs/bring_it_sim/description/bar.xml b/src/moveit_pro_ur_configs/bring_it_sim/description/bar.xml index d646db6e..801cdcdb 100644 --- a/src/moveit_pro_ur_configs/bring_it_sim/description/bar.xml +++ b/src/moveit_pro_ur_configs/bring_it_sim/description/bar.xml @@ -1,33 +1,93 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/moveit_pro_ur_configs/bring_it_sim/description/bar_scene.xml b/src/moveit_pro_ur_configs/bring_it_sim/description/bar_scene.xml index 2d59adee..bcb234ef 100644 --- a/src/moveit_pro_ur_configs/bring_it_sim/description/bar_scene.xml +++ b/src/moveit_pro_ur_configs/bring_it_sim/description/bar_scene.xml @@ -1,26 +1,67 @@ - - - - + + + - - - + + + - - - + + + - - + + + + + diff --git a/src/moveit_pro_ur_configs/bring_it_sim/description/front_left_wheel_link.xml b/src/moveit_pro_ur_configs/bring_it_sim/description/front_left_wheel_link.xml index da2e9afd..f06fcf85 100644 --- a/src/moveit_pro_ur_configs/bring_it_sim/description/front_left_wheel_link.xml +++ b/src/moveit_pro_ur_configs/bring_it_sim/description/front_left_wheel_link.xml @@ -1,77 +1,357 @@ - - - + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + diff --git a/src/moveit_pro_ur_configs/bring_it_sim/description/front_right_wheel_link.xml b/src/moveit_pro_ur_configs/bring_it_sim/description/front_right_wheel_link.xml index b5c73ef1..38c6acce 100644 --- a/src/moveit_pro_ur_configs/bring_it_sim/description/front_right_wheel_link.xml +++ b/src/moveit_pro_ur_configs/bring_it_sim/description/front_right_wheel_link.xml @@ -1,77 +1,357 @@ - - - + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + diff --git a/src/moveit_pro_ur_configs/bring_it_sim/description/rear_left_wheel_link.xml b/src/moveit_pro_ur_configs/bring_it_sim/description/rear_left_wheel_link.xml index 129f3f0e..2484daab 100644 --- a/src/moveit_pro_ur_configs/bring_it_sim/description/rear_left_wheel_link.xml +++ b/src/moveit_pro_ur_configs/bring_it_sim/description/rear_left_wheel_link.xml @@ -1,77 +1,357 @@ - - - + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + diff --git a/src/moveit_pro_ur_configs/bring_it_sim/description/rear_right_wheel_link.xml b/src/moveit_pro_ur_configs/bring_it_sim/description/rear_right_wheel_link.xml index 32b03f9a..c044b53c 100644 --- a/src/moveit_pro_ur_configs/bring_it_sim/description/rear_right_wheel_link.xml +++ b/src/moveit_pro_ur_configs/bring_it_sim/description/rear_right_wheel_link.xml @@ -1,77 +1,357 @@ - - - + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + diff --git a/src/moveit_pro_ur_configs/bring_it_sim/description/ur5e_ridgeback.xacro b/src/moveit_pro_ur_configs/bring_it_sim/description/ur5e_ridgeback.xacro index 72d2f41c..fa9ecc77 100644 --- a/src/moveit_pro_ur_configs/bring_it_sim/description/ur5e_ridgeback.xacro +++ b/src/moveit_pro_ur_configs/bring_it_sim/description/ur5e_ridgeback.xacro @@ -63,23 +63,23 @@ - + - + - + - - + + @@ -87,7 +87,7 @@ - + diff --git a/src/moveit_pro_ur_configs/bring_it_sim/description/ur5e_ridgeback.xml b/src/moveit_pro_ur_configs/bring_it_sim/description/ur5e_ridgeback.xml index a46fe9d3..509b2d11 100644 --- a/src/moveit_pro_ur_configs/bring_it_sim/description/ur5e_ridgeback.xml +++ b/src/moveit_pro_ur_configs/bring_it_sim/description/ur5e_ridgeback.xml @@ -1,303 +1,759 @@ - + - diff --git a/src/moveit_pro_ur_configs/bring_it_sim/launch/agent_bridge.launch.xml b/src/moveit_pro_ur_configs/bring_it_sim/launch/agent_bridge.launch.xml new file mode 100644 index 00000000..b2aa7543 --- /dev/null +++ b/src/moveit_pro_ur_configs/bring_it_sim/launch/agent_bridge.launch.xml @@ -0,0 +1,6 @@ + + + + diff --git a/src/moveit_pro_ur_configs/bring_it_sim/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/moveit_pro_ur_configs/bring_it_sim/launch/sim/robot_drivers_to_persist_sim.launch.py index 7e852c92..533f9db2 100644 --- a/src/moveit_pro_ur_configs/bring_it_sim/launch/sim/robot_drivers_to_persist_sim.launch.py +++ b/src/moveit_pro_ur_configs/bring_it_sim/launch/sim/robot_drivers_to_persist_sim.launch.py @@ -273,10 +273,10 @@ def generate_launch_description(): # Publish odometry as joint state messages odom_to_joint_state_repub = Node( - package='bring_it_sim', - executable='odometry_joint_state_publisher.py', - name='odometry_joint_state_publisher', - output='log' + package="bring_it_sim", + executable="odometry_joint_state_publisher.py", + name="odometry_joint_state_publisher", + output="log", ) # Create the launch description and populate diff --git a/src/moveit_pro_ur_configs/bring_it_sim/script/odometry_joint_state_publisher.py b/src/moveit_pro_ur_configs/bring_it_sim/script/odometry_joint_state_publisher.py index ad5cb68f..bb0839c6 100755 --- a/src/moveit_pro_ur_configs/bring_it_sim/script/odometry_joint_state_publisher.py +++ b/src/moveit_pro_ur_configs/bring_it_sim/script/odometry_joint_state_publisher.py @@ -1,3 +1,31 @@ +# Copyright 2024 PickNik Inc. +# +# 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 PickNik Inc. 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. + #!/usr/bin/env python3 import rclpy from rclpy.node import Node @@ -5,9 +33,15 @@ from nav_msgs.msg import Odometry from sensor_msgs.msg import JointState -from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy +from rclpy.qos import ( + QoSProfile, + QoSReliabilityPolicy, + QoSDurabilityPolicy, + QoSHistoryPolicy, +) from math import atan2 + class OdometryJointStateRepublisher(Node): """! The Odometry to JointState republisher node. @@ -24,31 +58,35 @@ def __init__(self, odom_topic, joint_states_topic): @return Instance of the OdometryJointStateRepublisher node """ - super().__init__('odometry_joint_state_republisher') + super().__init__("odometry_joint_state_republisher") # Define a QoS profile qos_profile_sub = QoSProfile( reliability=QoSReliabilityPolicy.BEST_EFFORT, # Ensure reliability durability=QoSDurabilityPolicy.VOLATILE, # Non-persistent messages - history=QoSHistoryPolicy.KEEP_LAST, # Keep only the last few messages - depth=1 # Queue size + history=QoSHistoryPolicy.KEEP_LAST, # Keep only the last few messages + depth=1, # Queue size ) # Define a QoS profile qos_profile_pub = QoSProfile( reliability=QoSReliabilityPolicy.RELIABLE, # Ensure reliability durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, # Non-persistent messages - history=QoSHistoryPolicy.KEEP_LAST, # Keep only the last few messages - depth=1 # Queue size + history=QoSHistoryPolicy.KEEP_LAST, # Keep only the last few messages + depth=1, # Queue size ) - self.odom_sub_ = self.create_subscription(Odometry, odom_topic, self.odom_callback, qos_profile_sub) - self.joint_states_pub_ = self.create_publisher(JointState, joint_states_topic, qos_profile_pub) + self.odom_sub_ = self.create_subscription( + Odometry, odom_topic, self.odom_callback, qos_profile_sub + ) + self.joint_states_pub_ = self.create_publisher( + JointState, joint_states_topic, qos_profile_pub + ) def odom_callback(self, odom_msg): """! Subscription callback to run for incoming Odometry messages - Each Odometry message is copied into JointState messages and republished + Each Odometry message is copied into JointState messages and republished to /joint_states. """ @@ -56,32 +94,36 @@ def odom_callback(self, odom_msg): # Get yaw angle from quaternion orientation, # see https://stackoverflow.com/questions/5782658/extracting-yaw-from-a-quaternion q = odom_msg.pose.pose.orientation - rotation_yaw = atan2(2.0 * (q.z * q.w + q.x * q.y), -1.0 + 2.0 * (q.w * q.w + q.x * q.x)); + rotation_yaw = atan2( + 2.0 * (q.z * q.w + q.x * q.y), -1.0 + 2.0 * (q.w * q.w + q.x * q.x) + ) # Populate joint state message with x,y,yaw joint dimensions joint_state_msg = JointState() joint_state_msg.name = [ "linear_x_joint", "linear_y_joint", - "rotational_yaw_joint" + "rotational_yaw_joint", ] joint_state_msg.position = [ odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, - rotation_yaw + rotation_yaw, ] self.joint_states_pub_.publish(joint_state_msg) + def main(args=None): - rclpy.init(args=args) + rclpy.init(args=args) + + odometry_repub = OdometryJointStateRepublisher("/odom", "/joint_states") - odometry_repub = OdometryJointStateRepublisher('/odom', '/joint_states') + rclpy.spin(odometry_repub) - rclpy.spin(odometry_repub) + odometry_repub.destroy_node() - odometry_repub.destroy_node() + rclpy.shutdown() - rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/dual_arm_robot.xml b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/dual_arm_robot.xml index ccc2c71e..d0e1889d 100644 --- a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/dual_arm_robot.xml +++ b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/dual_arm_robot.xml @@ -1,12 +1,20 @@ - - diff --git a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/dual_arm_scene.xml b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/dual_arm_scene.xml index 902f43b5..de1bccea 100644 --- a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/dual_arm_scene.xml +++ b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/dual_arm_scene.xml @@ -1,45 +1,101 @@ - - - - + + + + + - - - + + + - - - + + + - - - - + + + + - - + + - - - + + - - + + - - diff --git a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/front_left_wheel_link.xml b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/front_left_wheel_link.xml index da2e9afd..f06fcf85 100644 --- a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/front_left_wheel_link.xml +++ b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/front_left_wheel_link.xml @@ -1,77 +1,357 @@ - - - + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + diff --git a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/front_right_wheel_link.xml b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/front_right_wheel_link.xml index b5c73ef1..38c6acce 100644 --- a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/front_right_wheel_link.xml +++ b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/front_right_wheel_link.xml @@ -1,77 +1,357 @@ - - - + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + diff --git a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/lunar_scene.xml b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/lunar_scene.xml index 6268a9a0..102fd73b 100644 --- a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/lunar_scene.xml +++ b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/lunar_scene.xml @@ -1,34 +1,82 @@ - - - + + - - - + + + - - + + - - - + + + - - - + + + - + diff --git a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/rear_left_wheel_link.xml b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/rear_left_wheel_link.xml index 129f3f0e..2484daab 100644 --- a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/rear_left_wheel_link.xml +++ b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/rear_left_wheel_link.xml @@ -1,77 +1,357 @@ - - - + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + diff --git a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/rear_right_wheel_link.xml b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/rear_right_wheel_link.xml index 32b03f9a..c044b53c 100644 --- a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/rear_right_wheel_link.xml +++ b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/rear_right_wheel_link.xml @@ -1,77 +1,357 @@ - - - + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + diff --git a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/scene.xml b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/scene.xml index 2b4517f6..1c2c0438 100644 --- a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/scene.xml +++ b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/scene.xml @@ -1,39 +1,108 @@ - - - + + - - - + + + - - - - - - - - + + + + + + + + - - - + + + - - + + - - + + diff --git a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/shelf.xml b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/shelf.xml index 7f5e98c4..c170bc1a 100644 --- a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/shelf.xml +++ b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/shelf.xml @@ -1,130 +1,475 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - + + + + + + diff --git a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/ur5e.xml b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/ur5e.xml index f40a5fb9..29baa526 100644 --- a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/ur5e.xml +++ b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/description/ur5e.xml @@ -1,303 +1,759 @@ - + - diff --git a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/launch/agent_bridge.launch.xml b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/launch/agent_bridge.launch.xml index 1729a3c8..70ee0b38 100644 --- a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/launch/agent_bridge.launch.xml +++ b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/launch/agent_bridge.launch.xml @@ -1,5 +1,9 @@ - + - - + + diff --git a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/objectives/take_snap.xml b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/objectives/take_snap.xml index 4963deee..095166aa 100644 --- a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/objectives/take_snap.xml +++ b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/objectives/take_snap.xml @@ -1,18 +1,18 @@ - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + diff --git a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/package.xml b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/package.xml index 9762c4c5..d8f4b10b 100644 --- a/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/package.xml +++ b/src/moveit_pro_ur_configs/ur5e_ridgeback_mujoco_config/package.xml @@ -1,9 +1,12 @@ - + ur5e_ridgeback_mujoco_config 0.0.1 - MoveIt Pro configuration MuJoCo simulation of Clearpath's Ridgeback with a UR5e arm + + MoveIt Pro configuration MuJoCo simulation of Clearpath's Ridgeback with a + UR5e arm + Paul Gesel