Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 18:10:47 +02:00
Code Releases Activity

Merge branch 'speed_rate_limit'

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-10-08 15:38:50 +02:00
parent f592d2189a 589dca8e2a
commit 8c9595bbf2
8 changed files with 108 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
README.md
View File

@@ -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

55
config/ur10_controllers.yaml
View File

@@ -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

18
config/ur5_controllers.yaml
View File

@@ -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

6
include/ur_modern_driver/ur_hardware_interface.h
View File

@@ -58,6 +58,7 @@
#include <controller_manager/controller_manager.h>
#include <boost/scoped_ptr.hpp>
#include <ros/ros.h>
#include <math.h>
#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<hardware_interface::ControllerInfo> &start_list,
const std::list<hardware_interface::ControllerInfo> &stop_list) const;
@@ -111,10 +114,13 @@ protected:
std::vector<double> joint_effort_;
std::vector<double> joint_position_command_;
std::vector<double> joint_velocity_command_;
std::vector<double> 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_;

4
launch/ur10_ros_control.launch
View File

@@ -29,11 +29,11 @@
<!-- spawn controller manager -->
<node name="ros_control_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args="joint_state_controller force_torque_sensor_controller velocity_based_position_trajectory_controller" />
output="screen" args="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller" />
<!-- load other controller -->
<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
output="screen" args="load position_based_position_trajectory_controller" />
output="screen" args="load pos_based_pos_traj_controller" />
<!-- Convert joint states to /tf tranforms -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

4
launch/ur5_ros_control.launch
View File

@@ -30,11 +30,11 @@
<!-- spawn controller manager -->
<node name="ros_control_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args="joint_state_controller force_torque_sensor_controller velocity_based_position_trajectory_controller" />
output="screen" args="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller" />
<!-- load other controller -->
<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
output="screen" args="load position_based_position_trajectory_controller" />
output="screen" args="load pos_based_pos_traj_controller" />
<!-- Convert joint states to /tf tranforms -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

29
src/ur_hardware_interface.cpp
View File

@@ -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<double> 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_);
}

8
src/ur_ros_wrapper.cpp
View File

@@ -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(