Skip to content

Commit

Permalink
Merge pull request #1 from UniversalRobots/velocity_interface
Browse files Browse the repository at this point in the history 
Adds a velocity interface to the driver.
  • Loading branch information
@fmauch
fmauch authored Feb 5, 2020
2 parents acfffd4 + 00e8994 commit 6f0f2c2
Show file tree
Hide file tree
Showing 26 changed files with 784 additions and 49 deletions.
7 changes: 6 additions & 1 deletion ur_controllers/controller_plugins.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,12 @@
</class>
<class name="position_controllers/ScaledJointTrajectoryController" type="position_controllers::ScaledJointTrajectoryController" base_class_type="controller_interface::ControllerBase">
<description>
Scaled joint trajectory controller
Scaled velocity-based joint trajectory controller
</description>
</class>
<class name="velocity_controllers/ScaledJointTrajectoryController" type="velocity_controllers::ScaledJointTrajectoryController" base_class_type="controller_interface::ControllerBase">
<description>
Scaled velocity-based joint trajectory controller
</description>
</class>
</library>
128 changes: 128 additions & 0 deletions ur_controllers/include/ur_controllers/hardware_interface_adapter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,4 +100,132 @@ class HardwareInterfaceAdapter<ur_controllers::ScaledPositionJointInterface, Sta
std::vector<ur_controllers::ScaledJointHandle>* joint_handles_ptr_;
};

namespace ur_controllers
{
/**
* \brief Helper base class template for closed loop HardwareInterfaceAdapter implementations.
*
* Adapters leveraging (specializing) this class will generate a command given the desired state and state error using a
* velocity feedforward term plus a corrective PID term.
*
* Use one of the available template specializations of this class (or create your own) to adapt the
* ScaledJointTrajectoryController to a specific hardware interface.
*/
template <class State>
class ClosedLoopHardwareInterfaceAdapter
{
public:
ClosedLoopHardwareInterfaceAdapter() : joint_handles_ptr_(0)
{
}

bool init(std::vector<ur_controllers::ScaledJointHandle>& joint_handles, ros::NodeHandle& controller_nh)
{
// Store pointer to joint handles
joint_handles_ptr_ = &joint_handles;

// Initialize PIDs
pids_.resize(joint_handles.size());
for (unsigned int i = 0; i < pids_.size(); ++i)
{
// Node handle to PID gains
ros::NodeHandle joint_nh(controller_nh, std::string("gains/") + joint_handles[i].getName());

// Init PID gains from ROS parameter server
pids_[i].reset(new control_toolbox::Pid());
if (!pids_[i]->init(joint_nh))
{
ROS_WARN_STREAM("Failed to initialize PID gains from ROS parameter server.");
return false;
}
}

// Load velocity feedforward gains from parameter server
velocity_ff_.resize(joint_handles.size());
for (unsigned int i = 0; i < velocity_ff_.size(); ++i)
{
controller_nh.param(std::string("velocity_ff/") + joint_handles[i].getName(), velocity_ff_[i], 0.0);
}

return true;
}

void starting(const ros::Time& /*time*/)
{
if (!joint_handles_ptr_)
{
return;
}

// Reset PIDs, zero commands
for (unsigned int i = 0; i < pids_.size(); ++i)
{
pids_[i]->reset();
(*joint_handles_ptr_)[i].setCommand(0.0);
}
}

void stopping(const ros::Time& /*time*/)
{
}

void updateCommand(const ros::Time& /*time*/, const ros::Duration& period, const State& desired_state,
const State& state_error)
{
const unsigned int n_joints = joint_handles_ptr_->size();

// Preconditions
if (!joint_handles_ptr_)
return;
assert(n_joints == state_error.position.size());
assert(n_joints == state_error.velocity.size());

// Update PIDs
for (unsigned int i = 0; i < n_joints; ++i)
{
const double command = (desired_state.velocity[i] * velocity_ff_[i]) +
pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);
(*joint_handles_ptr_)[i].setCommand(command);
}
}

private:
typedef std::shared_ptr<control_toolbox::Pid> PidPtr;
std::vector<PidPtr> pids_;

std::vector<double> velocity_ff_;

std::vector<ur_controllers::ScaledJointHandle>* joint_handles_ptr_;
};
} // namespace ur_controllers

/**
* \brief Adapter for an velocity-controlled hardware interface. Maps position and velocity errors to velocity commands
* through a velocity PID loop.
*
* The following is an example configuration of a controller that uses this adapter. Notice the \p gains and \p
* velocity_ff entries: \code head_controller: type: "velocity_controllers/ScaledJointTrajectoryController" joints:
* - head_1_joint
* - head_2_joint
* gains:
* head_1_joint: {p: 200, d: 1, i: 5, i_clamp: 1}
* head_2_joint: {p: 200, d: 1, i: 5, i_clamp: 1}
* velocity_ff:
* head_1_joint: 1.0
* head_2_joint: 1.0
* constraints:
* goal_time: 0.6
* stopped_velocity_tolerance: 0.02
* head_1_joint: {trajectory: 0.05, goal: 0.02}
* head_2_joint: {trajectory: 0.05, goal: 0.02}
* stop_trajectory_duration: 0.5
* state_publish_rate: 25
* \endcode
*/
template <class State>
class HardwareInterfaceAdapter<ur_controllers::ScaledVelocityJointInterface, State>
: public ur_controllers::ClosedLoopHardwareInterfaceAdapter<State>
{
};

