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

Merge branch 'multiple_controllers'

Browse Source 
Adds support for standard position based controllers and not only our
own ur_controllers.
This commit is contained in:
Felix Mauch
2019-06-27 14:53:02 +02:00
parent 72330cb98b a6a06526ac
commit 053da5c95f
21 changed files with 242 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
ur_controllers/include/ur_controllers/scaled_joint_command_interface.h
View File

@@ -44,28 +44,26 @@
#include <hardware_interface/internal/hardware_resource_manager.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
namespace ur_controllers
{
class ScaledJointHandle : public hardware_interface::JointStateHandle
class ScaledJointHandle : public hardware_interface::JointHandle
{
public:
ScaledJointHandle() : cmd_(0), scaling_factor_(0)
ScaledJointHandle() : hardware_interface::JointHandle(), scaling_factor_(0)
{
}
/**
* \param js This joint's state handle
* \param cmd A pointer to the storage for this joint's output command
* \param scaling_factor A pointer to the storage for this joint's scaling factor
*/
ScaledJointHandle(const hardware_interface::JointStateHandle& js, double* cmd, double* scaling_factor)
: hardware_interface::JointStateHandle(js), cmd_(cmd), scaling_factor_(scaling_factor)
: hardware_interface::JointHandle(js, cmd), scaling_factor_(scaling_factor)
{
if (cmd_ == nullptr)
{
throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() +
"'. Command data pointer is null.");
}
if (scaling_factor_ == nullptr)
{
throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() +
@@ -75,16 +73,6 @@ public:
virtual ~ScaledJointHandle() = default;
void setCommand(double command)
{
assert(cmd_);
*cmd_ = command;
}
double getCommand() const
{
assert(cmd_);
return *cmd_;
}
void setScalingFactor(double scaling_factor)
{
assert(scaling_factor_);
@@ -97,7 +85,6 @@ public:
}
private:
double* cmd_;
double* scaling_factor_;
};

24
ur_rtde_driver/config/ur10_controllers.yaml
View File

@@ -29,7 +29,7 @@ speed_scaling_state_controller:
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
pos_based_pos_traj_controller:
scaled_pos_traj_controller:
type: position_controllers/ScaledJointTrajectoryController
joints:
- shoulder_pan_joint
@@ -50,3 +50,25 @@ pos_based_pos_traj_controller:
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10
pos_traj_controller:
type: position_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.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10

24
ur_rtde_driver/config/ur10e_controllers.yaml
View File

@@ -29,7 +29,7 @@ speed_scaling_state_controller:
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
pos_based_pos_traj_controller:
scaled_pos_traj_controller:
type: position_controllers/ScaledJointTrajectoryController
joints:
- shoulder_pan_joint
@@ -50,3 +50,25 @@ pos_based_pos_traj_controller:
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10
pos_traj_controller:
type: position_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.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10

24
ur_rtde_driver/config/ur3_controllers.yaml
View File

@@ -29,7 +29,7 @@ speed_scaling_state_controller:
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
pos_based_pos_traj_controller:
scaled_pos_traj_controller:
type: position_controllers/ScaledJointTrajectoryController
joints:
- shoulder_pan_joint
@@ -50,3 +50,25 @@ pos_based_pos_traj_controller:
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10
pos_traj_controller:
type: position_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.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10

24
ur_rtde_driver/config/ur3e_controllers.yaml
View File

@@ -29,7 +29,7 @@ speed_scaling_state_controller:
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
pos_based_pos_traj_controller:
scaled_pos_traj_controller:
type: position_controllers/ScaledJointTrajectoryController
joints:
- shoulder_pan_joint
@@ -50,3 +50,25 @@ pos_based_pos_traj_controller:
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10
pos_traj_controller:
type: position_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.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10

24
ur_rtde_driver/config/ur5_controllers.yaml
View File

@@ -29,7 +29,7 @@ speed_scaling_state_controller:
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
pos_based_pos_traj_controller:
scaled_pos_traj_controller:
type: position_controllers/ScaledJointTrajectoryController
joints:
- shoulder_pan_joint
@@ -50,3 +50,25 @@ pos_based_pos_traj_controller:
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10
pos_traj_controller:
type: position_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.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10

24
ur_rtde_driver/config/ur5e_controllers.yaml
View File

@@ -29,7 +29,7 @@ speed_scaling_state_controller:
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
pos_based_pos_traj_controller:
scaled_pos_traj_controller:
type: position_controllers/ScaledJointTrajectoryController
joints:
- shoulder_pan_joint
@@ -50,3 +50,25 @@ pos_based_pos_traj_controller:
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10
pos_traj_controller:
type: position_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.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10

23
ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h
View File

@@ -57,20 +57,23 @@ public:
server_.disconnectClient();
}
bool write(const vector6d_t& positions)
bool write(const vector6d_t* positions, const int32_t type = 2)
{
uint8_t buffer[sizeof(uint32_t) * 7];
uint8_t* b_pos = buffer;
for (auto const& pos : positions)
{
int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE);
val = htobe32(val);
b_pos += append(b_pos, val);
}
// TODO: Make this a parameter: Number of allowed missed packages
int32_t val = htobe32(5);
append(b_pos, val);
int32_t val = htobe32(type);
b_pos += append(b_pos, val);
if (positions != nullptr)
{
for (auto const& pos : *positions)
{
int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE);
val = htobe32(val);
b_pos += append(b_pos, val);
}
}
size_t written;

1
ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
View File

@@ -133,6 +133,7 @@ protected:
ros::Publisher program_state_pub_;
bool controller_reset_necessary_;
bool controllers_initialized_;
std::string robot_ip_;
};

18
ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h
View File

@@ -81,8 +81,26 @@ public:
return rtde_frequency_;
}
/*!
* \brief Writes a joint command together with a keepalive signal onto the socket being sent to
* the robot.
*
* \param values Desired joint positions
*
* \returns True on successful write.
*/
bool writeJointCommand(const vector6d_t& values);
/*!
* \brief Write a keepalive signal only.
*
* This signals the robot that the connection is still
* active in times when no commands are to be sent (e.g. no controller is active.)
*
* \returns True on successful write.
*/
bool writeKeepalive();
void startWatchdog();
private:

4
ur_rtde_driver/launch/ur10_bringup.launch
View File

@@ -10,8 +10,8 @@
<arg name="robot_ip"/>
<arg name="limited" default="false"/>
<arg name="tf_prefix" default="" />
<arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default=""/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default="pos_traj_controller"/>
<include file="$(find ur_rtde_driver)/launch/ur_common.launch">
<arg name="debug" value="$(arg debug)"/>

4
ur_rtde_driver/launch/ur10e_bringup.launch
View File

@@ -11,8 +11,8 @@
<arg name="robot_ip"/>
<arg name="limited" default="false"/>
<arg name="tf_prefix" default="" />
<arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default=""/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default="pos_traj_controller"/>
<arg name="tool_voltage" default="12"/>
<arg name="tool_parity" default="0"/>
<arg name="tool_baud_rate" default="115200"/>

4
ur_rtde_driver/launch/ur3_bringup.launch
View File

@@ -10,8 +10,8 @@
<arg name="robot_ip"/>
<arg name="limited" default="false"/>
<arg name="tf_prefix" default="" />
<arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default=""/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default="pos_traj_controller"/>
<include file="$(find ur_rtde_driver)/launch/ur_common.launch">
<arg name="debug" value="$(arg debug)"/>

4
ur_rtde_driver/launch/ur3e_bringup.launch
View File

@@ -11,8 +11,8 @@
<arg name="robot_ip"/>
<arg name="limited" default="false"/>
<arg name="tf_prefix" default="" />
<arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default=""/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default="pos_traj_controller"/>
<arg name="tool_voltage" default="12"/>
<arg name="tool_parity" default="0"/>
<arg name="tool_baud_rate" default="115200"/>

4
ur_rtde_driver/launch/ur5_bringup.launch
View File

@@ -10,8 +10,8 @@
<arg name="robot_ip"/>
<arg name="limited" default="false"/>
<arg name="tf_prefix" default="" />
<arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default=""/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default="pos_traj_controller"/>
<include file="$(find ur_rtde_driver)/launch/ur_common.launch">
<arg name="debug" value="$(arg debug)"/>

4
ur_rtde_driver/launch/ur5e_bringup.launch
View File

@@ -11,8 +11,8 @@
<arg name="robot_ip"/>
<arg name="limited" default="false"/>
<arg name="tf_prefix" default="" />
<arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default=""/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default="pos_traj_controller"/>
<arg name="tool_voltage" default="12"/>
<arg name="tool_parity" default="0"/>
<arg name="tool_baud_rate" default="115200"/>

