From 0bf3b6fa287e60c8ff87636ac4a0f453d6d8ba6f Mon Sep 17 00:00:00 2001 From: Nick Walker Date: Fri, 17 Sep 2021 11:05:36 -0700 Subject: [PATCH 1/5] Improve joint simulation Add gazebo_ros_control PID gains Tuned manually to make movement via the teleop interface behave well Wheels still unspecified because they cause the robot to collapse Use effort joint interface for the gripper so that gripper contacts are well simulated --- .../config/gazebo_ros_control_params.yaml | 14 ++++++++++++++ stretch_gazebo/config/gripper.yaml | 6 +++++- stretch_gazebo/launch/gazebo.launch | 3 +++ stretch_gazebo/urdf/stretch_gazebo.urdf.xacro | 4 ++-- 4 files changed, 24 insertions(+), 3 deletions(-) create mode 100644 stretch_gazebo/config/gazebo_ros_control_params.yaml diff --git a/stretch_gazebo/config/gazebo_ros_control_params.yaml b/stretch_gazebo/config/gazebo_ros_control_params.yaml new file mode 100644 index 00000000..ca2d722b --- /dev/null +++ b/stretch_gazebo/config/gazebo_ros_control_params.yaml @@ -0,0 +1,14 @@ +gazebo_ros_control/pid_gains: + #joint_right_wheel: {p: 10000, i: 0.0, d: 0.4} + #joint_left_wheel: {p: 10000, i: 0.0, d: 0.4} + joint_lift: {p: 5000, i: 0.0, d: 200} + joint_arm_l0: {p: 100, i: 0.0, d: 30} + joint_arm_l1: {p: 100, i: 0.0, d: 30} + joint_arm_l2: {p: 100, i: 0.0, d: 30} + joint_arm_l3: {p: 100, i: 0.0, d: 30} + joint_wrist_yaw: {p: 20, i: 5.0, d: 0.4} + joint_head_pan: {p: 2, i: 0.0, d: 0.4} + joint_head_tilt: {p: 2, i: 0.0, d: 0.4} + joint_gripper_finger_left: {p: 0.5, i: 0.0, d: 0.4} + joint_gripper_finger_right: {p: 0.5, i: 0.0, d: 0.4} + diff --git a/stretch_gazebo/config/gripper.yaml b/stretch_gazebo/config/gripper.yaml index 9e2442c1..2a82301e 100644 --- a/stretch_gazebo/config/gripper.yaml +++ b/stretch_gazebo/config/gripper.yaml @@ -1,5 +1,5 @@ stretch_gripper_controller: - type: "position_controllers/JointTrajectoryController" + type: "effort_controllers/JointTrajectoryController" joints: - joint_gripper_finger_right - joint_gripper_finger_left @@ -13,3 +13,7 @@ stretch_gripper_controller: stop_trajectory_duration: 0.5 state_publish_rate: 25 action_monitor_rate: 10 + + gains: + joint_gripper_finger_right: {p: 100, d: 1, i: 0, i_clamp: 1} + joint_gripper_finger_left: {p: 100, d: 1, i: 0, i_clamp: 1} diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch index 6b75bcd5..4725dc57 100644 --- a/stretch_gazebo/launch/gazebo.launch +++ b/stretch_gazebo/launch/gazebo.launch @@ -33,6 +33,9 @@ + + diff --git a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro index 1e516a35..a3db11b7 100644 --- a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro +++ b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro @@ -585,7 +585,7 @@ 1 - hardware_interface/PositionJointInterface + hardware_interface/EffortJointInterface @@ -594,7 +594,7 @@ 1 - hardware_interface/PositionJointInterface + hardware_interface/EffortJointInterface From cfc40d0a0adb7dc3bfd50483a399044129004622 Mon Sep 17 00:00:00 2001 From: Nick Walker Date: Sun, 31 Oct 2021 18:28:46 -0700 Subject: [PATCH 2/5] Use effort control for all arm joints Removes most of the instability in simulation behavior when lifting objects --- stretch_gazebo/config/arm.yaml | 10 +++++++++- stretch_gazebo/urdf/stretch_gazebo.urdf.xacro | 12 ++++++------ 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/stretch_gazebo/config/arm.yaml b/stretch_gazebo/config/arm.yaml index 1e179fc5..2bda06e7 100644 --- a/stretch_gazebo/config/arm.yaml +++ b/stretch_gazebo/config/arm.yaml @@ -1,5 +1,5 @@ stretch_arm_controller: - type: "position_controllers/JointTrajectoryController" + type: "effort_controllers/JointTrajectoryController" joints: - joint_lift - joint_arm_l3 @@ -21,3 +21,11 @@ stretch_arm_controller: stop_trajectory_duration: 0.5 state_publish_rate: 25 action_monitor_rate: 10 + + gains: + joint_lift: { p: 500, d: 100, i: 0, i_clamp: 1 } + joint_arm_l3: { p: 100, d: 30, i: 0, i_clamp: 1 } + joint_arm_l2: { p: 100, d: 30, i: 0, i_clamp: 1 } + joint_arm_l1: { p: 100, d: 30, i: 0, i_clamp: 1 } + joint_arm_l0: { p: 100, d: 30, i: 0, i_clamp: 1 } + joint_wrist_yaw: { p: 100, d: 1, i: 0, i_clamp: 1 } diff --git a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro index a3db11b7..739ca868 100644 --- a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro +++ b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro @@ -372,7 +372,7 @@ 1 - hardware_interface/PositionJointInterface + hardware_interface/EffortJointInterface @@ -428,7 +428,7 @@ 1 - hardware_interface/PositionJointInterface + hardware_interface/EffortJointInterface @@ -437,7 +437,7 @@ 1 - hardware_interface/PositionJointInterface + hardware_interface/EffortJointInterface @@ -446,7 +446,7 @@ 1 - hardware_interface/PositionJointInterface + hardware_interface/EffortJointInterface @@ -455,7 +455,7 @@ 1 - hardware_interface/PositionJointInterface + hardware_interface/EffortJointInterface @@ -475,7 +475,7 @@ 1 - hardware_interface/PositionJointInterface + hardware_interface/EffortJointInterface From 2ef81b31299647e534d39af9d537696558ebc742 Mon Sep 17 00:00:00 2001 From: Nick Walker Date: Sun, 31 Oct 2021 18:41:55 -0700 Subject: [PATCH 3/5] Increase mass of base Makes tipping behavior in simulation more plausible I assume that the mass of an assembled base is at least this heavy, given that the robot is around 50lbs total --- stretch_description/urdf/stretch_main.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stretch_description/urdf/stretch_main.xacro b/stretch_description/urdf/stretch_main.xacro index 6309cde4..dec1cf1a 100644 --- a/stretch_description/urdf/stretch_main.xacro +++ b/stretch_description/urdf/stretch_main.xacro @@ -13,7 +13,7 @@ xyz="-0.109461304328163 -0.000741018909047708 0.0914915269429946" rpy="0 0 0" /> + value="6.0" /> Date: Sun, 31 Oct 2021 18:56:25 -0700 Subject: [PATCH 4/5] Use a single controller for the simulation This better matches the physical robot, though this interface doesn't handle the virtual joints that the robot uses --- stretch_gazebo/config/arm.yaml | 31 ---------------- stretch_gazebo/config/body.yaml | 35 +++++++++++++++++++ stretch_gazebo/config/gripper.yaml | 19 ---------- stretch_gazebo/config/head.yaml | 15 -------- stretch_gazebo/launch/gazebo.launch | 10 ++---- stretch_gazebo/urdf/stretch_gazebo.urdf.xacro | 4 +-- 6 files changed, 39 insertions(+), 75 deletions(-) delete mode 100644 stretch_gazebo/config/arm.yaml create mode 100644 stretch_gazebo/config/body.yaml delete mode 100644 stretch_gazebo/config/gripper.yaml delete mode 100644 stretch_gazebo/config/head.yaml diff --git a/stretch_gazebo/config/arm.yaml b/stretch_gazebo/config/arm.yaml deleted file mode 100644 index 2bda06e7..00000000 --- a/stretch_gazebo/config/arm.yaml +++ /dev/null @@ -1,31 +0,0 @@ -stretch_arm_controller: - type: "effort_controllers/JointTrajectoryController" - joints: - - joint_lift - - joint_arm_l3 - - joint_arm_l2 - - joint_arm_l1 - - joint_arm_l0 - - joint_wrist_yaw - allow_partial_joints_goal: true - - constraints: - goal_time: 0.6 - stopped_velocity_tolerance: 0.05 - joint_lift: {trajectory: 0.1, goal: 0.1} - joint_arm_l3: {trajectory: 0.1, goal: 0.1} - joint_arm_l2: {trajectory: 0.1, goal: 0.1} - joint_arm_l1: {trajectory: 0.1, goal: 0.1} - joint_arm_l0: {trajectory: 0.1, goal: 0.1} - joint_wrist_yaw: {trajectory: 0.1, goal: 0.1} - stop_trajectory_duration: 0.5 - state_publish_rate: 25 - action_monitor_rate: 10 - - gains: - joint_lift: { p: 500, d: 100, i: 0, i_clamp: 1 } - joint_arm_l3: { p: 100, d: 30, i: 0, i_clamp: 1 } - joint_arm_l2: { p: 100, d: 30, i: 0, i_clamp: 1 } - joint_arm_l1: { p: 100, d: 30, i: 0, i_clamp: 1 } - joint_arm_l0: { p: 100, d: 30, i: 0, i_clamp: 1 } - joint_wrist_yaw: { p: 100, d: 1, i: 0, i_clamp: 1 } diff --git a/stretch_gazebo/config/body.yaml b/stretch_gazebo/config/body.yaml new file mode 100644 index 00000000..323124c1 --- /dev/null +++ b/stretch_gazebo/config/body.yaml @@ -0,0 +1,35 @@ +stretch_controller_raw: + type: "effort_controllers/JointTrajectoryController" + joints: + - joint_head_pan + - joint_head_tilt + - joint_lift + - joint_arm_l3 + - joint_arm_l2 + - joint_arm_l1 + - joint_arm_l0 + - joint_wrist_yaw + - joint_gripper_finger_right + - joint_gripper_finger_left + allow_partial_joints_goal: true + + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + + stop_trajectory_duration: 0.5 + state_publish_rate: 25 + action_monitor_rate: 10 + + + gains: + joint_head_pan: {p: 10, d: 1, i: 0, i_clamp: 1} + joint_head_tilt: {p: 10, d: 1, i: 0, i_clamp: 1} + joint_lift: {p: 500, d: 100, i: 0, i_clamp: 1} + joint_arm_l3: {p: 100, d: 30, i: 0, i_clamp: 1} + joint_arm_l2: {p: 100, d: 30, i: 0, i_clamp: 1} + joint_arm_l1: {p: 100, d: 30, i: 0, i_clamp: 1} + joint_arm_l0: {p: 100, d: 30, i: 0, i_clamp: 1} + joint_wrist_yaw: {p: 100, d: 1, i: 0, i_clamp: 1} + joint_gripper_finger_right: {p: 100, d: 1, i: 0, i_clamp: 1} + joint_gripper_finger_left: {p: 100, d: 1, i: 0, i_clamp: 1} \ No newline at end of file diff --git a/stretch_gazebo/config/gripper.yaml b/stretch_gazebo/config/gripper.yaml deleted file mode 100644 index 2a82301e..00000000 --- a/stretch_gazebo/config/gripper.yaml +++ /dev/null @@ -1,19 +0,0 @@ -stretch_gripper_controller: - type: "effort_controllers/JointTrajectoryController" - joints: - - joint_gripper_finger_right - - joint_gripper_finger_left - allow_partial_joints_goal: true - - constraints: - goal_time: 0.6 - stopped_velocity_tolerance: 0.05 - joint_gripper_finger_right: {trajectory: 0.1, goal: 0.1} - joint_gripper_finger_left: {trajectory: 0.1, goal: 0.1} - stop_trajectory_duration: 0.5 - state_publish_rate: 25 - action_monitor_rate: 10 - - gains: - joint_gripper_finger_right: {p: 100, d: 1, i: 0, i_clamp: 1} - joint_gripper_finger_left: {p: 100, d: 1, i: 0, i_clamp: 1} diff --git a/stretch_gazebo/config/head.yaml b/stretch_gazebo/config/head.yaml deleted file mode 100644 index e0ef3e76..00000000 --- a/stretch_gazebo/config/head.yaml +++ /dev/null @@ -1,15 +0,0 @@ -stretch_head_controller: - type: "position_controllers/JointTrajectoryController" - joints: - - joint_head_pan - - joint_head_tilt - allow_partial_joints_goal: true - - constraints: - goal_time: 0.6 - stopped_velocity_tolerance: 0.05 - joint_head_pan: {trajectory: 0.1, goal: 0.1} - joint_head_tilt: {trajectory: 0.1, goal: 0.1} - stop_trajectory_duration: 0.5 - state_publish_rate: 25 - action_monitor_rate: 10 \ No newline at end of file diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch index 4725dc57..98ebe844 100644 --- a/stretch_gazebo/launch/gazebo.launch +++ b/stretch_gazebo/launch/gazebo.launch @@ -45,16 +45,10 @@ ns="stretch_diff_drive_controller" /> - - - - + file="$(find stretch_gazebo)/config/body.yaml" /> + args="stretch_joint_state_controller stretch_diff_drive_controller stretch_controller_raw"/> diff --git a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro index 739ca868..110d5ab8 100644 --- a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro +++ b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro @@ -510,7 +510,7 @@ 1 - hardware_interface/PositionJointInterface + hardware_interface/EffortJointInterface @@ -520,7 +520,7 @@ 1 - hardware_interface/PositionJointInterface + hardware_interface/EffortJointInterface From 56c3e4c303ffed74d6040afa7f377f96562b4ead Mon Sep 17 00:00:00 2001 From: Nick Walker Date: Sun, 31 Oct 2021 19:39:31 -0700 Subject: [PATCH 5/5] Add custom Gazebo plugin This plug in is a modified version of the original ros_control plugin and that plugin's "default hardware plugin" (confusing, I know) The modified model plugin provides a /runstop service to match the real robot's driver, and hooks this up to ros_controls existing estop implementation The modified hardware plugin provides the mode switching services that the real robot has (mocked for now) and publishes joint states that include the virtual joints used by the real platform. The wrist extension joint behaves the same as the driver implementation. The rotation and translation joints aren't hooked up yet, and it may be difficult to provide their functionality without forking the actual "follow_joint_trajectory" implementation, which expects only joints that are present in the URDF Mirror the real platform setup and use a joint state publisher node to republish the subset of joints that match the URDF --- stretch_gazebo/CMakeLists.txt | 54 +- stretch_gazebo/config/joints.yaml | 2 - .../stretch_gazebo/stretch_hardware_gazebo.h | 173 +++++ .../stretch_hardware_gazebo_plugin.h | 143 ++++ stretch_gazebo/launch/gazebo.launch | 13 +- stretch_gazebo/package.xml | 11 + .../src/stretch_hardware_gazebo.cpp | 612 ++++++++++++++++++ .../src/stretch_hardware_gazebo_plugin.cpp | 340 ++++++++++ stretch_gazebo/stretch_gazebo_plugins.xml | 10 + stretch_gazebo/urdf/stretch_gazebo.urdf.xacro | 5 +- 10 files changed, 1345 insertions(+), 18 deletions(-) delete mode 100644 stretch_gazebo/config/joints.yaml create mode 100644 stretch_gazebo/include/stretch_gazebo/stretch_hardware_gazebo.h create mode 100644 stretch_gazebo/include/stretch_gazebo/stretch_hardware_gazebo_plugin.h create mode 100644 stretch_gazebo/src/stretch_hardware_gazebo.cpp create mode 100644 stretch_gazebo/src/stretch_hardware_gazebo_plugin.cpp create mode 100644 stretch_gazebo/stretch_gazebo_plugins.xml diff --git a/stretch_gazebo/CMakeLists.txt b/stretch_gazebo/CMakeLists.txt index ac6fa979..0a861364 100644 --- a/stretch_gazebo/CMakeLists.txt +++ b/stretch_gazebo/CMakeLists.txt @@ -1,16 +1,50 @@ cmake_minimum_required(VERSION 3.0.2) project(stretch_gazebo) -find_package(catkin REQUIRED) +find_package(catkin REQUIRED COMPONENTS + gazebo_dev + roscpp + std_msgs + control_toolbox + controller_manager + hardware_interface + transmission_interface + pluginlib + joint_limits_interface + urdf + angles) -catkin_package() +catkin_package(INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME}_ros_control_plugin ${PROJECT_NAME} + CATKIN_DEPENDS + roscpp + std_msgs + controller_manager + control_toolbox + pluginlib + hardware_interface + transmission_interface + joint_limits_interface + urdf + angles + hardware_interface) -install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) -install(DIRECTORY launch +include_directories(include ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS}) + + +add_library(${PROJECT_NAME}_ros_control_plugin src/stretch_hardware_gazebo_plugin.cpp) +target_link_libraries(${PROJECT_NAME}_ros_control_plugin ${catkin_LIBRARIES}) + +add_library(${PROJECT_NAME} src/stretch_hardware_gazebo.cpp) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) + + +install(DIRECTORY config launch urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) -install(DIRECTORY urdf -DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) + ) + +install(TARGETS ${PROJECT_NAME}_ros_control_plugin ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) \ No newline at end of file diff --git a/stretch_gazebo/config/joints.yaml b/stretch_gazebo/config/joints.yaml deleted file mode 100644 index b5cae6ab..00000000 --- a/stretch_gazebo/config/joints.yaml +++ /dev/null @@ -1,2 +0,0 @@ -type: "joint_state_controller/JointStateController" -publish_rate: 50 diff --git a/stretch_gazebo/include/stretch_gazebo/stretch_hardware_gazebo.h b/stretch_gazebo/include/stretch_gazebo/stretch_hardware_gazebo.h new file mode 100644 index 00000000..94d464a6 --- /dev/null +++ b/stretch_gazebo/include/stretch_gazebo/stretch_hardware_gazebo.h @@ -0,0 +1,173 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation + * Copyright (c) 2013, The Johns Hopkins University + * All rights reserved. + * + * 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 Open Source Robotics Foundation + * 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 OWNER 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. + *********************************************************************/ + +/* Author: Dave Coleman, Jonathan Bohren + Desc: Hardware Interface for any simulated robot in Gazebo +*/ + +#pragma once +// ros_control +#include +#include +#include +#include +#include +#include +#include + +// Gazebo +#include +#include +#include + +// ROS +#include +#include +#include +#include +#include + +// gazebo_ros_control +#include + +// URDF +#include + + + +namespace stretch_gazebo +{ + +class StretchHardwareGazebo : public gazebo_ros_control::RobotHWSim +{ +public: + + virtual bool initSim( + const std::string& robot_namespace, + ros::NodeHandle model_nh, + gazebo::physics::ModelPtr parent_model, + const urdf::Model *const urdf_model, + std::vector transmissions); + + virtual void readSim(ros::Time time, ros::Duration period); + + virtual void writeSim(ros::Time time, ros::Duration period); + + virtual void eStopActive(const bool active); + +protected: + // Methods used to control a joint. + enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID}; + + enum RobotMode {MANIPULATION, NAVIGATION, POSITION_MODE}; + + + // Register the limits of the joint specified by joint_name and joint_handle. The limits are + // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. + // Return the joint's type, lower position limit, upper position limit, and effort limit. + void registerJointLimits(const std::string& joint_name, + const hardware_interface::JointHandle& joint_handle, + const ControlMethod ctrl_method, + const ros::NodeHandle& joint_limit_nh, + const urdf::Model *const urdf_model, + int *const joint_type, double *const lower_limit, + double *const upper_limit, double *const effort_limit); + + bool setManipulationModeCB(std_srvs::SetBool::Request &req, + std_srvs::SetBool::Response &res); + + bool setNavigationModeCB(std_srvs::SetBool::Request &req, + std_srvs::SetBool::Response &res); + + bool setPositionModeCB(std_srvs::SetBool::Request &req, + std_srvs::SetBool::Response &res); + + void updateJointStates(const ros::TimerEvent& e); + + unsigned int n_dof_; + + hardware_interface::JointStateInterface js_interface_; + hardware_interface::EffortJointInterface ej_interface_; + hardware_interface::PositionJointInterface pj_interface_; + hardware_interface::VelocityJointInterface vj_interface_; + + joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_; + joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_; + joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_; + joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_; + joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_; + joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_; + + std::vector joint_names_; + std::vector joint_types_; + std::vector joint_lower_limits_; + std::vector joint_upper_limits_; + std::vector joint_effort_limits_; + std::vector joint_control_methods_; + std::vector pid_controllers_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_effort_command_; + std::vector joint_position_command_; + std::vector last_joint_position_command_; + std::vector joint_velocity_command_; + + int telescoping_joint_indices_[4]; + + std::vector sim_joints_; + + std::string physics_type_; + + // e_stop_active_ is true if the emergency stop is active. + bool e_stop_active_, last_e_stop_active_; + + ros::ServiceServer manipulation_mode_srv_; + ros::ServiceServer navigation_mode_srv_; + ros::ServiceServer position_mode_srv_; + + RobotMode current_mode_; + + ros::Timer joint_states_timer_; + + std::shared_ptr > realtime_pub_; + ros::Time last_publish_time_; + double publish_rate_; +}; + +typedef boost::shared_ptr StretchHardwareGazeboPtr; + +} diff --git a/stretch_gazebo/include/stretch_gazebo/stretch_hardware_gazebo_plugin.h b/stretch_gazebo/include/stretch_gazebo/stretch_hardware_gazebo_plugin.h new file mode 100644 index 00000000..d5d67e0c --- /dev/null +++ b/stretch_gazebo/include/stretch_gazebo/stretch_hardware_gazebo_plugin.h @@ -0,0 +1,143 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2013, Open Source Robotics Foundation +* Copyright (c) 2013, The Johns Hopkins University +* All rights reserved. +* +* 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 Open Source Robotics Foundation +* 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 OWNER 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. +*********************************************************************/ + +/* Author: Dave Coleman, Jonathan Bohren + Desc: Gazebo plugin for ros_control that allows 'hardware_interfaces' to be plugged in + using pluginlib +*/ + +#pragma once + +// Boost +#include +#include + +// ROS +#include +#include +#include +#include +#include + +// Gazebo +#include +#include +#include + +// ros_control +#include +#include +#include + +namespace stretch_gazebo +{ + +class GazeboRosControlPlugin : public gazebo::ModelPlugin +{ +public: + + virtual ~GazeboRosControlPlugin(); + + // Overloaded Gazebo entry point + virtual void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf); + + // Called by the world update start event + void Update(); + + // Called on world reset + virtual void Reset(); + + // Get the URDF XML from the parameter server + std::string getURDF(std::string param_name) const; + + // Get Transmissions from the URDF + bool parseTransmissionsFromURDF(const std::string& urdf_string); + +protected: + void eStopCB(const std_msgs::BoolConstPtr& e_stop_active); + bool triggerEStopCB(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res); + + bool setRunstopCB(std_srvs::SetBool::Request &req, + std_srvs::SetBool::Response &res); + + // Node Handles + ros::NodeHandle model_nh_; // namespaces to robot name + + // Pointer to the model + gazebo::physics::ModelPtr parent_model_; + sdf::ElementPtr sdf_; + + // deferred load in case ros is blocking + boost::thread deferred_load_thread_; + + // Pointer to the update event connection + gazebo::event::ConnectionPtr update_connection_; + + // Interface loader + boost::shared_ptr > robot_hw_sim_loader_; + void load_robot_hw_sim_srv(); + + // Strings + std::string robot_namespace_; + std::string robot_description_; + + // Transmissions in this plugin's scope + std::vector transmissions_; + + // Robot simulator interface + std::string robot_hw_sim_type_str_; + boost::shared_ptr robot_hw_sim_; + + // Controller manager + boost::shared_ptr controller_manager_; + + // Timing + ros::Duration control_period_; + ros::Time last_update_sim_time_ros_; + ros::Time last_write_sim_time_ros_; + + // e_stop_active_ is true if the emergency stop is active. + bool e_stop_active_, last_e_stop_active_; + ros::Subscriber e_stop_sub_; // Emergency stop subscriber + ros::ServiceServer e_stop_srv_; + + ros::ServiceServer runstop_srv_; + +}; + + +} diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch index 98ebe844..0a7e0d09 100644 --- a/stretch_gazebo/launch/gazebo.launch +++ b/stretch_gazebo/launch/gazebo.launch @@ -27,6 +27,13 @@ + + + + [/stretch/joint_states] + + + @@ -36,10 +43,6 @@ - - @@ -48,7 +51,7 @@ file="$(find stretch_gazebo)/config/body.yaml" /> + args="stretch_diff_drive_controller stretch_controller_raw"/> diff --git a/stretch_gazebo/package.xml b/stretch_gazebo/package.xml index 5924c10f..47327ab8 100644 --- a/stretch_gazebo/package.xml +++ b/stretch_gazebo/package.xml @@ -12,22 +12,33 @@ catkin + angles + control_toolbox controller_manager gazebo_msgs gazebo_plugins gazebo_ros_control gazebo_ros + hardware_interface + joint_limits_interface nav_msgs + pluginlib realsense_gazebo_plugin realsense2_camera realsense2_description robot_state_publisher ros_controllers + roscpp joint_state_controller rospy rviz std_msgs teleop_twist_joy + transmission_interface + urdf xacro + + + diff --git a/stretch_gazebo/src/stretch_hardware_gazebo.cpp b/stretch_gazebo/src/stretch_hardware_gazebo.cpp new file mode 100644 index 00000000..71187961 --- /dev/null +++ b/stretch_gazebo/src/stretch_hardware_gazebo.cpp @@ -0,0 +1,612 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation + * Copyright (c) 2013, The Johns Hopkins University + * All rights reserved. + * + * 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 Open Source Robotics Foundation + * 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 OWNER 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. + *********************************************************************/ + +/* Author: Dave Coleman, Jonathan Bohren + Desc: Hardware Interface for any simulated robot in Gazebo + Modified to provide mocks of Stretch mode interfaces and virtual joints states + References: + https://github.com/keyan/jaco_simulation/blob/0c898c5a3a6dd7d708223f8a740630a618c6478d/src/jaco_robot_hw_sim.cpp + https://github.com/VincentLam386/biped_robot2/blob/e76f358df33081eb600dae983cbd3ca2da7c282c/biped2_sim_hardware/src/biped2_sim_hardware.cpp + + +*/ + + + +#include +#include +#include + +namespace +{ + +double clamp(const double val, const double min_val, const double max_val) +{ + return std::min(std::max(val, min_val), max_val); +} + +} + +namespace stretch_gazebo +{ + + +bool StretchHardwareGazebo::initSim( + const std::string& robot_namespace, + ros::NodeHandle model_nh, + gazebo::physics::ModelPtr parent_model, + const urdf::Model *const urdf_model, + std::vector transmissions) +{ + // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each + // parameter's name is "joint_limits/". An example is "joint_limits/axle_joint". + const ros::NodeHandle joint_limit_nh(model_nh); + + // Resize vectors to our DOF + n_dof_ = transmissions.size(); + joint_names_.resize(n_dof_); + joint_types_.resize(n_dof_); + joint_lower_limits_.resize(n_dof_); + joint_upper_limits_.resize(n_dof_); + joint_effort_limits_.resize(n_dof_); + joint_control_methods_.resize(n_dof_); + pid_controllers_.resize(n_dof_); + joint_position_.resize(n_dof_); + joint_velocity_.resize(n_dof_); + joint_effort_.resize(n_dof_); + joint_effort_command_.resize(n_dof_); + joint_position_command_.resize(n_dof_); + joint_velocity_command_.resize(n_dof_); + + // Initialize values + for(unsigned int j=0; j < n_dof_; j++) + { + // Check that this transmission has one joint + if(transmissions[j].joints_.size() == 0) + { + ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ + << " has no associated joints."); + continue; + } + else if(transmissions[j].joints_.size() > 1) + { + ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ + << " has more than one joint. Currently the default robot hardware simulation " + << " interface only supports one."); + continue; + } + + std::vector joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_; + if (joint_interfaces.empty() && + !(transmissions[j].actuators_.empty()) && + !(transmissions[j].actuators_[0].hardware_interfaces_.empty())) + { + // TODO: Deprecate HW interface specification in actuators in ROS J + joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_; + ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "The element of tranmission " << + transmissions[j].name_ << " should be nested inside the element, not . " << + "The transmission will be properly loaded, but please update " << + "your robot model to remain compatible with future versions of the plugin."); + } + if (joint_interfaces.empty()) + { + ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ << + " of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " << + "Not adding it to the robot hardware simulation."); + continue; + } + else if (joint_interfaces.size() > 1) + { + ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ << + " of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " << + "Currently the default robot hardware simulation interface only supports one. Using the first entry"); + //continue; + } + + // Add data from transmission + joint_names_[j] = transmissions[j].joints_[0].name_; + joint_position_[j] = 1.0; + joint_velocity_[j] = 0.0; + joint_effort_[j] = 1.0; // N/m for continuous joints + joint_effort_command_[j] = 0.0; + joint_position_command_[j] = 0.0; + joint_velocity_command_[j] = 0.0; + + const std::string& hardware_interface = joint_interfaces.front(); + + // Debug + ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j] + << "' of type '" << hardware_interface << "'"); + + // Create joint state interface for all joints + js_interface_.registerHandle(hardware_interface::JointStateHandle( + joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j])); + + // Decide what kind of command interface this actuator/joint has + hardware_interface::JointHandle joint_handle; + if(hardware_interface == "EffortJointInterface" || hardware_interface == "hardware_interface/EffortJointInterface") + { + // Create effort joint interface + joint_control_methods_[j] = EFFORT; + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), + &joint_effort_command_[j]); + ej_interface_.registerHandle(joint_handle); + } + else if(hardware_interface == "PositionJointInterface" || hardware_interface == "hardware_interface/PositionJointInterface") + { + // Create position joint interface + joint_control_methods_[j] = POSITION; + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), + &joint_position_command_[j]); + pj_interface_.registerHandle(joint_handle); + } + else if(hardware_interface == "VelocityJointInterface" || hardware_interface == "hardware_interface/VelocityJointInterface") + { + // Create velocity joint interface + joint_control_methods_[j] = VELOCITY; + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), + &joint_velocity_command_[j]); + vj_interface_.registerHandle(joint_handle); + } + else + { + ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for '" + << hardware_interface << "' while loading interfaces for " << joint_names_[j] ); + return false; + } + + if(hardware_interface == "EffortJointInterface" || hardware_interface == "PositionJointInterface" || hardware_interface == "VelocityJointInterface") { + ROS_WARN_STREAM("Deprecated syntax, please prepend 'hardware_interface/' to '" << hardware_interface << "' within the tag in joint '" << joint_names_[j] << "'."); + } + + // Get the gazebo joint that corresponds to the robot joint. + //ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim", "Getting pointer to gazebo joint: " + // << joint_names_[j]); + gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]); + if (!joint) + { + ROS_ERROR_STREAM_NAMED("default_robot_hw", "This robot has a joint named \"" << joint_names_[j] + << "\" which is not in the gazebo model."); + return false; + } + sim_joints_.push_back(joint); + + // get physics engine type +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics(); +#else + gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->GetPhysicsEngine(); +#endif + physics_type_ = physics->GetType(); + if (physics_type_.empty()) + { + ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "No physics type found."); + } + + registerJointLimits(joint_names_[j], joint_handle, joint_control_methods_[j], + joint_limit_nh, urdf_model, + &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j], + &joint_effort_limits_[j]); + if (joint_control_methods_[j] != EFFORT) + { + // Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or + // joint->SetParam("vel") to control the joint. + const ros::NodeHandle nh(robot_namespace + "/gazebo_ros_control/pid_gains/" + + joint_names_[j]); + if (pid_controllers_[j].init(nh)) + { + switch (joint_control_methods_[j]) + { + case POSITION: + joint_control_methods_[j] = POSITION_PID; + break; + case VELOCITY: + joint_control_methods_[j] = VELOCITY_PID; + break; + } + } + else + { + // joint->SetParam("fmax") must be called if joint->SetAngle() or joint->SetParam("vel") are + // going to be called. joint->SetParam("fmax") must *not* be called if joint->SetForce() is + // going to be called. +#if GAZEBO_MAJOR_VERSION > 2 + joint->SetParam("fmax", 0, joint_effort_limits_[j]); +#else + joint->SetMaxForce(0, joint_effort_limits_[j]); +#endif + } + } + } + + // Register interfaces + registerInterface(&js_interface_); + registerInterface(&ej_interface_); + registerInterface(&pj_interface_); + registerInterface(&vj_interface_); + + // Initialize the emergency stop code. + e_stop_active_ = false; + last_e_stop_active_ = false; + + telescoping_joint_indices_[0] = std::find(joint_names_.begin(), joint_names_.end(), "joint_arm_l0") - joint_names_.begin(); + telescoping_joint_indices_[1] = std::find(joint_names_.begin(), joint_names_.end(), "joint_arm_l1") - joint_names_.begin(); + telescoping_joint_indices_[2] = std::find(joint_names_.begin(), joint_names_.end(), "joint_arm_l2") - joint_names_.begin(); + telescoping_joint_indices_[3] = std::find(joint_names_.begin(), joint_names_.end(), "joint_arm_l3") - joint_names_.begin(); + + // realtime joint state publisher + // FIXME(nickswalker,10-31-21): Doing this here is weird and the interfaces don't line up for us to get this from the SDF. + // Maybe there's a better way. + realtime_pub_.reset(new realtime_tools::RealtimePublisher(joint_limit_nh, "/stretch/joint_states", 4)); + realtime_pub_->msg_.name.resize(n_dof_ + 2); + realtime_pub_->msg_.name.assign(joint_names_.begin(), joint_names_.end()); + realtime_pub_->msg_.name.push_back("wrist_extension"); + realtime_pub_->msg_.name.push_back("joint_mobile_base_translation"); + realtime_pub_->msg_.position.resize(n_dof_ + 2); + realtime_pub_->msg_.velocity.resize(n_dof_ + 2); + realtime_pub_->msg_.effort.resize(n_dof_ + 2); + + + manipulation_mode_srv_ = model_nh.advertiseService("/switch_to_manipulation_mode", &StretchHardwareGazebo::setManipulationModeCB, this); + navigation_mode_srv_ = model_nh.advertiseService("/switch_to_navigation_mode", &StretchHardwareGazebo::setNavigationModeCB, this); + position_mode_srv_ = model_nh.advertiseService("/switch_to_position_mode", &StretchHardwareGazebo::setPositionModeCB, this); + + last_publish_time_ = ros::Time::now(); + publish_rate_ = 25; + joint_states_timer_ = model_nh.createTimer(ros::Duration(0.01), &StretchHardwareGazebo::updateJointStates,this); + joint_states_timer_.start(); + return true; +} + +void StretchHardwareGazebo::readSim(ros::Time time, ros::Duration period) +{ + for(unsigned int j=0; j < n_dof_; j++) + { + // Gazebo has an interesting API... +#if GAZEBO_MAJOR_VERSION >= 8 + double position = sim_joints_[j]->Position(0); +#else + double position = sim_joints_[j]->GetAngle(0).Radian(); +#endif + if (joint_types_[j] == urdf::Joint::PRISMATIC) + { + joint_position_[j] = position; + } + else + { + joint_position_[j] += angles::shortest_angular_distance(joint_position_[j], + position); + } + joint_velocity_[j] = sim_joints_[j]->GetVelocity(0); + joint_effort_[j] = sim_joints_[j]->GetForce((unsigned int)(0)); + } +} + +void StretchHardwareGazebo::writeSim(ros::Time time, ros::Duration period) +{ + // If the E-stop is active, joints controlled by position commands will maintain their positions. + if (e_stop_active_) + { + if (!last_e_stop_active_) + { + last_joint_position_command_ = joint_position_; + last_e_stop_active_ = true; + } + joint_position_command_ = last_joint_position_command_; + } + else + { + last_e_stop_active_ = false; + } + + ej_sat_interface_.enforceLimits(period); + ej_limits_interface_.enforceLimits(period); + pj_sat_interface_.enforceLimits(period); + pj_limits_interface_.enforceLimits(period); + vj_sat_interface_.enforceLimits(period); + vj_limits_interface_.enforceLimits(period); + + for(unsigned int j=0; j < n_dof_; j++) + { + switch (joint_control_methods_[j]) + { + case EFFORT: + { + const double effort = e_stop_active_ ? 0 : joint_effort_command_[j]; + sim_joints_[j]->SetForce(0, effort); + } + break; + + case POSITION: +#if GAZEBO_MAJOR_VERSION >= 9 + sim_joints_[j]->SetPosition(0, joint_position_command_[j], true); +#else + sim_joints_[j]->SetPosition(0, joint_position_command_[j]); +#endif + break; + + case POSITION_PID: + { + double error; + switch (joint_types_[j]) + { + case urdf::Joint::REVOLUTE: + angles::shortest_angular_distance_with_limits(joint_position_[j], + joint_position_command_[j], + joint_lower_limits_[j], + joint_upper_limits_[j], + error); + break; + case urdf::Joint::CONTINUOUS: + error = angles::shortest_angular_distance(joint_position_[j], + joint_position_command_[j]); + break; + default: + error = joint_position_command_[j] - joint_position_[j]; + } + + const double effort_limit = joint_effort_limits_[j]; + const double effort = clamp(pid_controllers_[j].computeCommand(error, period), + -effort_limit, effort_limit); + sim_joints_[j]->SetForce(0, effort); + } + break; + + case VELOCITY: +#if GAZEBO_MAJOR_VERSION > 2 + if (physics_type_.compare("dart") == 0) + { + sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]); + } + else + { + sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_velocity_command_[j]); + } +#else + sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]); +#endif + break; + + case VELOCITY_PID: + double error; + if (e_stop_active_) + error = -joint_velocity_[j]; + else + error = joint_velocity_command_[j] - joint_velocity_[j]; + const double effort_limit = joint_effort_limits_[j]; + const double effort = clamp(pid_controllers_[j].computeCommand(error, period), + -effort_limit, effort_limit); + sim_joints_[j]->SetForce(0, effort); + break; + } + } +} + +void StretchHardwareGazebo::eStopActive(const bool active) +{ + e_stop_active_ = active; +} + +bool StretchHardwareGazebo::setManipulationModeCB(std_srvs::SetBool::Request &req, + std_srvs::SetBool::Response &res) +{ + current_mode_ = MANIPULATION; + res.success = true; + res.message = "Now in manipulation mode."; + return true; +} + +bool StretchHardwareGazebo::setNavigationModeCB(std_srvs::SetBool::Request &req, + std_srvs::SetBool::Response &res) +{ + current_mode_ = NAVIGATION; + res.success = true; + res.message = "Now in navigation mode."; + return true; +} + +bool StretchHardwareGazebo::setPositionModeCB(std_srvs::SetBool::Request &req, + std_srvs::SetBool::Response &res) +{ + current_mode_ = POSITION_MODE; + res.success = true; + res.message = "Now in position mode."; + return true; +} + +// Register the limits of the joint specified by joint_name and joint_handle. The limits are +// retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. +// Return the joint's type, lower position limit, upper position limit, and effort limit. +void StretchHardwareGazebo::registerJointLimits(const std::string& joint_name, + const hardware_interface::JointHandle& joint_handle, + const ControlMethod ctrl_method, + const ros::NodeHandle& joint_limit_nh, + const urdf::Model *const urdf_model, + int *const joint_type, double *const lower_limit, + double *const upper_limit, double *const effort_limit) +{ + *joint_type = urdf::Joint::UNKNOWN; + *lower_limit = -std::numeric_limits::max(); + *upper_limit = std::numeric_limits::max(); + *effort_limit = std::numeric_limits::max(); + + joint_limits_interface::JointLimits limits; + bool has_limits = false; + joint_limits_interface::SoftJointLimits soft_limits; + bool has_soft_limits = false; + + if (urdf_model != NULL) + { + const urdf::JointConstSharedPtr urdf_joint = urdf_model->getJoint(joint_name); + if (urdf_joint != NULL) + { + *joint_type = urdf_joint->type; + // Get limits from the URDF file. + if (joint_limits_interface::getJointLimits(urdf_joint, limits)) + has_limits = true; + if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) + has_soft_limits = true; + } + } + // Get limits from the parameter server. + if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits)) + has_limits = true; + + if (!has_limits) + return; + + if (*joint_type == urdf::Joint::UNKNOWN) + { + // Infer the joint type. + + if (limits.has_position_limits) + { + *joint_type = urdf::Joint::REVOLUTE; + } + else + { + if (limits.angle_wraparound) + *joint_type = urdf::Joint::CONTINUOUS; + else + *joint_type = urdf::Joint::PRISMATIC; + } + } + + if (limits.has_position_limits) + { + *lower_limit = limits.min_position; + *upper_limit = limits.max_position; + } + if (limits.has_effort_limits) + *effort_limit = limits.max_effort; + + if (has_soft_limits) + { + switch (ctrl_method) + { + case EFFORT: + { + const joint_limits_interface::EffortJointSoftLimitsHandle + limits_handle(joint_handle, limits, soft_limits); + ej_limits_interface_.registerHandle(limits_handle); + } + break; + case POSITION: + { + const joint_limits_interface::PositionJointSoftLimitsHandle + limits_handle(joint_handle, limits, soft_limits); + pj_limits_interface_.registerHandle(limits_handle); + } + break; + case VELOCITY: + { + const joint_limits_interface::VelocityJointSoftLimitsHandle + limits_handle(joint_handle, limits, soft_limits); + vj_limits_interface_.registerHandle(limits_handle); + } + break; + } + } + else + { + switch (ctrl_method) + { + case EFFORT: + { + const joint_limits_interface::EffortJointSaturationHandle + sat_handle(joint_handle, limits); + ej_sat_interface_.registerHandle(sat_handle); + } + break; + case POSITION: + { + const joint_limits_interface::PositionJointSaturationHandle + sat_handle(joint_handle, limits); + pj_sat_interface_.registerHandle(sat_handle); + } + break; + case VELOCITY: + { + const joint_limits_interface::VelocityJointSaturationHandle + sat_handle(joint_handle, limits); + vj_sat_interface_.registerHandle(sat_handle); + } + break; + } + } +} + +void StretchHardwareGazebo::updateJointStates(const ros::TimerEvent& e) +{ + ros::Time time = e.current_real; + // limit rate of publishing + if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0/publish_rate_) < time){ + // try to publish + if (realtime_pub_->trylock()){ + + // we're actually publishing, so increment time + last_publish_time_ = last_publish_time_ + ros::Duration(1.0/publish_rate_); + + // populate joint state message + realtime_pub_->msg_.header.stamp = time; + for (unsigned i=0; i< joint_position_.size(); i++){ + realtime_pub_->msg_.position[i] = joint_position_[i]; + realtime_pub_->msg_.velocity[i] = joint_velocity_[i]; + realtime_pub_->msg_.effort[i] = joint_effort_[i]; + } + const auto wrist_extension_position = realtime_pub_->msg_.position.end() - 2; + const auto wrist_extension_velocity = realtime_pub_->msg_.velocity.end() - 2; + const auto wrist_extension_effort = realtime_pub_->msg_.effort.end() - 2; + *wrist_extension_position = 0; + *wrist_extension_velocity = 0; + *wrist_extension_effort = 0; + for (int i : telescoping_joint_indices_) { + *wrist_extension_position += joint_position_[i]; + *wrist_extension_velocity += joint_velocity_[i]; + *wrist_extension_effort += joint_effort_[i]; + } + // TODO: figure out translation joint + *(realtime_pub_->msg_.position.end() - 1) = 0; + *(realtime_pub_->msg_.velocity.end() - 1) = 0; + *(realtime_pub_->msg_.effort.end() - 1) = 0; + realtime_pub_->unlockAndPublish(); + } + } +} + +} + +PLUGINLIB_EXPORT_CLASS( stretch_gazebo::StretchHardwareGazebo, gazebo_ros_control::RobotHWSim) \ No newline at end of file diff --git a/stretch_gazebo/src/stretch_hardware_gazebo_plugin.cpp b/stretch_gazebo/src/stretch_hardware_gazebo_plugin.cpp new file mode 100644 index 00000000..eb7f9768 --- /dev/null +++ b/stretch_gazebo/src/stretch_hardware_gazebo_plugin.cpp @@ -0,0 +1,340 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2013, Open Source Robotics Foundation +* Copyright (c) 2013, The Johns Hopkins University +* All rights reserved. +* +* 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 Open Source Robotics Foundation +* 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 OWNER 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. +*********************************************************************/ + +/* Author: Dave Coleman, Jonathan Bohren + Desc: Gazebo plugin for ros_control that allows 'hardware_interfaces' to be plugged in + using pluginlib + + Modified to provide mocked Stretch runstop service +*/ + +// Boost +#include + +#include +#include +#include + +namespace stretch_gazebo +{ + +GazeboRosControlPlugin::~GazeboRosControlPlugin() +{ + // Disconnect from gazebo events + update_connection_.reset(); +} + +// Overloaded Gazebo entry point +void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) +{ + ROS_INFO_STREAM_NAMED("gazebo_ros_control","Loading stretch_gazebo_ros_control plugin"); + + + // Save pointers to the model + parent_model_ = parent; + sdf_ = sdf; + + // Error message if the model couldn't be found + if (!parent_model_) + { + ROS_ERROR_STREAM_NAMED("loadThread","parent model is NULL"); + return; + } + + // Check that ROS has been initialized + if(!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("gazebo_ros_control","A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + // Get namespace for nodehandle + if(sdf_->HasElement("robotNamespace")) + { + robot_namespace_ = sdf_->GetElement("robotNamespace")->Get(); + } + else + { + robot_namespace_ = parent_model_->GetName(); // default + } + + // Get robot_description ROS param name + if (sdf_->HasElement("robotParam")) + { + robot_description_ = sdf_->GetElement("robotParam")->Get(); + } + else + { + robot_description_ = "robot_description"; // default + } + + // Get the robot simulation interface type + if(sdf_->HasElement("robotSimType")) + { + robot_hw_sim_type_str_ = sdf_->Get("robotSimType"); + } + else + { + robot_hw_sim_type_str_ = "gazebo_ros_control/DefaultRobotHWSim"; + ROS_DEBUG_STREAM_NAMED("loadThread","Using default plugin for RobotHWSim (none specified in URDF/SDF)\""<= 8 + ros::Duration gazebo_period(parent_model_->GetWorld()->Physics()->GetMaxStepSize()); +#else + ros::Duration gazebo_period(parent_model_->GetWorld()->GetPhysicsEngine()->GetMaxStepSize()); +#endif + + // Decide the plugin control period + if(sdf_->HasElement("controlPeriod")) + { + control_period_ = ros::Duration(sdf_->Get("controlPeriod")); + + // Check the period against the simulation period + if( control_period_ < gazebo_period ) + { + ROS_ERROR_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("< gazebo_period ) + { + ROS_WARN_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("<HasElement("eStopTopic")) + { + const std::string e_stop_topic = sdf_->GetElement("eStopTopic")->Get(); + e_stop_sub_ = model_nh_.subscribe(e_stop_topic, 1, &GazeboRosControlPlugin::eStopCB, this); + e_stop_srv_ = model_nh_.advertiseService("/stop_the_robot", &GazeboRosControlPlugin::triggerEStopCB, this); + runstop_srv_ = model_nh_.advertiseService("/runstop", &GazeboRosControlPlugin::setRunstopCB, this); + } + + if (sdf_->HasElement("jointStatesTopic")) + { + const std::string joint_states_topic = sdf_->GetElement("jointStatesTopic")->Get(); + } + + ROS_INFO_NAMED("gazebo_ros_control", "Starting gazebo_ros_control plugin in namespace: %s", robot_namespace_.c_str()); + + // Read urdf from ros parameter server then + // setup actuators and mechanism control node. + // This call will block if ROS is not properly initialized. + const std::string urdf_string = getURDF(robot_description_); + if (!parseTransmissionsFromURDF(urdf_string)) + { + ROS_ERROR_NAMED("gazebo_ros_control", "Error parsing URDF in gazebo_ros_control plugin, plugin not active.\n"); + return; + } + + // Load the RobotHWSim abstraction to interface the controllers with the gazebo model + try + { + robot_hw_sim_loader_.reset + (new pluginlib::ClassLoader + ("gazebo_ros_control", + "gazebo_ros_control::RobotHWSim")); + + robot_hw_sim_ = robot_hw_sim_loader_->createInstance(robot_hw_sim_type_str_); + urdf::Model urdf_model; + const urdf::Model *const urdf_model_ptr = urdf_model.initString(urdf_string) ? &urdf_model : NULL; + + if(!robot_hw_sim_->initSim(robot_ns, model_nh_, parent_model_, urdf_model_ptr, transmissions_)) + { + ROS_FATAL_NAMED("gazebo_ros_control","Could not initialize robot simulation interface"); + return; + } + + // Create the controller manager + ROS_DEBUG_STREAM_NAMED("ros_control_plugin","Loading controller_manager"); + controller_manager_.reset + (new controller_manager::ControllerManager(robot_hw_sim_.get(), model_nh_)); + + // Listen to the update event. This event is broadcast every simulation iteration. + update_connection_ = + gazebo::event::Events::ConnectWorldUpdateBegin + (boost::bind(&GazeboRosControlPlugin::Update, this)); + + } + catch(pluginlib::LibraryLoadException &ex) + { + ROS_FATAL_STREAM_NAMED("gazebo_ros_control","Failed to create robot simulation interface loader: "<= 8 + gazebo::common::Time gz_time_now = parent_model_->GetWorld()->SimTime(); +#else + gazebo::common::Time gz_time_now = parent_model_->GetWorld()->GetSimTime(); +#endif + ros::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec); + ros::Duration sim_period = sim_time_ros - last_update_sim_time_ros_; + + robot_hw_sim_->eStopActive(e_stop_active_); + + // Check if we should update the controllers + if(sim_period >= control_period_) { + // Store this simulation time + last_update_sim_time_ros_ = sim_time_ros; + + // Update the robot simulation with the state of the gazebo model + robot_hw_sim_->readSim(sim_time_ros, sim_period); + + // Compute the controller commands + bool reset_ctrlrs; + if (e_stop_active_) + { + reset_ctrlrs = false; + last_e_stop_active_ = true; + } + else + { + if (last_e_stop_active_) + { + reset_ctrlrs = true; + last_e_stop_active_ = false; + } + else + { + reset_ctrlrs = false; + } + } + controller_manager_->update(sim_time_ros, sim_period, reset_ctrlrs); + } + + // Update the gazebo model with the result of the controller + // computation + robot_hw_sim_->writeSim(sim_time_ros, sim_time_ros - last_write_sim_time_ros_); + last_write_sim_time_ros_ = sim_time_ros; +} + +// Called on world reset +void GazeboRosControlPlugin::Reset() +{ + // Reset timing variables to not pass negative update periods to controllers on world reset + last_update_sim_time_ros_ = ros::Time(); + last_write_sim_time_ros_ = ros::Time(); +} + +// Get the URDF XML from the parameter server +std::string GazeboRosControlPlugin::getURDF(std::string param_name) const +{ + std::string urdf_string; + + // search and wait for robot_description on param server + while (urdf_string.empty()) + { + std::string search_param_name; + if (model_nh_.searchParam(param_name, search_param_name)) + { + ROS_INFO_ONCE_NAMED("gazebo_ros_control", "gazebo_ros_control plugin is waiting for model" + " URDF in parameter [%s] on the ROS param server.", search_param_name.c_str()); + + model_nh_.getParam(search_param_name, urdf_string); + } + else + { + ROS_INFO_ONCE_NAMED("gazebo_ros_control", "gazebo_ros_control plugin is waiting for model" + " URDF in parameter [%s] on the ROS param server.", robot_description_.c_str()); + + model_nh_.getParam(param_name, urdf_string); + } + + usleep(100000); + } + ROS_DEBUG_STREAM_NAMED("gazebo_ros_control", "Recieved urdf from param server, parsing..."); + + return urdf_string; +} + +// Get Transmissions from the URDF +bool GazeboRosControlPlugin::parseTransmissionsFromURDF(const std::string& urdf_string) +{ + transmission_interface::TransmissionParser::parse(urdf_string, transmissions_); + return true; +} + +// Emergency stop callback +void GazeboRosControlPlugin::eStopCB(const std_msgs::BoolConstPtr& e_stop_active) +{ + e_stop_active_ = e_stop_active->data; +} + +bool GazeboRosControlPlugin::triggerEStopCB(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) +{ + e_stop_active_ = true; + res.success = true; + res.message = "Stopped the robot."; + return true; +} + +bool GazeboRosControlPlugin::setRunstopCB(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) +{ + e_stop_active_ = req.data; + res.success = true; + res.message = std::string("is_runstopped: ") + (req.data ? "true" : "false"); + return true; +} + + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(stretch_gazebo::GazeboRosControlPlugin); +} // namespace diff --git a/stretch_gazebo/stretch_gazebo_plugins.xml b/stretch_gazebo/stretch_gazebo_plugins.xml new file mode 100644 index 00000000..323538d2 --- /dev/null +++ b/stretch_gazebo/stretch_gazebo_plugins.xml @@ -0,0 +1,10 @@ + + + + Stretch robot simulation interface. + + + diff --git a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro index 110d5ab8..1db0f6e9 100644 --- a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro +++ b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro @@ -11,8 +11,11 @@ - + + stretch_gazebo/StretchHardwareGazebo / + estop + stretch/joint_states