Commit c1d6e781 authored by Felix Exner's avatar Felix Exner
Browse files

Added scaled vel traj controller

Do it for all robots
parent 6142793b
......@@ -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>
......@@ -100,4 +100,132 @@ private:
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
......@@ -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)
......@@ -61,7 +61,44 @@ pos_traj_controller:
state_publish_rate: *loop_hz
action_monitor_rate: 10
vel_based_pos_traj_controller:
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: 10.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 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:
......
......@@ -204,6 +204,7 @@ protected:
hardware_interface::PositionJointInterface pj_interface_;
ur_controllers::SpeedScalingInterface speedsc_interface_;
hardware_interface::VelocityJointInterface vj_interface_;
ur_controllers::ScaledVelocityJointInterface svj_interface_;
hardware_interface::ForceTorqueSensorInterface fts_interface_;
vector6d_t joint_position_command_;
......
<?xml version="1.0"?>
<launch>
<group ns="frank">
<arg name="robot_ip" default="169.254.198.38"/>
<param name="robot_ip" value="$(arg robot_ip)"/>
</group>
<group ns="klaus">
<arg name="robot_ip" default="169.254.198.35"/>
<param name="robot_ip" value="$(arg robot_ip)"/>
</group>
</launch>
......@@ -97,18 +97,19 @@ socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
control_mode = MODE_UNINITIALIZED
thread_move = 0
keepalive = -2
global keepalive = -2
textmsg("External control active")
params_mult = socket_read_binary_integer(1+6+1, "reverse_socket")
keepalive = params_mult[1]
while keepalive > 0 and control_mode > MODE_STOPPED:
enter_critical
socket_send_line(1, "reverse_socket")
params_mult = socket_read_binary_integer(1+6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
enter_critical
if params_mult[0] > 0:
keepalive = params_mult[1]
if control_mode != params_mult[8]:
control_mode = params_mult[8]
join thread_move
if control_mode == MODE_SERVOJ:
thread_move = run servoThread()
elif control_mode == MODE_SPEEDJ:
......
......@@ -287,6 +287,8 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i]));
spj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i], &speed_scaling_combined_));
svj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
js_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i], &speed_scaling_combined_));
}
speedsc_interface_.registerHandle(
......@@ -300,6 +302,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
registerInterface(&spj_interface_);
registerInterface(&pj_interface_);
registerInterface(&vj_interface_);
registerInterface(&svj_interface_);
registerInterface(&speedsc_interface_);
registerInterface(&fts_interface_);
......@@ -488,10 +491,6 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period
}
packet_read_ = false;
}
else
{
ur_driver_->stopControl();
}
}
bool HardwareInterface::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
......@@ -533,6 +532,10 @@ void HardwareInterface::doSwitch(const std::list<hardware_interface::ControllerI
{
position_controller_running_ = true;
}
if (resource_it.hardware_interface == "ur_controllers::ScaledVelocityJointInterface")
{
velocity_controller_running_ = true;
}
if (resource_it.hardware_interface == "hardware_interface::VelocityJointInterface")
{
velocity_controller_running_ = true;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment