Skip to content

Commit 811b006

Browse files
nickswalkernickswalker
committed
Improve sim-real correspondence
Add 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 Add controller shim that mimics the fake joints used by the real robot Use effort control for all arm joints 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 Use single controller for the simulation This better matches the physical robot, though this interface doesn't handle the virtual joints that the robot uses This commit tracks the changes proposed in hello-robot/stretch_ros#51
1 parent 8e9e9fa commit 811b006

15 files changed

Lines changed: 1732 additions & 46 deletions

‎CMakeLists.txt‎

Lines changed: 52 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,12 +2,62 @@ cmake_minimum_required(VERSION 2.8.3)
22
project(hcrl_gazebo)
33

44
find_package(catkin REQUIRED COMPONENTS
5-
gazebo_ros
5+
gazebo_dev
6+
roscpp
7+
std_msgs
8+
control_toolbox
9+
controller_manager
10+
hardware_interface
11+
transmission_interface
12+
pluginlib
13+
joint_limits_interface
14+
urdf
15+
angles
616
)
717

8-
catkin_package()
18+
catkin_package(
19+
INCLUDE_DIRS include
20+
LIBRARIES ${PROJECT_NAME}
21+
CATKIN_DEPENDS
22+
roscpp
23+
std_msgs
24+
controller_manager
25+
control_toolbox
26+
pluginlib
27+
hardware_interface
28+
transmission_interface
29+
joint_limits_interface
30+
urdf
31+
angles
32+
hardware_interface)
933

1034
install(DIRECTORY launch models worlds maps photos robots
1135
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
1236
)
1337

