From 1a1084c627cb939859f35ff6f9a3d72e9c163157 Mon Sep 17 00:00:00 2001 From: thomason <84239247+thomasonzhou@users.noreply.github.com> Date: Tue, 22 Apr 2025 18:46:27 +0900 Subject: [PATCH] Add moveit2 package with dual arm/gripper (#5) * Create mujoco_ros2_control package * Enable Mujoco hardware interface for OpenArm * Verify connection with ros2_control * Update actuators to pos/vel * Add motor after creation * Tune KD for smoother motion * Restore realsense camera * Change camera to be 10 tilt from vertical to match physical model * Repair urdf to match physical robot * Checkpoint moveit2_configuration * Remove collision between camera and pedestal and remove additional ros2_control interfaces for mock * Enable octomap perception * Fix base_link to world transform * Update openarm to support moveit2 bimanual planning with end effectors * Improve safety of openarm_hardware zero position autocalibration * Remove openarm_mujoco_ros2_control from moveit2 branch * Restore sensor functionality --- .../urdf/openarm_bimanual.urdf.xacro | 4 +- .../urdf/openarm_bimanual_sensors.xacro | 2 +- .../.setup_assistant | 2 +- .../config/initial_positions.yaml | 2 + .../config/joint_limits.yaml | 10 +- .../config/kinematics.yaml | 16 ++ .../config/moveit_controllers.yaml | 14 ++ .../openarm_bimanual.ros2_control.xacro | 142 ++++++++++++++++++ .../config/openarm_bimanual.srdf | 100 +++++++++++- .../config/openarm_bimanual.urdf.xacro | 12 ++ .../config/ros2_controllers.yaml | 85 +++++++++++ .../config/sensors_3d.yaml | 14 +- .../config/controllers.yaml | 84 +++++------ openarm_description/urdf/openarm.xacro | 12 +- .../openarm_hardware/openarm_hardware.hpp | 6 +- openarm_hardware/src/openarm_hardware.cpp | 65 ++++---- 16 files changed, 471 insertions(+), 99 deletions(-) create mode 100644 openarm_bimanual_moveit_config/config/openarm_bimanual.ros2_control.xacro diff --git a/openarm_bimanual_description/urdf/openarm_bimanual.urdf.xacro b/openarm_bimanual_description/urdf/openarm_bimanual.urdf.xacro index ec84ce7..437dcdd 100644 --- a/openarm_bimanual_description/urdf/openarm_bimanual.urdf.xacro +++ b/openarm_bimanual_description/urdf/openarm_bimanual.urdf.xacro @@ -2,8 +2,8 @@ - - + + diff --git a/openarm_bimanual_description/urdf/openarm_bimanual_sensors.xacro b/openarm_bimanual_description/urdf/openarm_bimanual_sensors.xacro index 24d718e..804a699 100644 --- a/openarm_bimanual_description/urdf/openarm_bimanual_sensors.xacro +++ b/openarm_bimanual_description/urdf/openarm_bimanual_sensors.xacro @@ -2,7 +2,7 @@ - + diff --git a/openarm_bimanual_moveit_config/.setup_assistant b/openarm_bimanual_moveit_config/.setup_assistant index 2cfa29b..e43974b 100644 --- a/openarm_bimanual_moveit_config/.setup_assistant +++ b/openarm_bimanual_moveit_config/.setup_assistant @@ -7,4 +7,4 @@ moveit_setup_assistant_config: package_settings: author_name: Thomason Zhou author_email: t95zhou@uwaterloo.ca - generated_timestamp: 1744266716 \ No newline at end of file + generated_timestamp: 1745222265 \ No newline at end of file diff --git a/openarm_bimanual_moveit_config/config/initial_positions.yaml b/openarm_bimanual_moveit_config/config/initial_positions.yaml index 953e855..a3894f6 100644 --- a/openarm_bimanual_moveit_config/config/initial_positions.yaml +++ b/openarm_bimanual_moveit_config/config/initial_positions.yaml @@ -1,6 +1,7 @@ # Default initial positions for openarm_bimanual's ros2_control fake system initial_positions: + left_left_pris1: 0 left_rev1: 0 left_rev2: 0 left_rev3: 0 @@ -8,6 +9,7 @@ initial_positions: left_rev5: 0 left_rev6: 0 left_rev7: 0 + right_left_pris1: 0 right_rev1: 0 right_rev2: 0 right_rev3: 0 diff --git a/openarm_bimanual_moveit_config/config/joint_limits.yaml b/openarm_bimanual_moveit_config/config/joint_limits.yaml index 972282b..a2d9a60 100644 --- a/openarm_bimanual_moveit_config/config/joint_limits.yaml +++ b/openarm_bimanual_moveit_config/config/joint_limits.yaml @@ -9,8 +9,8 @@ default_acceleration_scaling_factor: 0.1 # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: left_left_pris1: - has_velocity_limits: false - max_velocity: 0 + has_velocity_limits: true + max_velocity: 0.10000000000000001 has_acceleration_limits: false max_acceleration: 0 left_rev1: @@ -48,9 +48,9 @@ joint_limits: max_velocity: 0 has_acceleration_limits: false max_acceleration: 0 - left_right_pris2: - has_velocity_limits: false - max_velocity: 0 + right_left_pris1: + has_velocity_limits: true + max_velocity: 0.10000000000000001 has_acceleration_limits: false max_acceleration: 0 right_rev1: diff --git a/openarm_bimanual_moveit_config/config/kinematics.yaml b/openarm_bimanual_moveit_config/config/kinematics.yaml index 6f91597..8ad5571 100644 --- a/openarm_bimanual_moveit_config/config/kinematics.yaml +++ b/openarm_bimanual_moveit_config/config/kinematics.yaml @@ -1,4 +1,20 @@ left_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 +left_side: + kinematics_solver: cached_ik_kinematics_plugin/CachedKDLKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 +right_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 +right_side: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 +upper_body: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.0050000000000000001 kinematics_solver_timeout: 0.0050000000000000001 \ No newline at end of file diff --git a/openarm_bimanual_moveit_config/config/moveit_controllers.yaml b/openarm_bimanual_moveit_config/config/moveit_controllers.yaml index 3584579..b8a1de0 100644 --- a/openarm_bimanual_moveit_config/config/moveit_controllers.yaml +++ b/openarm_bimanual_moveit_config/config/moveit_controllers.yaml @@ -6,6 +6,8 @@ moveit_simple_controller_manager: controller_names: - left_arm_controller - right_arm_controller + - left_gripper_controller + - right_gripper_controller left_arm_controller: type: FollowJointTrajectory @@ -30,4 +32,16 @@ moveit_simple_controller_manager: - right_rev6 - right_rev7 action_ns: follow_joint_trajectory + default: true + left_gripper_controller: + type: GripperCommand + joints: + - left_left_pris1 + action_ns: gripper_cmd + default: true + right_gripper_controller: + type: GripperCommand + joints: + - right_left_pris1 + action_ns: gripper_cmd default: true \ No newline at end of file diff --git a/openarm_bimanual_moveit_config/config/openarm_bimanual.ros2_control.xacro b/openarm_bimanual_moveit_config/config/openarm_bimanual.ros2_control.xacro new file mode 100644 index 0000000..7bb6240 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/openarm_bimanual.ros2_control.xacro @@ -0,0 +1,142 @@ + + + + + + + + + mock_components/GenericSystem + + + + + + ${initial_positions['left_rev1']} + + + + + + + + ${initial_positions['left_rev2']} + + + + + + + + ${initial_positions['left_rev3']} + + + + + + + + ${initial_positions['left_rev4']} + + + + + + + + ${initial_positions['left_rev5']} + + + + + + + + ${initial_positions['left_rev6']} + + + + + + + + ${initial_positions['left_rev7']} + + + + + + + + ${initial_positions['right_rev1']} + + + + + + + + ${initial_positions['right_rev2']} + + + + + + + + ${initial_positions['right_rev3']} + + + + + + + + ${initial_positions['right_rev4']} + + + + + + + + ${initial_positions['right_rev5']} + + + + + + + + ${initial_positions['right_rev6']} + + + + + + + + ${initial_positions['right_rev7']} + + + + + + + + ${initial_positions['left_left_pris1']} + + + + + + + + ${initial_positions['right_left_pris1']} + + + + + + + diff --git a/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf b/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf index 32cec20..0b32430 100644 --- a/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf +++ b/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf @@ -18,6 +18,7 @@ + @@ -25,6 +26,7 @@ + @@ -36,6 +38,7 @@ + @@ -43,20 +46,109 @@ + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - + + + + + + diff --git a/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro b/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro index 0fbc43d..2f014e8 100644 --- a/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro +++ b/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro @@ -5,4 +5,16 @@ + + + + + + + + + diff --git a/openarm_bimanual_moveit_config/config/ros2_controllers.yaml b/openarm_bimanual_moveit_config/config/ros2_controllers.yaml index 0461344..7007b61 100644 --- a/openarm_bimanual_moveit_config/config/ros2_controllers.yaml +++ b/openarm_bimanual_moveit_config/config/ros2_controllers.yaml @@ -11,6 +11,26 @@ controller_manager: type: joint_trajectory_controller/JointTrajectoryController + left_gripper_controller: + type: position_controllers/GripperActionController + + + right_gripper_controller: + type: position_controllers/GripperActionController + + + left_side_controller: + type: joint_trajectory_controller/JointTrajectoryController + + + right_side_controller: + type: joint_trajectory_controller/JointTrajectoryController + + + upper_body_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster @@ -43,6 +63,71 @@ right_arm_controller: command_interfaces: - position - velocity + state_interfaces: + - position + - velocity +left_gripper_controller: + ros__parameters: + joint: left_left_pris1 +right_gripper_controller: + ros__parameters: + joint: right_left_pris1 +left_side_controller: + ros__parameters: + joints: + - left_rev1 + - left_rev2 + - left_rev3 + - left_rev4 + - left_rev5 + - left_rev6 + - left_rev7 + - left_left_pris1 + command_interfaces: + - position + - velocity + state_interfaces: + - position + - velocity +right_side_controller: + ros__parameters: + joints: + - right_rev1 + - right_rev2 + - right_rev3 + - right_rev4 + - right_rev5 + - right_rev6 + - right_rev7 + - right_left_pris1 + command_interfaces: + - position + - velocity + state_interfaces: + - position + - velocity +upper_body_controller: + ros__parameters: + joints: + - left_rev1 + - left_rev2 + - left_rev3 + - left_rev4 + - left_rev5 + - left_rev6 + - left_rev7 + - left_left_pris1 + - right_rev1 + - right_rev2 + - right_rev3 + - right_rev4 + - right_rev5 + - right_rev6 + - right_rev7 + - right_left_pris1 + command_interfaces: + - position + - velocity state_interfaces: - position - velocity \ No newline at end of file diff --git a/openarm_bimanual_moveit_config/config/sensors_3d.yaml b/openarm_bimanual_moveit_config/config/sensors_3d.yaml index 99dcaeb..d2fe2ab 100644 --- a/openarm_bimanual_moveit_config/config/sensors_3d.yaml +++ b/openarm_bimanual_moveit_config/config/sensors_3d.yaml @@ -2,16 +2,14 @@ sensors: - default_sensor - kinect_depthimage default_sensor: - far_clipping_plane_distance: 5.0 filtered_cloud_topic: filtered_cloud - image_topic: /head_mount_kinect/depth_registered/image_raw + max_range: 5.0 max_update_rate: 1.0 - near_clipping_plane_distance: 0.3 - padding_offset: 0.03 - padding_scale: 4.0 - queue_size: 5 - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater - shadow_threshold: 0.2 + padding_offset: 0.1 + padding_scale: 1.0 + point_cloud_topic: /camera/camera/depth/color/points + point_subsample: 1 + sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater kinect_depthimage: far_clipping_plane_distance: 5.0 filtered_cloud_topic: filtered_cloud diff --git a/openarm_bimanual_teleop/config/controllers.yaml b/openarm_bimanual_teleop/config/controllers.yaml index 4d85899..4750945 100644 --- a/openarm_bimanual_teleop/config/controllers.yaml +++ b/openarm_bimanual_teleop/config/controllers.yaml @@ -2,49 +2,47 @@ controller_manager: ros__parameters: update_rate: 100 # Hz - left_arm_controller: - type: joint_trajectory_controller/JointTrajectoryController - - - right_arm_controller: - type: joint_trajectory_controller/JointTrajectoryController - joint_state_broad: type: joint_state_broadcaster/JointStateBroadcaster -left_arm_controller: - ros__parameters: - joints: - - left_rev1 - - left_rev2 - - left_rev3 - - left_rev4 - - left_rev5 - - left_rev6 - - left_rev7 - command_interfaces: - - position - - velocity - - effort - state_interfaces: - - position - - velocity - - effort -right_arm_controller: - ros__parameters: - joints: - - right_rev1 - - right_rev2 - - right_rev3 - - right_rev4 - - right_rev5 - - right_rev6 - - right_rev7 - command_interfaces: - - position - - velocity - - effort - state_interfaces: - - position - - velocity - - effort \ No newline at end of file +# left_arm_controller: +# type: joint_trajectory_controller/JointTrajectoryController + +# right_arm_controller: +# type: joint_trajectory_controller/JointTrajectoryController + + +# left_arm_controller: +# ros__parameters: +# joints: +# - left_rev1 +# - left_rev2 +# - left_rev3 +# - left_rev4 +# - left_rev5 +# - left_rev6 +# - left_rev7 +# command_interfaces: +# - position +# - velocity +# state_interfaces: +# - position +# - velocity +# - effort +# right_arm_controller: +# ros__parameters: +# joints: +# - right_rev1 +# - right_rev2 +# - right_rev3 +# - right_rev4 +# - right_rev5 +# - right_rev6 +# - right_rev7 +# command_interfaces: +# - position +# - velocity +# state_interfaces: +# - position +# - velocity +# - effort \ No newline at end of file diff --git a/openarm_description/urdf/openarm.xacro b/openarm_description/urdf/openarm.xacro index 0ba2eb6..492cb1e 100644 --- a/openarm_description/urdf/openarm.xacro +++ b/openarm_description/urdf/openarm.xacro @@ -240,15 +240,15 @@ - - + + - + @@ -282,14 +282,14 @@ - - + + - + diff --git a/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp b/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp index 75e6313..7a05571 100644 --- a/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp +++ b/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp @@ -43,13 +43,13 @@ static const std::size_t ARM_DOF = 7; static const std::size_t GRIPPER_DOF = 1; static const std::size_t TOTAL_DOF = ARM_DOF + GRIPPER_DOF; static const std::array KP = {80.0, 80.0, 20.0, 55.0, 5.0, 5.0, 5.0, 0.5}; -static const std::array KD = {1.25, 0.17, 0.015, 0.07, 0.07, 0.05, 0.05, 0.01}; -static const std::array SLOW_KP = {10.0, 10.0, 10.0, 7.5, 5.0, 5.0, 5.0, 0.5}; +static const std::array KD = {2.75, 2.5, 0.7, 0.4, 0.7, 0.6, 0.5, 0.1}; static const double START_POS_TOLERANCE_RAD = 0.1; -static const double POS_JUMP_TOLERANCE_RAD = 3.1415 / 2.0; +static const double POS_JUMP_TOLERANCE_RAD = 3.1415 / 16.0; static const bool USING_GRIPPER = true; static const double GRIPPER_REFERENCE_GEAR_RADIUS_M = 0.00853; +static const double GRIPPER_GEAR_DIRECTION_MULTIPLIER = -1.0; static const int GRIPPER_INDEX = TOTAL_DOF - 1; class OpenArmHW : public hardware_interface::SystemInterface diff --git a/openarm_hardware/src/openarm_hardware.cpp b/openarm_hardware/src/openarm_hardware.cpp index c1b27a7..42b38d3 100644 --- a/openarm_hardware/src/openarm_hardware.cpp +++ b/openarm_hardware/src/openarm_hardware.cpp @@ -71,9 +71,7 @@ hardware_interface::CallbackReturn OpenArmHW::on_init( motors_.resize(curr_dof); for(size_t i = 0; i < curr_dof; ++i){ motors_[i] = std::make_unique(motor_types[i], can_device_ids[i], can_master_ids[i]); - } - for(const auto& motor: motors_){ - motor_control_->addMotor(*motor); + motor_control_->addMotor(*motors_[i]); } pos_states_.resize(curr_dof, 0.0); @@ -141,27 +139,39 @@ hardware_interface::CallbackReturn OpenArmHW::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { read(rclcpp::Time(0), rclcpp::Duration(0, 0)); - - // for (std::size_t m = 0; m < curr_dof; ++m){ - // double diff = pos_states_[m] - pos_commands_[m]; - // while(abs(diff) > START_POS_TOLERANCE_RAD){ - // // linear interpolation - // // take the min of max_step and the difference - - // double max_step = std::min(POS_JUMP_TOLERANCE_RAD, std::abs(diff)); - // if (diff > 0){ - // pos_commands_[m] = pos_states_[m] - max_step; - // } - // else{ - // pos_commands_[m] = pos_states_[m] + max_step; - // } - // motor_control_->controlMIT(*motors_[m], SLOW_KP[m], KD[m], pos_commands_[m], 0.0, 0.0); - // } - // } - refresh_motors(); + bool zeroed = false; for(const auto& motor: motors_){ motor_control_->enable(*motor); } + + while(!zeroed){ + bool all_zero = true; + for (std::size_t m = 0; m < curr_dof; ++m){ + const double diff = pos_commands_[m] - pos_states_[m]; + if (std::abs(diff) > START_POS_TOLERANCE_RAD){ + all_zero = false; + } + + const double max_step = std::min(POS_JUMP_TOLERANCE_RAD, std::abs(diff)); + double command = pos_states_[m]; + if (pos_states_[m] < pos_commands_[m]){ + command += max_step; + } + else{ + command -= max_step; + } + motor_control_->controlMIT(*motors_[m], KP[m], KD[m], command, 0.0, 0.0); + + } + if (all_zero){ + zeroed = true; + } + else{ + sleep(0.01); + read(rclcpp::Time(0), rclcpp::Duration(0, 0)); + } + } + read(rclcpp::Time(0), rclcpp::Duration(0, 0)); return CallbackReturn::SUCCESS; @@ -187,9 +197,9 @@ hardware_interface::return_type OpenArmHW::read( tau_states_[i] = motors_[i]->getTorque(); } if(USING_GRIPPER){ - pos_states_[GRIPPER_INDEX] = -motors_[GRIPPER_INDEX]->getPosition() * GRIPPER_REFERENCE_GEAR_RADIUS_M; - vel_states_[GRIPPER_INDEX] = motors_[GRIPPER_INDEX]->getVelocity() * GRIPPER_REFERENCE_GEAR_RADIUS_M; - tau_states_[GRIPPER_INDEX] = motors_[GRIPPER_INDEX]->getTorque() * GRIPPER_REFERENCE_GEAR_RADIUS_M; + pos_states_[GRIPPER_INDEX] = -motors_[GRIPPER_INDEX]->getPosition() * GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER; + vel_states_[GRIPPER_INDEX] = motors_[GRIPPER_INDEX]->getVelocity() * GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER; + tau_states_[GRIPPER_INDEX] = motors_[GRIPPER_INDEX]->getTorque() * GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER; } @@ -201,7 +211,7 @@ hardware_interface::return_type OpenArmHW::write( { if (disable_torque_){ // refresh motor state on write - for(size_t i = 0; i < TOTAL_DOF; ++i){ + for(size_t i = 0; i < curr_dof; ++i){ motor_control_->controlMIT(*motors_[i], 0.0, 0.0, 0.0, 0.0, 0.0); return hardware_interface::return_type::OK; } @@ -217,7 +227,10 @@ hardware_interface::return_type OpenArmHW::write( motor_control_->controlMIT(*motors_[i], KP.at(i), KD.at(i), pos_commands_[i], vel_commands_[i], tau_ff_commands_[i]); } if(USING_GRIPPER){ - motor_control_->controlMIT(*motors_[GRIPPER_INDEX], KP.at(GRIPPER_INDEX), KD.at(GRIPPER_INDEX), -pos_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M, vel_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M, tau_ff_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M); + motor_control_->controlMIT(*motors_[GRIPPER_INDEX], KP.at(GRIPPER_INDEX), KD.at(GRIPPER_INDEX), + -pos_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER, + vel_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER, + tau_ff_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER); } return hardware_interface::return_type::OK; }