From 589dca8e2a02e26ae03deec6785c0877f40d4f6a Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 8 Oct 2015 15:37:49 +0200 Subject: [PATCH] Improved ros_control performance and stability --- README.md | 10 +++- config/ur10_controllers.yaml | 55 ++++++++++++++++--- config/ur5_controllers.yaml | 18 +++--- .../ur_modern_driver/ur_hardware_interface.h | 8 ++- launch/ur10_ros_control.launch | 6 +- launch/ur5_ros_control.launch | 4 +- src/ur_hardware_interface.cpp | 29 +++++++++- src/ur_ros_wrapper.cpp | 8 +++ 8 files changed, 108 insertions(+), 30 deletions(-) diff --git a/README.md b/README.md index 8748d54..88a3acd 100644 --- a/README.md +++ b/README.md @@ -28,9 +28,13 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * Added support for ros_control. * As ros_control wants to have control over the robot at all times, ros_control compatability is set via a parameter at launch-time. * With ros_control active, the driver doesn't open the action_lib interface nor publish joint_states or wrench msgs. This is handled by ros_control instead. + * Currently two controllers are available, both controlling the joint position of the robot, useable for trajectroy execution + * The velocity based controller sends joint speed commands to the robot, using the speej command + * The position based controller send joint position commands to the robot, using the servoj command + * I have so far only used the velocity based controller, but which one is optimal depends on the application. * As ros_control continuesly controls the robot, using the teach pendant while a controller is running will cause the controller **on the robot** to crash, as it obviously can't handle conflicting control input from two sources. Thus be sure to stop the running controller **before** moving the robot via the teach pendant: * A list of the loaded and running controllers can be found by a call to the controller_manager ```rosservice call /controller_manager/list_controllers {} ``` - * The running position trajectory controller can be stopped with a call to ```rosservice call /universal_robot/controller_manager/switch_controller "start_controllers: - '' stop_controllers: - 'position_based_position_trajectory_controller' strictness: 1" ``` (Remember you can use tab-completion for this) + * The running position trajectory controller can be stopped with a call to ```rosservice call /universal_robot/controller_manager/switch_controller "start_controllers: - '' stop_controllers: - 'pos_based_pos_traj_controller' strictness: 1" ``` (Remember you can use tab-completion for this) ## Installation @@ -61,12 +65,12 @@ Be sure to stop the currently running controller **either before or in the same The position based controller *should* stay closer to the commanded path, while the velocity based react faster (trajectory execution start within 50-70 ms, while it is in the 150-180ms range for the position_based. Usage without ros_control as well as the old driver is also in the 170ms range, as mentioned at my lightning talk @ ROSCon 2013). -**Note** that the PID values are not tweaked as of this moment. +**Note** that the PID values are not optimally tweaked as of this moment. To use ros_control together with MoveIt, be sure to add the desired controller to the ```controllers.yaml``` in the urXX_moveit_config/config folder. Add the following ``` controller_list: - - name: velocity_based_position_trajectory_controller #or position_based_position_trajectory_controller + - name: vel_based_pos_traj_controller #or pos_based_pos_traj_controller action_ns: follow_joint_trajectory type: FollowJointTrajectory default: true diff --git a/config/ur10_controllers.yaml b/config/ur10_controllers.yaml index dd679bc..8c0c5bc 100644 --- a/config/ur10_controllers.yaml +++ b/config/ur10_controllers.yaml @@ -22,10 +22,10 @@ force_torque_sensor_controller: type: force_torque_sensor_controller/ForceTorqueSensorController publish_rate: 125 -# Joint Trajectory Controller ------------------------------- +# Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -velocity_based_position_trajectory_controller: - type: velocity_controllers/JointTrajectoryController +pos_based_pos_traj_controller: + type: position_controllers/JointTrajectoryController joints: - shoulder_pan_joint - shoulder_lift_joint @@ -47,12 +47,49 @@ velocity_based_position_trajectory_controller: action_monitor_rate: 10 gains: #!!These values have not been optimized!! - shoulder_pan_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - shoulder_lift_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - elbow_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - wrist_1_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - wrist_2_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - wrist_3_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + shoulder_pan_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + shoulder_lift_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + elbow_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + wrist_1_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + wrist_2_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + wrist_3_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + + # state_publish_rate: 50 # Defaults to 50 + # action_monitor_rate: 20 # Defaults to 20 + #hold_trajectory_duration: 0 # Defaults to 0.5 + +# Joint Trajectory Controller ------------------------------- +# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller +vel_based_pos_traj_controller: + type: velocity_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.1, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} + elbow_joint: {trajectory: 0.1, goal: 0.1} + wrist_1_joint: {trajectory: 0.1, goal: 0.1} + wrist_2_joint: {trajectory: 0.1, goal: 0.1} + wrist_3_joint: {trajectory: 0.1, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 125 + action_monitor_rate: 10 + gains: + #!!These values are useable, but maybe not optimal + shoulder_pan_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + shoulder_lift_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + elbow_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + wrist_1_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + wrist_2_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + wrist_3_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + # state_publish_rate: 50 # Defaults to 50 # action_monitor_rate: 20 # Defaults to 20 diff --git a/config/ur5_controllers.yaml b/config/ur5_controllers.yaml index 57591ca..a1054b7 100644 --- a/config/ur5_controllers.yaml +++ b/config/ur5_controllers.yaml @@ -24,7 +24,7 @@ force_torque_sensor_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -position_based_position_trajectory_controller: +pos_based_pos_traj_controller: type: position_controllers/JointTrajectoryController joints: - shoulder_pan_joint @@ -60,7 +60,7 @@ position_based_position_trajectory_controller: # Joint Trajectory Controller - velocity based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -velocity_based_position_trajectory_controller: +vel_based_pos_traj_controller: type: velocity_controllers/JointTrajectoryController joints: - shoulder_pan_joint @@ -82,13 +82,13 @@ velocity_based_position_trajectory_controller: state_publish_rate: 125 action_monitor_rate: 10 gains: - #!!These values have not been optimized!! - shoulder_pan_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - shoulder_lift_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - elbow_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - wrist_1_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - wrist_2_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - wrist_3_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + #!!These values are useable, but maybe not optimal + shoulder_pan_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + shoulder_lift_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + elbow_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + wrist_1_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + wrist_2_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + wrist_3_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} # state_publish_rate: 50 # Defaults to 50 # action_monitor_rate: 20 # Defaults to 20 diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h index c2de2d7..ff9572a 100644 --- a/include/ur_modern_driver/ur_hardware_interface.h +++ b/include/ur_modern_driver/ur_hardware_interface.h @@ -58,6 +58,7 @@ #include #include #include +#include #include "do_output.h" #include "ur_driver.h" @@ -86,6 +87,8 @@ public: /// \brief write the command to the robot hardware. virtual void write(); + void setMaxVelChange(double inp); + bool canSwitch( const std::list &start_list, const std::list &stop_list) const; @@ -111,10 +114,13 @@ protected: std::vector joint_effort_; std::vector joint_position_command_; std::vector joint_velocity_command_; - std::size_t num_joints_; + std::vector prev_joint_velocity_command_; + std::size_t num_joints_; double robot_force_[3] = { 0., 0., 0. }; double robot_torque_[3] = { 0., 0., 0. }; + double max_vel_change_; + // Robot API UrDriver* robot_; diff --git a/launch/ur10_ros_control.launch b/launch/ur10_ros_control.launch index f570298..d60d02f 100644 --- a/launch/ur10_ros_control.launch +++ b/launch/ur10_ros_control.launch @@ -29,13 +29,13 @@ + output="screen" args="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller" /> + output="screen" args="load pos_based_pos_traj_controller" /> - \ No newline at end of file + diff --git a/launch/ur5_ros_control.launch b/launch/ur5_ros_control.launch index b03a107..b81b017 100644 --- a/launch/ur5_ros_control.launch +++ b/launch/ur5_ros_control.launch @@ -30,11 +30,11 @@ + output="screen" args="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller" /> + output="screen" args="load pos_based_pos_traj_controller" /> diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp index 60dfadc..0222ede 100644 --- a/src/ur_hardware_interface.cpp +++ b/src/ur_hardware_interface.cpp @@ -57,6 +57,8 @@ UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : // Initialize shared memory and interfaces here init(); // this implementation loads from rosparam + max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2 + ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); } @@ -79,6 +81,7 @@ void UrHardwareInterface::init() { joint_effort_.resize(num_joints_); joint_position_command_.resize(num_joints_); joint_velocity_command_.resize(num_joints_); + prev_joint_velocity_command_.resize(num_joints_); // Initialize controller for (std::size_t i = 0; i < num_joints_; ++i) { @@ -102,6 +105,7 @@ void UrHardwareInterface::init() { hardware_interface::JointHandle( joint_state_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i])); + prev_joint_velocity_command_[i] = 0.; } // Create force torque interface @@ -135,11 +139,30 @@ void UrHardwareInterface::read() { } +void UrHardwareInterface::setMaxVelChange(double inp) { + max_vel_change_ = inp; +} + void UrHardwareInterface::write() { if (velocity_interface_running_) { - robot_->setSpeed(joint_velocity_command_[0], joint_velocity_command_[1], - joint_velocity_command_[2], joint_velocity_command_[3], - joint_velocity_command_[4], joint_velocity_command_[5], 100); + std::vector cmd; + //do some rate limiting + cmd.resize(joint_velocity_command_.size()); + for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) { + cmd[i] = joint_velocity_command_[i]; + if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) { + cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_; + } else if (cmd[i] + < prev_joint_velocity_command_[i] - max_vel_change_) { + cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_; + } + if (cmd[i] > M_PI/2.) + cmd[i] = M_PI/2.; + else if (cmd[i] < -M_PI/2.) + cmd[i] = -M_PI/2.; + prev_joint_velocity_command_[i] = cmd[i]; + } + robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_*125); } else if (position_interface_running_) { robot_->servoj(joint_position_command_); } diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 2035161..eabfdbd 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -129,6 +129,14 @@ public: } robot_.setServojTime(servoj_time); + double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2 + if (ros::param::get("~max_acceleration", max_vel_change)) { + max_vel_change = max_vel_change/125; + } + sprintf(buf, "Max acceleration set to: %f [rad/sec²]", max_vel_change*125); + print_debug(buf); + hardware_interface_->setMaxVelChange(max_vel_change); + if (robot_.start()) { if (use_ros_control_) { ros_control_thread_ = new std::thread(