diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index 13d7879..cec3fdf 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -15,20 +15,35 @@ #include #include #include "ur_realtime_communication.h" +#include +#include class UrDriver { private: double maximum_time_step_; + std::vector joint_names_; public: UrRealtimeCommunication* rt_interface_; UrDriver(std::condition_variable& msg_cond, std::string host, + std::vector joint_names, unsigned int safety_count_max = 12); void start(); void halt(); + void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.); + void addTraj(std::vector inp_timestamps, + std::vector > positions, + std::vector > velocities); + void stopTraj(); + std::vector interp_cubic(double t, double T, + std::vector p0_pos, std::vector p1_pos, + std::vector p0_vel, std::vector p1_vel); + + std::vector getJointNames(); + void setJointNames(std::vector jn); }; #endif /* UR_DRIVER_H_ */ diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index bd09f77..aa88f0c 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -12,12 +12,86 @@ #include "ur_modern_driver/ur_driver.h" UrDriver::UrDriver(std::condition_variable& msg_cond, std::string host, - unsigned int safety_count_max) { + std::vector joint_names, unsigned int safety_count_max) { rt_interface_ = new UrRealtimeCommunication(msg_cond, host, safety_count_max); + maximum_time_step_ = 2.; + joint_names_ = joint_names; } +std::vector UrDriver::interp_cubic(double t, double T, + std::vector p0_pos, std::vector p1_pos, + std::vector p0_vel, std::vector p1_vel) { + /*Returns positions of the joints at time 't' */ + std::vector positions; + for (unsigned int i = 0; i < p0_pos.size(); i++) { + double a = p0_pos[i]; + double b = p0_vel[i]; + double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] + - T * p1_vel[i]) / pow(T, 2); + double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] + + T * p1_vel[i]) / pow(T, 3); + positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3)); + } + return positions; +} + +void UrDriver::addTraj(std::vector inp_timestamps, + std::vector > inp_positions, + std::vector > inp_velocities) { + + std::vector timestamps; + std::vector > positions; + std::string command_string = "def traj():\n"; + + for (unsigned int i = 1; i < inp_timestamps.size(); i++) { + timestamps.push_back(inp_timestamps[i - 1]); + double dt = inp_timestamps[i] - inp_timestamps[i - 1]; + unsigned int steps = (unsigned int) ceil(dt / maximum_time_step_); + double step_size = dt / steps; + for (unsigned int j = 1; j < steps; j++) { + timestamps.push_back(inp_timestamps[i - 1] + step_size * j); + } + } + //make sure we come to a smooth stop + /*while (timestamps.back() < inp_timestamps.back()) { + timestamps.push_back(timestamps.back() + 0.008); + } + timestamps.pop_back(); + */ + unsigned int j = 0; + for (unsigned int i = 0; i < timestamps.size(); i++) { + while (inp_timestamps[j] <= timestamps[i]) { + j += 1; + } + positions.push_back( + UrDriver::interp_cubic(timestamps[i], inp_timestamps[j], + inp_positions[j - 1], inp_positions[j], + inp_velocities[j - 1], inp_velocities[j])); + } + + timestamps.push_back(inp_timestamps[inp_timestamps.size() - 1]); + positions.push_back(inp_positions[inp_positions.size() - 1]); + for (unsigned int i = 1; i < timestamps.size(); i++) { + char buf[128]; + sprintf(buf, + "\tservoj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], t=%1.5f)\n", + positions[i][0], positions[i][1], positions[i][2], + positions[i][3], positions[i][4], positions[i][5], + timestamps[i] - timestamps[i - 1]); + command_string += buf; + } + command_string += "end\ntraj()\n"; + //printf("%s", command_string.c_str()); + rt_interface_->addCommandToQueue(command_string); + +} + +void UrDriver::stopTraj() { + rt_interface_->addCommandToQueue("stopj(10)\n"); +} + void UrDriver::start() { rt_interface_->start(); } @@ -31,3 +105,10 @@ void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4, rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc); } +std::vector UrDriver::getJointNames() { + return joint_names_; +} + +void UrDriver::setJointNames(std::vector jn) { + joint_names_ = jn; +} diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 99edfad..31da4b3 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -35,7 +35,7 @@ UrRealtimeCommunication::UrRealtimeCommunication( setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); connected_ = false; keepalive_ = false; - safety_count_ = 0; + safety_count_ = safety_count_max+1; safety_count_max_ = safety_count_max; } diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 9fac419..3fd745d 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -16,19 +16,146 @@ #include #include #include +#include + #include "sensor_msgs/JointState.h" #include "geometry_msgs/WrenchStamped.h" -#include +#include "control_msgs/FollowJointTrajectoryAction.h" +#include "actionlib/server/simple_action_server.h" +#include "trajectory_msgs/JointTrajectoryPoint.h" std::condition_variable g_msg_cond; -void publishRTMsg(UrDriver robot, std::vector joint_names) { - ros::NodeHandle n_rt; - ros::Publisher joint_pub = n_rt.advertise("/joint_states", 1); - ros::Publisher wrench_pub = n_rt.advertise("/wrench", 1); +class URTrajectoryFollower { +protected: + ros::NodeHandle nh_; + actionlib::SimpleActionServer as_; + std::string action_name_; + actionlib::SimpleActionServer::Goal goal_; + control_msgs::FollowJointTrajectoryFeedback feedback_; + control_msgs::FollowJointTrajectoryResult result_; + ros::Subscriber sub_; + UrDriver* robot_; +public: + URTrajectoryFollower(UrDriver* robot, std::string name) : + as_(nh_, "follow_joint_trajectory", false), action_name_(name) { + robot_ = robot; + //register the goal and feeback callbacks + as_.registerGoalCallback( + boost::bind(&URTrajectoryFollower::goalCB, this)); + as_.registerPreemptCallback( + boost::bind(&URTrajectoryFollower::preemptCB, this)); + + //subscribe to the data topic of interest + as_.start(); + ROS_INFO("The action server for this driver has been started"); + } + void goalCB() { + ROS_INFO("on_goal"); + + actionlib::SimpleActionServer::GoalConstPtr goal = + as_.acceptNewGoal(); + goal_ = *goal; //make a copy that we can modify + if (!validateJointNames()) { + std::string outp_joint_names = ""; + for (unsigned int i = 0; i < goal_.trajectory.joint_names.size(); + i++) { + outp_joint_names += goal_.trajectory.joint_names[i] + " "; + } + ROS_ERROR("Received a goal with incorrect joint names: %s", + outp_joint_names.c_str()); + result_.error_code = result_.INVALID_JOINTS; + /*result_.error_string = + "Received a goal with incorrect joint names: %s", outp_joint_names.c_str(); */ + as_.setAborted(result_, ("Received a goal with incorrect joint names: %s", outp_joint_names.c_str())); + } + + reorder_traj_joints(); + std::vector timestamps; + std::vector > positions, velocities; + for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) { + timestamps.push_back(goal_.trajectory.points[i].time_from_start.toSec()); + positions.push_back(goal_.trajectory.points[i].positions); + velocities.push_back(goal_.trajectory.points[i].velocities); + + } + robot_->addTraj(timestamps, positions, velocities); + + //sleep? + result_.error_code = result_.SUCCESSFUL; + as_.setSucceeded(result_); + + } + + void preemptCB() { + ROS_INFO("%s: Preempted", action_name_.c_str()); + // set the action state to preempted + robot_->stopTraj(); + as_.setPreempted(); + } +private: + bool validateJointNames() { + std::vector actual_joint_names = robot_->getJointNames(); + if (goal_.trajectory.joint_names.size() != actual_joint_names.size()) + return false; + + for (unsigned int i = 0; i < goal_.trajectory.joint_names.size(); i++) { + unsigned int j; + for (j = 0; j < actual_joint_names.size(); j++) { + if (goal_.trajectory.joint_names[i] == actual_joint_names[j]) + break; + } + if (goal_.trajectory.joint_names[i] == actual_joint_names[j]) { + actual_joint_names.erase(actual_joint_names.begin() + j); + } else { + return false; + } + } + + return true; + } + + void reorder_traj_joints() { + std::vector actual_joint_names = robot_->getJointNames(); + std::vector mapping; + mapping.resize(actual_joint_names.size(), actual_joint_names.size()); + for (unsigned int i = 0; i < goal_.trajectory.joint_names.size(); i++) { + for (unsigned int j = 0; j < actual_joint_names.size(); j++) { + if (goal_.trajectory.joint_names[i] == actual_joint_names[j]) + mapping[j] = i; + } + } + goal_.trajectory.joint_names = actual_joint_names; + std::vector new_traj; + for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) { + trajectory_msgs::JointTrajectoryPoint new_point; + for (unsigned int j = 0; + j < goal_.trajectory.points[i].positions.size(); j++) { + new_point.positions.push_back( + goal_.trajectory.points[i].positions[mapping[j]]); + new_point.velocities.push_back( + goal_.trajectory.points[i].velocities[mapping[j]]); + new_point.accelerations.push_back( + goal_.trajectory.points[i].accelerations[mapping[j]]); + } + new_point.time_from_start = + goal_.trajectory.points[i].time_from_start; + new_traj.push_back(new_point); + } + goal_.trajectory.points = new_traj; + } + +}; + +void publishRTMsg(UrDriver robot) { + ros::NodeHandle nh_rt; + ros::Publisher joint_pub = nh_rt.advertise( + "/joint_states", 1); + ros::Publisher wrench_pub = nh_rt.advertise( + "/wrench", 1); while (ros::ok()) { sensor_msgs::JointState joint_msg; - joint_msg.name = joint_names; + joint_msg.name = robot.getJointNames(); geometry_msgs::WrenchStamped wrench_msg; std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex std::unique_lock locker(msg_lock); @@ -40,7 +167,8 @@ void publishRTMsg(UrDriver robot, std::vector joint_names) { joint_msg.velocity = robot.rt_interface_->robot_state_->getQdActual(); joint_msg.effort = robot.rt_interface_->robot_state_->getIActual(); joint_pub.publish(joint_msg); - std::vector tcp_force = robot.rt_interface_->robot_state_->getTcpForce(); + std::vector tcp_force = + robot.rt_interface_->robot_state_->getTcpForce(); wrench_msg.header.stamp = joint_msg.header.stamp; wrench_msg.wrench.force.x = tcp_force[0]; wrench_msg.wrench.force.y = tcp_force[1]; @@ -57,14 +185,12 @@ void publishRTMsg(UrDriver robot, std::vector joint_names) { int main(int argc, char **argv) { bool use_sim_time = false; - std::string joint_prefix =""; + std::string joint_prefix = ""; std::string host; std::vector joint_names; - UrDriver robot(g_msg_cond, host); - ros::init(argc, argv, "ur_driver"); - ros::NodeHandle n; + ros::NodeHandle nh; if (ros::param::get("use_sim_time", use_sim_time)) { ROS_WARN("use_sim_time is set!!"); } @@ -72,7 +198,7 @@ int main(int argc, char **argv) { ROS_INFO("Setting prefix to %s", joint_prefix.c_str()); } joint_names.push_back(joint_prefix + "shoulder_pan_joint"); - joint_names.push_back(joint_prefix + "should_lift_joint"); + joint_names.push_back(joint_prefix + "shoulder_lift_joint"); joint_names.push_back(joint_prefix + "elbow_joint"); joint_names.push_back(joint_prefix + "wrist_1_joint"); joint_names.push_back(joint_prefix + "wrist_2_joint"); @@ -81,12 +207,17 @@ int main(int argc, char **argv) { if (argc > 1) { host = argv[1]; } else if (!(ros::param::get("~robot_ip", host))) { - ROS_FATAL("Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip"); + ROS_FATAL( + "Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip"); exit(1); } + UrDriver robot(g_msg_cond, host, joint_names); + robot.start(); - std::thread rt_publish_thread(publishRTMsg, robot, joint_names); + std::thread rt_publish_thread(publishRTMsg, robot); + URTrajectoryFollower action_server(&robot, ros::this_node::getName()); + ros::spin(); robot.halt(); exit(0);