Skip to content

Commit

Permalink
Added service to activate or deactivate spline interpolation
Browse files Browse the repository at this point in the history
  • Loading branch information
@urmahp
urmahp committed Jan 9, 2023
1 parent 7bd1b58 commit 2f21636
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 67 deletions.
6 changes: 4 additions & 2 deletions ur_robot_driver/include/ur_robot_driver/hardware_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <std_srvs/Trigger.h>
#include <std_srvs/SetBool.h>
#include <realtime_tools/realtime_publisher.h>
#include <tf2_msgs/TFMessage.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
Expand Down Expand Up @@ -213,6 +214,7 @@ class HardwareInterface : public hardware_interface::RobotHW
bool resendRobotProgram(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
bool zeroFTSensor(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
void commandCallback(const std_msgs::StringConstPtr& msg);
bool activateSplineInterpolation(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res);

std::unique_ptr<urcl::UrDriver> ur_driver_;
std::unique_ptr<DashboardClientROS> dashboard_client_;
Expand All @@ -227,8 +229,6 @@ class HardwareInterface : public hardware_interface::RobotHW

void startJointInterpolation(const hardware_interface::JointTrajectory& trajectory);

void startJointSplineInterpolation(const hardware_interface::JointTrajectory& trajectory);

void startCartesianInterpolation(const hardware_interface::CartesianTrajectory& trajectory);

void cancelInterpolation();
Expand All @@ -238,6 +238,7 @@ class HardwareInterface : public hardware_interface::RobotHW
ros::ServiceServer deactivate_srv_;
ros::ServiceServer tare_sensor_srv_;
ros::ServiceServer set_payload_srv_;
ros::ServiceServer activate_spline_interpolation_srv_;

hardware_interface::JointStateInterface js_interface_;
scaled_controllers::ScaledPositionJointInterface spj_interface_;
Expand Down Expand Up @@ -328,6 +329,7 @@ class HardwareInterface : public hardware_interface::RobotHW
bool cartesian_forward_controller_running_;
bool twist_controller_running_;
bool pose_controller_running_;
bool use_spline_interpolation_;

PausingState pausing_state_;
double pausing_ramp_up_increment_;
Expand Down
124 changes: 59 additions & 65 deletions ur_robot_driver/src/hardware_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,7 +172,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw

// True if splines should be used as interpolation on the robot controller when forwarding trajectory, if false movej
// or movel commands are used
bool use_spline_interpolation = robot_hw_nh.param<bool>("use_spline_interpolation", "false");
use_spline_interpolation_ = robot_hw_nh.param<bool>("use_spline_interpolation", "false");

// Whenever the runtime state of the "External Control" program node in the UR-program changes, a
// message gets published here. So this is equivalent to the information whether the robot accepts
Expand Down Expand Up @@ -362,16 +362,8 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
"industrial_robot_status_handle", robot_status_resource_));

// Register callbacks for trajectory passthrough
if (use_spline_interpolation)
{
jnt_traj_interface_.registerGoalCallback(
std::bind(&HardwareInterface::startJointSplineInterpolation, this, std::placeholders::_1));
}
else
{
jnt_traj_interface_.registerGoalCallback(
std::bind(&HardwareInterface::startJointInterpolation, this, std::placeholders::_1));
}
jnt_traj_interface_.registerGoalCallback(
std::bind(&HardwareInterface::startJointInterpolation, this, std::placeholders::_1));
jnt_traj_interface_.registerCancelCallback(std::bind(&HardwareInterface::cancelInterpolation, this));
cart_traj_interface_.registerGoalCallback(
std::bind(&HardwareInterface::startCartesianInterpolation, this, std::placeholders::_1));
Expand Down Expand Up @@ -453,6 +445,11 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
// work when the robot is in remote-control mode.
tare_sensor_srv_ = robot_hw_nh.advertiseService("zero_ftsensor", &HardwareInterface::zeroFTSensor, this);

// Call this to activate or deactivate using spline interpolation locally on the UR controller, when forwarding
// trajectories to the UR robot.
activate_spline_interpolation_srv_ = robot_hw_nh.advertiseService(
"activate_spline_interpolation", &HardwareInterface::activateSplineInterpolation, this);

ur_driver_->startRTDECommunication();
ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_robot_driver hardware_interface");

Expand Down Expand Up @@ -1174,6 +1171,21 @@ void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg)
}
}

bool HardwareInterface::activateSplineInterpolation(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res)
{
use_spline_interpolation_ = req.data;
if (use_spline_interpolation_)
{
res.message = "Activated spline interpolation in forward trajectory mode";
}
else
{
res.message = "Deactivated spline interpolation in forward trajectory mode";
}
res.success = true;
return true;
}

void HardwareInterface::publishRobotAndSafetyMode()
{
if (robot_mode_pub_)
Expand Down Expand Up @@ -1239,63 +1251,45 @@ void HardwareInterface::startJointInterpolation(const hardware_interface::JointT
p[4] = point.positions[4];
p[5] = point.positions[5];
double next_time = point.time_from_start.toSec();
ur_driver_->writeTrajectoryPoint(p, false, next_time - last_time);
last_time = next_time;
}
ROS_DEBUG("Finished Sending Trajectory");
}

void HardwareInterface::startJointSplineInterpolation(const hardware_interface::JointTrajectory& trajectory)
{
size_t point_number = trajectory.trajectory.points.size();
ROS_DEBUG("Starting joint-based trajectory forward using splines");
ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, point_number,
true);
double last_time = 0.0;
for (size_t i = 0; i < point_number; i++)
{
trajectory_msgs::JointTrajectoryPoint point = trajectory.trajectory.points[i];
urcl::vector6d_t p;
p[0] = point.positions[0];
p[1] = point.positions[1];
p[2] = point.positions[2];
p[3] = point.positions[3];
p[4] = point.positions[4];
p[5] = point.positions[5];
double next_time = point.time_from_start.toSec();

if (point.velocities.size() == 6 && point.accelerations.size() == 6)
{
urcl::vector6d_t v, a;
v[0] = point.velocities[0];
v[1] = point.velocities[1];
v[2] = point.velocities[2];
v[3] = point.velocities[3];
v[4] = point.velocities[4];
v[5] = point.velocities[5];

a[0] = point.accelerations[0];
a[1] = point.accelerations[1];
a[2] = point.accelerations[2];
a[3] = point.accelerations[3];
a[4] = point.accelerations[4];
a[5] = point.accelerations[5];
ur_driver_->writeSplinePoint(p, v, a, next_time - last_time);
}
else if (point.velocities.size() == 6)
if (!use_spline_interpolation_)
{
urcl::vector6d_t v;
v[0] = point.velocities[0];
v[1] = point.velocities[1];
v[2] = point.velocities[2];
v[3] = point.velocities[3];
v[4] = point.velocities[4];
v[5] = point.velocities[5];
ur_driver_->writeSplinePoint(p, v, next_time - last_time);
ur_driver_->writeTrajectoryPoint(p, false, next_time - last_time);
}
else
else // Use spline interpolation
{
ur_driver_->writeSplinePoint(p, next_time - last_time);
if (point.velocities.size() == 6 && point.accelerations.size() == 6)
{
urcl::vector6d_t v, a;
v[0] = point.velocities[0];
v[1] = point.velocities[1];
v[2] = point.velocities[2];
v[3] = point.velocities[3];
v[4] = point.velocities[4];
v[5] = point.velocities[5];

a[0] = point.accelerations[0];
a[1] = point.accelerations[1];
a[2] = point.accelerations[2];
a[3] = point.accelerations[3];
a[4] = point.accelerations[4];
a[5] = point.accelerations[5];
ur_driver_->writeTrajectorySplinePoint(p, v, a, next_time - last_time);
}
else if (point.velocities.size() == 6)
{
urcl::vector6d_t v;
v[0] = point.velocities[0];
v[1] = point.velocities[1];
v[2] = point.velocities[2];
v[3] = point.velocities[3];
v[4] = point.velocities[4];
v[5] = point.velocities[5];
ur_driver_->writeTrajectorySplinePoint(p, v, next_time - last_time);
}
else
{
ur_driver_->writeTrajectorySplinePoint(p, next_time - last_time);
}
}
last_time = next_time;
}
Expand Down

0 comments on commit 2f21636

Please sign in to comment.