4
ur_rtde_driver/launch/ur_common.launch
View File

@@ -8,8 +8,8 @@
<arg name="robot_ip"/>
<arg name="limited" default="false"/>
<arg name="tf_prefix" default="" />
<arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default=""/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default="pos_traj_controller"/>
<arg name="tool_voltage" default="12"/>
<arg name="tool_parity" default="0"/>
<arg name="tool_baud_rate" default="115200"/>

4
ur_rtde_driver/launch/ur_control.launch
View File

@@ -11,8 +11,8 @@
<arg name="robot_ip"/>
<arg name="kinematics_config"/>
<arg name="tf_prefix" default="" />
<arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default=""/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default="pos_traj_controller"/>
<arg name="urscript_file" default="$(find ur_rtde_driver)/resources/servoj.urscript"/>
<arg name="rtde_recipe_file" default="$(find ur_rtde_driver)/resources/rtde_recipe.txt"/>
<arg name="tool_voltage" default="12"/>

16
ur_rtde_driver/resources/servoj.urscript
View File

@@ -71,16 +71,20 @@ socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
thread_servo = run servoThread()
keepalive = -2
params_mult = socket_read_binary_integer(6+1, "reverse_socket")
keepalive = params_mult[7]
params_mult = socket_read_binary_integer(1+6, "reverse_socket")
keepalive = params_mult[1]
while keepalive > 0:
enter_critical
socket_send_line(1, "reverse_socket")
params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
params_mult = socket_read_binary_integer(1+6, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
if params_mult[0] > 0:
keepalive = params_mult[7]
q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate]
set_servo_setpoint(q)
if params_mult[1] > 1:
keepalive = params_mult[1]
q = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
set_servo_setpoint(q)
else:
keepalive = 1
end
else:
keepalive = keepalive - 1
end

34
ur_rtde_driver/src/ros/hardware_interface.cpp
View File

@@ -47,6 +47,7 @@ HardwareInterface::HardwareInterface()
, position_controller_running_(false)
, pausing_state_(PausingState::RUNNING)
, pausing_ramp_up_increment_(0.01)
, controllers_initialized_(false)
{
}
@@ -171,8 +172,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
&joint_velocities_[i], &joint_efforts_[i]));
// Create joint position control interface
// pj_interface_.registerHandle(
// hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
pj_interface_.registerHandle(
hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
spj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i], &speed_scaling_combined_));
}
@@ -186,7 +187,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
// Register interfaces
registerInterface(&js_interface_);
registerInterface(&spj_interface_);
// registerInterface(&pj_interface_);
registerInterface(&pj_interface_);
registerInterface(&speedsc_interface_);
registerInterface(&fts_interface_);
@@ -292,13 +293,32 @@ void HardwareInterface ::write(const ros::Time& time, const ros::Duration& perio
{
ur_driver_->writeJointCommand(joint_position_command_);
}
else
{
ur_driver_->writeKeepalive();
}
}
bool HardwareInterface ::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
// TODO: Implement
return true;
bool ret_val = true;
if (controllers_initialized_ && !isRobotProgramRunning() && !start_list.empty())
{
for (auto& controller : start_list)
{
if (!controller.claimed_resources.empty())
{
ROS_ERROR_STREAM("Robot control is currently inactive. Starting controllers that claim resources is currently "
"not possible. Not starting controller '"
<< controller.name << "'");
ret_val = false;
}
}
}
controllers_initialized_ = true;
return ret_val;
}
void HardwareInterface ::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
@@ -313,6 +333,10 @@ void HardwareInterface ::doSwitch(const std::list<hardware_interface::Controller
{
position_controller_running_ = true;
}
if (resource_it.hardware_interface == "hardware_interface::PositionJointInterface")
{
position_controller_running_ = true;
}
}
}
}

17
ur_rtde_driver/src/ur/ur_driver.cpp
View File

@@ -138,14 +138,19 @@ bool UrDriver::writeJointCommand(const vector6d_t& values)
{
if (reverse_interface_active_)
{
reverse_interface_->write(values);
}
else
{
return false;
return reverse_interface_->write(&values);
}
return false;
}
return true;
bool UrDriver::writeKeepalive()
{
if (reverse_interface_active_)
{
vector6d_t* fake = nullptr;
return reverse_interface_->write(fake, 1);
}
return false;
}
void UrDriver::startWatchdog()