#endif // ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED
8 changes: 8 additions & 0 deletions ur_controllers/src/scaled_joint_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,4 +38,12 @@ typedef ur_controllers::ScaledJointTrajectoryController<trajectory_interface::Qu
ScaledJointTrajectoryController;
}

namespace velocity_controllers
{
typedef ur_controllers::ScaledJointTrajectoryController<trajectory_interface::QuinticSplineSegment<double>,
ur_controllers::ScaledVelocityJointInterface>
ScaledJointTrajectoryController;
}

PLUGINLIB_EXPORT_CLASS(position_controllers::ScaledJointTrajectoryController, controller_interface::ControllerBase)
PLUGINLIB_EXPORT_CLASS(velocity_controllers::ScaledJointTrajectoryController, controller_interface::ControllerBase)
79 changes: 79 additions & 0 deletions ur_robot_driver/config/ur10_controllers.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,3 +60,82 @@ pos_traj_controller:
stop_trajectory_duration: 0.5
state_publish_rate: *loop_hz
action_monitor_rate: 10

scaled_vel_traj_controller:
type: velocity_controllers/ScaledJointTrajectoryController
joints: *robot_joints
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: *loop_hz
action_monitor_rate: 10
gains:
#!!These values have not been optimized!!
shoulder_pan_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
shoulder_lift_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
elbow_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_1_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_2_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_3_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}

# Use a feedforward term to reduce the size of PID gains
velocity_ff:
shoulder_pan_joint: 1.0
shoulder_lift_joint: 1.0
elbow_joint: 1.0
wrist_1_joint: 1.0
wrist_2_joint: 1.0
wrist_3_joint: 1.0

# state_publish_rate: 50 # Defaults to 50
# action_monitor_rate: 20 # Defaults to 20
#stop_trajectory_duration: 0 # Defaults to 0.0

vel_traj_controller:
type: velocity_controllers/JointTrajectoryController
joints: *robot_joints
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: *loop_hz
action_monitor_rate: 10
gains:
#!!These values have not been optimized!!
shoulder_pan_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
shoulder_lift_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
elbow_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_1_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_2_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_3_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}

# Use a feedforward term to reduce the size of PID gains
velocity_ff:
shoulder_pan_joint: 1.0
shoulder_lift_joint: 1.0
elbow_joint: 1.0
wrist_1_joint: 1.0
wrist_2_joint: 1.0
wrist_3_joint: 1.0

# state_publish_rate: 50 # Defaults to 50
# action_monitor_rate: 20 # Defaults to 20
#stop_trajectory_duration: 0 # Defaults to 0.0

# Pass an array of joint velocities directly to the joints
joint_group_vel_controller:
type: velocity_controllers/JointGroupVelocityController
joints: *robot_joints
79 changes: 79 additions & 0 deletions ur_robot_driver/config/ur10e_controllers.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,3 +60,82 @@ pos_traj_controller:
stop_trajectory_duration: 0.5
state_publish_rate: *loop_hz
action_monitor_rate: 10

scaled_vel_traj_controller:
type: velocity_controllers/ScaledJointTrajectoryController
joints: *robot_joints
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: *loop_hz
action_monitor_rate: 10
gains:
#!!These values have not been optimized!!
shoulder_pan_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
shoulder_lift_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
elbow_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_1_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_2_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_3_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}

# Use a feedforward term to reduce the size of PID gains
velocity_ff:
shoulder_pan_joint: 1.0
shoulder_lift_joint: 1.0
elbow_joint: 1.0
wrist_1_joint: 1.0
wrist_2_joint: 1.0
wrist_3_joint: 1.0

# state_publish_rate: 50 # Defaults to 50
# action_monitor_rate: 20 # Defaults to 20
#stop_trajectory_duration: 0 # Defaults to 0.0

vel_traj_controller:
type: velocity_controllers/JointTrajectoryController
joints: *robot_joints
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: *loop_hz
action_monitor_rate: 10
gains:
#!!These values have not been optimized!!
shoulder_pan_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
shoulder_lift_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
elbow_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_1_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_2_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_3_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}

# Use a feedforward term to reduce the size of PID gains
velocity_ff:
shoulder_pan_joint: 1.0
shoulder_lift_joint: 1.0
elbow_joint: 1.0
wrist_1_joint: 1.0
wrist_2_joint: 1.0
wrist_3_joint: 1.0

# state_publish_rate: 50 # Defaults to 50
# action_monitor_rate: 20 # Defaults to 20
#stop_trajectory_duration: 0 # Defaults to 0.0

# Pass an array of joint velocities directly to the joints
joint_group_vel_controller:
type: velocity_controllers/JointGroupVelocityController
joints: *robot_joints
Loading

2 comments on commit 6f0f2c2

@kavonszadkowski
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm just setting up a UR5 with ROS and was wondering: Does this commit mean line 3 of the feature list needs updating?
I'm not yet sure if I may need to use velocity control in my current project, so I was considering falling back on ur_modern_driver due to that limitation...

@fmauch
Copy link
Collaborator Author

@fmauch fmauch commented on 6f0f2c2 Feb 26, 2020

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes indeed! Thank you for the reminder!

Please sign in to comment.