38+
39+
include_directories(include ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
40+
41+
add_library(${PROJECT_NAME}_ros_control_plugin src/stretch_hardware_gazebo_plugin.cpp)
42+
target_link_libraries(${PROJECT_NAME}_ros_control_plugin ${catkin_LIBRARIES})
43+
44+
add_library(${PROJECT_NAME} src/stretch_hardware_gazebo.cpp)
45+
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
46+
47+
48+
install(TARGETS ${PROJECT_NAME}
49+
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
50+
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
51+
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
52+
)
53+
54+
install(DIRECTORY include/
55+
DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
56+
FILES_MATCHING PATTERN "*.h"
57+
)
58+
install (FILES hcrl_gazebo_plugins.xml
59+
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
60+
61+
62+
catkin_install_python(PROGRAMS scripts/follow_joint_trajectory_server scripts/unpause_after_wait
63+
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

‎config/stretch/body.yaml‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
stretch_controller_raw:
2+
type: "effort_controllers/JointTrajectoryController"
3+
joints:
4+
- joint_head_pan
5+
- joint_head_tilt
6+
- joint_lift
7+
- joint_arm_l3
8+
- joint_arm_l2
9+
- joint_arm_l1
10+
- joint_arm_l0
11+
- joint_wrist_yaw
12+
- joint_gripper_finger_right
13+
- joint_gripper_finger_left
14+
allow_partial_joints_goal: true
15+
16+
constraints:
17+
goal_time: 0.6
18+
stopped_velocity_tolerance: 0.05
19+
20+
stop_trajectory_duration: 0.5
21+
state_publish_rate: 25
22+
action_monitor_rate: 10
23+
24+
25+
gains:
26+
joint_head_pan: {p: 10, d: 1, i: 0, i_clamp: 1}
27+
joint_head_tilt: {p: 10, d: 1, i: 0, i_clamp: 1}
28+
joint_lift: {p: 500, d: 100, i: 0, i_clamp: 1}
29+
joint_arm_l3: {p: 100, d: 30, i: 0, i_clamp: 1}
30+
joint_arm_l2: {p: 100, d: 30, i: 0, i_clamp: 1}
31+
joint_arm_l1: {p: 100, d: 30, i: 0, i_clamp: 1}
32+
joint_arm_l0: {p: 100, d: 30, i: 0, i_clamp: 1}
33+
joint_wrist_yaw: {p: 100, d: 1, i: 0, i_clamp: 1}
34+
joint_gripper_finger_right: {p: 100, d: 1, i: 0, i_clamp: 1}
35+
joint_gripper_finger_left: {p: 100, d: 1, i: 0, i_clamp: 1}

‎hcrl_gazebo_plugins.xml‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
<library path="lib/libhcrl_gazebo">
2+
3+
<class
4+
name="hcrl_gazebo/StretchHardwareGazebo"
5+
type="hcrl_gazebo::StretchHardwareGazebo"
6+
base_class_type="gazebo_ros_control::RobotHWSim">
7+
<description>
8+
Stretch robot simulation interface.
9+
</description>
10+
</class>
11+
</library>
12+

‎include/hcrl_gazebo/stretch_hardware_gazebo.h‎

Lines changed: 175 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,175 @@
1+
#pragma once
2+
3+
/*********************************************************************
4+
* Software License Agreement (BSD License)
5+
*
6+
* Copyright (c) 2013, Open Source Robotics Foundation
7+
* Copyright (c) 2013, The Johns Hopkins University
8+
* All rights reserved.
9+
*
10+
* Redistribution and use in source and binary forms, with or without
11+
* modification, are permitted provided that the following conditions
12+
* are met:
13+
*
14+
* * Redistributions of source code must retain the above copyright
15+
* notice, this list of conditions and the following disclaimer.
16+
* * Redistributions in binary form must reproduce the above
17+
* copyright notice, this list of conditions and the following
18+
* disclaimer in the documentation and/or other materials provided
19+
* with the distribution.
20+
* * Neither the name of the Open Source Robotics Foundation
21+
* nor the names of its contributors may be
22+
* used to endorse or promote products derived
23+
* from this software without specific prior written permission.
24+
*
25+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36+
* POSSIBILITY OF SUCH DAMAGE.
37+
*********************************************************************/
38+
39+
/* Author: Dave Coleman, Jonathan Bohren
40+
Desc: Hardware Interface for any simulated robot in Gazebo
41+
*/
42+
43+
44+
// ros_control
45+
#include <control_toolbox/pid.h>
46+
#include <hardware_interface/joint_command_interface.h>
47+
#include <hardware_interface/robot_hw.h>
48+
#include <joint_limits_interface/joint_limits.h>
49+
#include <joint_limits_interface/joint_limits_interface.h>
50+
#include <joint_limits_interface/joint_limits_rosparam.h>
51+
#include <joint_limits_interface/joint_limits_urdf.h>
52+
53+
// Gazebo
54+
#include <gazebo/common/common.hh>
55+
#include <gazebo/physics/physics.hh>
56+
#include <gazebo/gazebo.hh>
57+
58+
// ROS
59+
#include <ros/ros.h>
60+
#include <angles/angles.h>
61+
#include <pluginlib/class_list_macros.h>
62+
#include <sensor_msgs/JointState.h>
63+
#include <std_srvs/SetBool.h>
64+
65+
// gazebo_ros_control
66+
#include <gazebo_ros_control/robot_hw_sim.h>
67+
68+
// URDF
69+
#include <urdf/model.h>
70+
71+
72+
73+
namespace hcrl_gazebo
74+
{
75+
76+
class StretchHardwareGazebo : public gazebo_ros_control::RobotHWSim
77+
{
78+
public:
79+
80+
virtual bool initSim(
81+
const std::string& robot_namespace,
82+
ros::NodeHandle model_nh,
83+
gazebo::physics::ModelPtr parent_model,
84+
const urdf::Model *const urdf_model,
85+
std::vector<transmission_interface::TransmissionInfo> transmissions);
86+
87+
virtual void readSim(ros::Time time, ros::Duration period);
88+
89+
virtual void writeSim(ros::Time time, ros::Duration period);
90+
91+
virtual void eStopActive(const bool active);
92+
93+
protected:
94+
// Methods used to control a joint.
95+
enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID};
96+
97+
enum RobotMode {MANIPULATION, NAVIGATION, POSITION_MODE};
98+
99+
100+
// Register the limits of the joint specified by joint_name and joint_handle. The limits are
101+
// retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also.
102+
// Return the joint's type, lower position limit, upper position limit, and effort limit.
103+
void registerJointLimits(const std::string& joint_name,
104+
const hardware_interface::JointHandle& joint_handle,
105+
const ControlMethod ctrl_method,
106+
const ros::NodeHandle& joint_limit_nh,
107+
const urdf::Model *const urdf_model,
108+
int *const joint_type, double *const lower_limit,
109+
double *const upper_limit, double *const effort_limit);
110+
111+
bool setManipulationModeCB(std_srvs::SetBool::Request &req,
112+
std_srvs::SetBool::Response &res);
113+
114+
bool setNavigationModeCB(std_srvs::SetBool::Request &req,
115+
std_srvs::SetBool::Response &res);
116+
117+
bool setPositionModeCB(std_srvs::SetBool::Request &req,
118+
std_srvs::SetBool::Response &res);
119+
120+
void updateJointStates(const ros::TimerEvent& e);
121+
122+
unsigned int n_dof_;
123+
124+
hardware_interface::JointStateInterface js_interface_;
125+
hardware_interface::EffortJointInterface ej_interface_;
126+
hardware_interface::PositionJointInterface pj_interface_;
127+
hardware_interface::VelocityJointInterface vj_interface_;
128+
129+
joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_;
130+
joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_;
131+
joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_;
132+
joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_;
133+
joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_;
134+
joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_;
135+
136+
std::vector<std::string> joint_names_;
137+
std::vector<int> joint_types_;
138+
std::vector<double> joint_lower_limits_;
139+
std::vector<double> joint_upper_limits_;
140+
std::vector<double> joint_effort_limits_;
141+
std::vector<ControlMethod> joint_control_methods_;
142+
std::vector<control_toolbox::Pid> pid_controllers_;
143+
std::vector<double> joint_position_;
144+
std::vector<double> joint_velocity_;
145+
std::vector<double> joint_effort_;
146+
std::vector<double> joint_effort_command_;
147+
std::vector<double> joint_position_command_;
148+
std::vector<double> last_joint_position_command_;
149+
std::vector<double> joint_velocity_command_;
150+
151+
int telescoping_joint_indices_[4];
152+
153+
std::vector<gazebo::physics::JointPtr> sim_joints_;
154+
155+
std::string physics_type_;
156+
157+
// e_stop_active_ is true if the emergency stop is active.
158+
bool e_stop_active_, last_e_stop_active_;
159+
160+
ros::ServiceServer manipulation_mode_srv_;
161+
ros::ServiceServer navigation_mode_srv_;
162+
ros::ServiceServer position_mode_srv_;
163+
164+
RobotMode current_mode_;
165+
166+
ros::Timer joint_states_timer_;
167+
168+
std::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::JointState> > realtime_pub_;
169+
ros::Time last_publish_time_;
170+
double publish_rate_;
171+
};
172+
173+
typedef boost::shared_ptr<StretchHardwareGazebo> StretchHardwareGazeboPtr;
174+
175+
}

0 commit comments

Comments
 (0)