From 834b1031294305fafd50fd0e47a6ef2cbd11fd78 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Mon, 16 Nov 2015 10:21:50 +0100 Subject: [PATCH 01/19] Added launchfiles used for imitating old python driver --- launch/ur10_bringup_compatible.launch | 30 +++++++++++++++++++++++++++ launch/ur5_bringup_compatible.launch | 30 +++++++++++++++++++++++++++ 2 files changed, 60 insertions(+) create mode 100644 launch/ur10_bringup_compatible.launch create mode 100644 launch/ur5_bringup_compatible.launch diff --git a/launch/ur10_bringup_compatible.launch b/launch/ur10_bringup_compatible.launch new file mode 100644 index 0000000..436cb5b --- /dev/null +++ b/launch/ur10_bringup_compatible.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/ur5_bringup_compatible.launch b/launch/ur5_bringup_compatible.launch new file mode 100644 index 0000000..31f7b0b --- /dev/null +++ b/launch/ur5_bringup_compatible.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + From aa1096db3b587085cc41d2be86a01bf56972f712 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Mon, 16 Nov 2015 10:22:33 +0100 Subject: [PATCH 02/19] Corrected bug that returned target_q instead of target_qd --- src/robot_state_RT.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index 03921e6..10a9644 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -132,7 +132,7 @@ std::vector RobotStateRT::getQTarget() { std::vector RobotStateRT::getQdTarget() { std::vector ret; val_lock_.lock(); - ret = q_target_; + ret = qd_target_; val_lock_.unlock(); return ret; } From 67776ea0497ecf331f14074387dbeb7cfda4074b Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Mon, 16 Nov 2015 10:28:45 +0100 Subject: [PATCH 03/19] Kill servo_thread in urscript. Fixes #15 --- src/ur_driver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 70503e9..fe30c2c 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -213,6 +213,7 @@ bool UrDriver::uploadProg() { cmd_str += "\tend\n"; cmd_str += "\tsleep(.1)\n"; cmd_str += "\tsocket_close()\n"; + cmd_str += "\tkill thread_servo\n"; cmd_str += "end\n"; rt_interface_->addCommandToQueue(cmd_str); From e96dd6733da29172ea7a25620ace908373973871 Mon Sep 17 00:00:00 2001 From: "G.A. vd. Hoorn" Date: Thu, 19 Nov 2015 13:01:35 +0100 Subject: [PATCH 04/19] readme: fix minor typo --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 94461e5..7d71f30 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# ur_moden_driver +# ur_modern_driver A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is designed to replace the old driver transparently, while solving some issues, improving usability as well as enabling compatibility of ros_control. From 1064510bd1d4ccf61365f5910aec7a37e7916f64 Mon Sep 17 00:00:00 2001 From: Jethro Tan Date: Wed, 2 Dec 2015 14:08:51 +0100 Subject: [PATCH 05/19] Ignore malformed messages from UR5 running 1.8.X. --- src/robot_state_RT.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index 10a9644..e3c1dec 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -327,6 +327,14 @@ void RobotStateRT::unpack(uint8_t * buf) { val_lock_.unlock(); return; } + + if (version_ > 1.8 & version_ < 1.9 & len != 812) { + // In 1.8.14035, every 17th and 18th package is 560 and 9 bytes long/malformed. + //printf("Len: %i\n", len); + val_lock_.unlock(); + return; + } + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); time_ = RobotStateRT::ntohd(unpack_to); offset += sizeof(double); From 3402db3348216307a07057dd4ac7fea21b1b4b3c Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 3 Dec 2015 10:54:36 +0100 Subject: [PATCH 06/19] Fixed set_io service to work with 3.x --- include/ur_modern_driver/ur_driver.h | 1 + src/ur_driver.cpp | 32 ++++++++++++++++++++++------ 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index f2d9b7b..4781f4f 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -49,6 +49,7 @@ private: bool reverse_connected_; double servoj_time_; bool executing_traj_; + double firmware_version_; public: UrRealtimeCommunication* rt_interface_; UrCommunication* sec_interface_; diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index fe30c2c..274da91 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -181,7 +181,8 @@ bool UrDriver::uploadProg() { cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; if (sec_interface_->robot_state_->getVersion() >= 3.1) - sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=0.03)\n", servoj_time_); + sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=0.03)\n", + servoj_time_); else sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); cmd_str += buf; @@ -246,8 +247,8 @@ void UrDriver::closeServo(std::vector positions) { bool UrDriver::start() { if (!sec_interface_->start()) return false; - rt_interface_->robot_state_->setVersion( - sec_interface_->robot_state_->getVersion()); + firmware_version_ = sec_interface_->robot_state_->getVersion(); + rt_interface_->robot_state_->setVersion(firmware_version_); if (!rt_interface_->start()) return false; ip_addr_ = rt_interface_->getLocalIp(); @@ -295,15 +296,34 @@ void UrDriver::setFlag(unsigned int n, bool b) { } void UrDriver::setDigitalOut(unsigned int n, bool b) { char buf[256]; - sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, - b ? "True" : "False"); + if (firmware_version_ < 2) { + sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, + b ? "True" : "False"); + } else if (n > 9) { + sprintf(buf, + "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n", + n - 10, b ? "True" : "False"); + } else if (n > 7) { + sprintf(buf, "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n", + n - 8, b ? "True" : "False"); + + } else { + sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", + n, b ? "True" : "False"); + + } rt_interface_->addCommandToQueue(buf); print_debug(buf); } void UrDriver::setAnalogOut(unsigned int n, double f) { char buf[256]; - sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f); + if (firmware_version_ < 2) { + sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f); + } else { + sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f); + } + rt_interface_->addCommandToQueue(buf); print_debug(buf); } From a3e174626cb2a8847277264298ba9e4a90f54961 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 3 Dec 2015 11:29:21 +0100 Subject: [PATCH 07/19] Added verification of RT message length for firmware versions 1.6 to 3.2 --- src/robot_state_RT.cpp | 36 +++++++++++++++++++++++------------- src/ur_driver.cpp | 1 + 2 files changed, 24 insertions(+), 13 deletions(-) diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index e3c1dec..2074165 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -60,7 +60,6 @@ RobotStateRT::~RobotStateRT() { pMsg_cond_->notify_all(); } - void RobotStateRT::setDataPublished() { data_published_ = false; } @@ -75,7 +74,6 @@ bool RobotStateRT::getControllerUpdated() { return controller_updated_; } - double RobotStateRT::ntohd(uint64_t nf) { double x; nf = be64toh(nf); @@ -321,20 +319,32 @@ void RobotStateRT::unpack(uint8_t * buf) { offset += sizeof(len); len = ntohl(len); - if (version_ > 3. & version_ < 3.1 & len != 1044) { - //In 3.0, every 4th? package is malformed...? - //printf("Len: %i\n", len); + + //Check the correct message length is received + bool len_good = true; + if (version_ >= 1.6 && version_ < 1.7) { //v1.6 + if (len != 756) + len_good = false; + } else if (version_ >= 1.7 && version_ < 1.8) { //v1.7 + if (len != 764) + len_good = false; + } else if (version_ >= 1.8 && version_ < 1.9) { //v1.8 + if (len != 812) + len_good = false; + } else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1 + if (len != 1044) + len_good = false; + } else if (version_ >= 3.2 && version_ < 3.3) { //v3.2 + if (len != 1060) + len_good = false; + } + + if (!len_good) { + printf("Wrong length of message on RT interface: %i\n", len); val_lock_.unlock(); return; } - - if (version_ > 1.8 & version_ < 1.9 & len != 812) { - // In 1.8.14035, every 17th and 18th package is 560 and 9 bytes long/malformed. - //printf("Len: %i\n", len); - val_lock_.unlock(); - return; - } - + memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); time_ = RobotStateRT::ntohd(unpack_to); offset += sizeof(double); diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 274da91..5572aa2 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -30,6 +30,7 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond, struct sockaddr_in serv_addr; int n, flag; + firmware_version_ = 0; reverse_connected_ = false; executing_traj_ = false; rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, From f104b500262cf7ffc272ec0c237e45e47826d4f0 Mon Sep 17 00:00:00 2001 From: hemes Date: Wed, 13 Jan 2016 15:04:08 -0600 Subject: [PATCH 08/19] Added check to ensure robot pose matches initial trajectory point. --- src/ur_ros_wrapper.cpp | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index c95aa6b..abfbda1 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -298,7 +298,7 @@ private: print_error(result_.error_string); return; } - + if (!has_velocities()) { result_.error_code = result_.INVALID_GOAL; result_.error_string = "Received a goal without velocities"; @@ -326,6 +326,14 @@ private: } reorder_traj_joints(goal.trajectory); + + if (!start_positions_match(goal.trajectory, 0.01)) { + result_.error_code = result_.INVALID_GOAL; + result_.error_string = "Goal start doesn't match current pose"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } std::vector timestamps; std::vector > positions, velocities; @@ -476,6 +484,19 @@ private: return true; } + bool start_positions_match(const trajectory_msgs::JointTrajectory &traj, double eps) + { + for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) + { + std::vector qActual = robot_.rt_interface_->robot_state_->getQActual(); + if( fabs(traj.points[0].positions[i] - qActual[i]) > eps ) + { + return false; + } + } + return true; + } + bool has_limited_velocities() { actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); From a29fcac0c6a87d2768b6b718e54cb4fee62f2777 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 19 Feb 2016 11:38:29 +0100 Subject: [PATCH 09/19] Added test_move.py with joint_states subscriber --- test_move.py | 131 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 131 insertions(+) create mode 100755 test_move.py diff --git a/test_move.py b/test_move.py new file mode 100755 index 0000000..947d9d1 --- /dev/null +++ b/test_move.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python +import time +import roslib; roslib.load_manifest('ur_driver') +import rospy +import actionlib +from control_msgs.msg import * +from trajectory_msgs.msg import * +from sensor_msgs.msg import JointState +from math import pi + +JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', + 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] +Q1 = [2.2,0,-1.57,0,0,0] +Q2 = [1.5,0,-1.57,0,0,0] +Q3 = [1.5,-0.2,-1.57,0,0,0] +joints_pos = [0.0]*6 + +client = None + +def move1(): + global joints_pos + g = FollowJointTrajectoryGoal() + g.trajectory = JointTrajectory() + g.trajectory.joint_names = JOINT_NAMES + g.trajectory.points = [ + JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)), + JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)), + JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)), + JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))] + client.send_goal(g) + try: + client.wait_for_result() + except KeyboardInterrupt: + client.cancel_goal() + raise + +def move_disordered(): + global joints_pos + order = [4, 2, 3, 1, 5, 0] + g = FollowJointTrajectoryGoal() + g.trajectory = JointTrajectory() + g.trajectory.joint_names = [JOINT_NAMES[i] for i in order] + q1 = [Q1[i] for i in order] + q2 = [Q2[i] for i in order] + q3 = [Q3[i] for i in order] + g.trajectory.points = [ + JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)), + JointTrajectoryPoint(positions=q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)), + JointTrajectoryPoint(positions=q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)), + JointTrajectoryPoint(positions=q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))] + client.send_goal(g) + client.wait_for_result() + +def move_repeated(): + global joints_pos + g = FollowJointTrajectoryGoal() + g.trajectory = JointTrajectory() + g.trajectory.joint_names = JOINT_NAMES + + d = 2.0 + g.trajectory.points = [JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0))] + for i in range(10): + g.trajectory.points.append( + JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(d))) + d += 1 + g.trajectory.points.append( + JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(d))) + d += 1 + g.trajectory.points.append( + JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(d))) + d += 2 + client.send_goal(g) + try: + client.wait_for_result() + except KeyboardInterrupt: + client.cancel_goal() + raise + +def move_interrupt(): + global joints_pos + g = FollowJointTrajectoryGoal() + g.trajectory = JointTrajectory() + g.trajectory.joint_names = JOINT_NAMES + g.trajectory.points = [ + JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)), + JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)), + JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)), + JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))] + + client.send_goal(g) + time.sleep(2.0) + print "Interrupting" + client.send_goal(g) + try: + client.wait_for_result() + except KeyboardInterrupt: + client.cancel_goal() + raise + + +def joint_subscriber(msg): + global joints_pos + joints_pos = msg.position + +def main(): + global client + try: + rospy.init_node("test_move", anonymous=True, disable_signals=True) + client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction) + print "Waiting for server..." + client.wait_for_server() + print "Connected to server" + rospy.Subscriber("joint_states", JointState, joint_subscriber) + print "This program makes the robot move between the following three poses:" + print str([Q1[i]*180./pi for i in xrange(0,6)]) + print str([Q2[i]*180./pi for i in xrange(0,6)]) + print str([Q3[i]*180./pi for i in xrange(0,6)]) + print "Please make sure that your robot can move freely between these poses before proceeding!" + inp = raw_input("Continue? y/n: ")[0] + if (inp == 'y'): + #move1() + move_repeated() + #move_disordered() + #move_interrupt() + else: + print "Halting program" + except KeyboardInterrupt: + rospy.signal_shutdown("KeyboardInterrupt") + raise + +if __name__ == '__main__': main() From 56b4c6bae1c0b71794c8bb51fa06423b09a14775 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 19 Feb 2016 11:56:11 +0100 Subject: [PATCH 10/19] Changed robot_state_publisher to fix #32 --- launch/ur_common.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/ur_common.launch b/launch/ur_common.launch index 565fa86..a9754a7 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -18,7 +18,7 @@ - + From c05f16c8e96d7573ca5a9ef4472798a4b23c017e Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 19 Feb 2016 11:59:19 +0100 Subject: [PATCH 11/19] Tweaked ur5 PID controller values. Fixes #27 --- config/ur5_controllers.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/config/ur5_controllers.yaml b/config/ur5_controllers.yaml index 4eab52e..bcfe680 100644 --- a/config/ur5_controllers.yaml +++ b/config/ur5_controllers.yaml @@ -83,12 +83,12 @@ vel_based_pos_traj_controller: action_monitor_rate: 10 gains: #!!These values are useable, but maybe not optimal - shoulder_pan_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} - shoulder_lift_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} - elbow_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} - wrist_1_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} - wrist_2_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} - wrist_3_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + shoulder_pan_joint: {p: 1.2, i: 0.0, d: 0.1, i_clamp: 1} + shoulder_lift_joint: {p: 1.2, i: 0.0, d: 0.1, i_clamp: 1} + elbow_joint: {p: 1.2, i: 0.0, d: 0.1, i_clamp: 1} + wrist_1_joint: {p: 1.2, i: 0.0, d: 0.1, i_clamp: 1} + wrist_2_joint: {p: 1.2, i: 0.0, d: 0.1, i_clamp: 1} + wrist_3_joint: {p: 1.2, i: 0.0, d: 0.1, i_clamp: 1} # state_publish_rate: 50 # Defaults to 50 # action_monitor_rate: 20 # Defaults to 20 From 0b9bf8b549af90f1ae302e6e53fac89d41f00091 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 19 Feb 2016 14:16:49 +0100 Subject: [PATCH 12/19] Updated launch files to evaluate a 'prefix' argument and pass it to the driver and urdf file. Fixes #28 --- launch/ur10_bringup.launch | 3 ++- launch/ur10_bringup_joint_limited.launch | 4 +++- launch/ur3_bringup.launch | 3 ++- launch/ur3_bringup_joint_limited.launch | 4 +++- launch/ur5_bringup.launch | 4 +++- launch/ur5_bringup_joint_limited.launch | 6 ++++-- launch/ur_common.launch | 2 ++ test_move.py | 6 ++++++ 8 files changed, 25 insertions(+), 7 deletions(-) diff --git a/launch/ur10_bringup.launch b/launch/ur10_bringup.launch index 97d5584..8b09201 100644 --- a/launch/ur10_bringup.launch +++ b/launch/ur10_bringup.launch @@ -13,10 +13,11 @@ - + + diff --git a/launch/ur10_bringup_joint_limited.launch b/launch/ur10_bringup_joint_limited.launch index 1e07c20..6cc6ca6 100644 --- a/launch/ur10_bringup_joint_limited.launch +++ b/launch/ur10_bringup_joint_limited.launch @@ -11,12 +11,14 @@ - + + + diff --git a/launch/ur3_bringup.launch b/launch/ur3_bringup.launch index 1b28f72..c5d4a8f 100644 --- a/launch/ur3_bringup.launch +++ b/launch/ur3_bringup.launch @@ -13,10 +13,11 @@ - + + diff --git a/launch/ur3_bringup_joint_limited.launch b/launch/ur3_bringup_joint_limited.launch index 6d40005..e2c657e 100644 --- a/launch/ur3_bringup_joint_limited.launch +++ b/launch/ur3_bringup_joint_limited.launch @@ -11,12 +11,14 @@ - + + + diff --git a/launch/ur5_bringup.launch b/launch/ur5_bringup.launch index 212b8e6..a097d4e 100644 --- a/launch/ur5_bringup.launch +++ b/launch/ur5_bringup.launch @@ -13,14 +13,16 @@ - + + + diff --git a/launch/ur5_bringup_joint_limited.launch b/launch/ur5_bringup_joint_limited.launch index b8c3b10..643f5c4 100644 --- a/launch/ur5_bringup_joint_limited.launch +++ b/launch/ur5_bringup_joint_limited.launch @@ -11,12 +11,14 @@ - - + + + + diff --git a/launch/ur_common.launch b/launch/ur_common.launch index a9754a7..8b7836c 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -11,6 +11,7 @@ + @@ -23,6 +24,7 @@ + diff --git a/test_move.py b/test_move.py index 947d9d1..83292a2 100755 --- a/test_move.py +++ b/test_move.py @@ -111,6 +111,12 @@ def main(): client.wait_for_server() print "Connected to server" rospy.Subscriber("joint_states", JointState, joint_subscriber) + parameters = rospy.get_param(None) + index = str(parameters).find('prefix') + if (index > 0): + prefix = str(parameters)[index+len("prefix': '"):(index+len("prefix': '")+str(parameters)[index+len("prefix': '"):-1].find("'"))] + for i, name in enumerate(JOINT_NAMES): + JOINT_NAMES[i] = prefix + name print "This program makes the robot move between the following three poses:" print str([Q1[i]*180./pi for i in xrange(0,6)]) print str([Q2[i]*180./pi for i in xrange(0,6)]) From adc223cbd837d9331db774031ef42f0e7f73a4a6 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 19 Feb 2016 14:58:28 +0100 Subject: [PATCH 13/19] Added prefix args to ros_control launch files and to the tool0_controller transform frame --- launch/ur10_ros_control.launch | 9 +++++++-- launch/ur3_bringup.launch | 1 + launch/ur3_ros_control.launch | 4 ++++ launch/ur5_ros_control.launch | 10 +++++++--- launch/ur_common.launch | 4 ++-- src/ur_ros_wrapper.cpp | 5 +++-- 6 files changed, 24 insertions(+), 9 deletions(-) diff --git a/launch/ur10_ros_control.launch b/launch/ur10_ros_control.launch index ce6c1e1..bbf576a 100644 --- a/launch/ur10_ros_control.launch +++ b/launch/ur10_ros_control.launch @@ -1,20 +1,24 @@ + - + - + + + + @@ -22,6 +26,7 @@ + diff --git a/launch/ur3_bringup.launch b/launch/ur3_bringup.launch index c5d4a8f..9d267e1 100644 --- a/launch/ur3_bringup.launch +++ b/launch/ur3_bringup.launch @@ -25,6 +25,7 @@ + diff --git a/launch/ur3_ros_control.launch b/launch/ur3_ros_control.launch index 1c1cd89..c9a7fd2 100644 --- a/launch/ur3_ros_control.launch +++ b/launch/ur3_ros_control.launch @@ -1,3 +1,4 @@ + @@ -9,10 +10,12 @@ + + @@ -23,6 +26,7 @@ + diff --git a/launch/ur5_ros_control.launch b/launch/ur5_ros_control.launch index b81b017..301ffea 100644 --- a/launch/ur5_ros_control.launch +++ b/launch/ur5_ros_control.launch @@ -1,21 +1,24 @@ + - + - + + + - + @@ -23,6 +26,7 @@ + diff --git a/launch/ur_common.launch b/launch/ur_common.launch index 8b7836c..cc48d98 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -13,8 +13,8 @@ - - + + diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index abfbda1..6280868 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -157,9 +157,10 @@ public: robot_.setServojTime(servoj_time); //Base and tool frames - base_frame_ = "base"; - tool_frame_ = "tool0_controller"; + base_frame_ = joint_prefix + "base_link"; + tool_frame_ = joint_prefix + "tool0_controller"; if (ros::param::get("~base_frame", base_frame_)) { + base_frame_ = base_frame_; sprintf(buf, "Base frame set to: %s", base_frame_.c_str()); print_debug(buf); } From 161a7fc146eb5a23c247fa175b491fb4a38ee15b Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 19 Feb 2016 15:53:54 +0100 Subject: [PATCH 14/19] Removed the prefix tag from the ur_description launch call --- launch/ur10_bringup.launch | 1 - launch/ur10_ros_control.launch | 1 - launch/ur3_bringup.launch | 1 - launch/ur3_ros_control.launch | 1 - launch/ur5_bringup.launch | 1 - launch/ur5_ros_control.launch | 1 - 6 files changed, 6 deletions(-) diff --git a/launch/ur10_bringup.launch b/launch/ur10_bringup.launch index 8b09201..58f7abd 100644 --- a/launch/ur10_bringup.launch +++ b/launch/ur10_bringup.launch @@ -17,7 +17,6 @@ - diff --git a/launch/ur10_ros_control.launch b/launch/ur10_ros_control.launch index bbf576a..3004def 100644 --- a/launch/ur10_ros_control.launch +++ b/launch/ur10_ros_control.launch @@ -15,7 +15,6 @@ - diff --git a/launch/ur3_bringup.launch b/launch/ur3_bringup.launch index 9d267e1..9e7986d 100644 --- a/launch/ur3_bringup.launch +++ b/launch/ur3_bringup.launch @@ -17,7 +17,6 @@ - diff --git a/launch/ur3_ros_control.launch b/launch/ur3_ros_control.launch index c9a7fd2..7150955 100644 --- a/launch/ur3_ros_control.launch +++ b/launch/ur3_ros_control.launch @@ -15,7 +15,6 @@ - diff --git a/launch/ur5_bringup.launch b/launch/ur5_bringup.launch index a097d4e..eeaa318 100644 --- a/launch/ur5_bringup.launch +++ b/launch/ur5_bringup.launch @@ -17,7 +17,6 @@ - diff --git a/launch/ur5_ros_control.launch b/launch/ur5_ros_control.launch index 301ffea..d73ae7f 100644 --- a/launch/ur5_ros_control.launch +++ b/launch/ur5_ros_control.launch @@ -15,7 +15,6 @@ - From 682c77582b9e9c4052589508c9af33766c4828a2 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 19 Feb 2016 16:02:12 +0100 Subject: [PATCH 15/19] Removed info message about setting prefix when prefix is an empty string --- src/ur_ros_wrapper.cpp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 6280868..5dbb317 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -94,15 +94,17 @@ public: char buf[256]; if (ros::param::get("~prefix", joint_prefix)) { - sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); - print_info(buf); + if (joint_prefix.length() > 0) { + sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); + print_info(buf); + } + joint_names.push_back(joint_prefix + "shoulder_pan_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"); + joint_names.push_back(joint_prefix + "wrist_3_joint"); } - joint_names.push_back(joint_prefix + "shoulder_pan_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"); - joint_names.push_back(joint_prefix + "wrist_3_joint"); robot_.setJointNames(joint_names); use_ros_control_ = false; From 30b9ee5330da3a76bfdd618ed2843801782fa395 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Mon, 22 Feb 2016 17:08:40 +0100 Subject: [PATCH 16/19] Fixed 'No joint names published when not using a prefix'. Closes #34 --- src/ur_ros_wrapper.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 5dbb317..01d87d8 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -97,13 +97,13 @@ public: if (joint_prefix.length() > 0) { sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); print_info(buf); - } - joint_names.push_back(joint_prefix + "shoulder_pan_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"); - joint_names.push_back(joint_prefix + "wrist_3_joint"); + } + joint_names.push_back(joint_prefix + "shoulder_pan_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"); + joint_names.push_back(joint_prefix + "wrist_3_joint"); } robot_.setJointNames(joint_names); From 70279d5ece579bda083c27e04ac0cce4ea041916 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Mon, 22 Feb 2016 17:10:00 +0100 Subject: [PATCH 17/19] Fixed 'No joint names published when not using a prefix'. Closes #34 --- src/ur_ros_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 01d87d8..e5a2d20 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -98,13 +98,13 @@ public: sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); print_info(buf); } + } joint_names.push_back(joint_prefix + "shoulder_pan_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"); joint_names.push_back(joint_prefix + "wrist_3_joint"); - } robot_.setJointNames(joint_names); use_ros_control_ = false; From 91450da1739913c764e979409dece176d77d1055 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 23 Feb 2016 09:47:37 +0100 Subject: [PATCH 18/19] Changed test_move.py to use wait_for_message instead of global vars --- test_move.py | 113 +++++++++++++++++++++++++++------------------------ 1 file changed, 61 insertions(+), 52 deletions(-) diff --git a/test_move.py b/test_move.py index 83292a2..fb6b0c5 100755 --- a/test_move.py +++ b/test_move.py @@ -13,7 +13,6 @@ JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', Q1 = [2.2,0,-1.57,0,0,0] Q2 = [1.5,0,-1.57,0,0,0] Q3 = [1.5,-0.2,-1.57,0,0,0] -joints_pos = [0.0]*6 client = None @@ -22,20 +21,23 @@ def move1(): g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() g.trajectory.joint_names = JOINT_NAMES - g.trajectory.points = [ - JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)), - JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)), - JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)), - JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))] - client.send_goal(g) try: + joint_states = rospy.wait_for_message("joint_states", JointState) + joints_pos = joint_states.position + g.trajectory.points = [ + JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)), + JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)), + JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)), + JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))] + client.send_goal(g) client.wait_for_result() except KeyboardInterrupt: client.cancel_goal() raise + except: + raise def move_disordered(): - global joints_pos order = [4, 2, 3, 1, 5, 0] g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() @@ -43,64 +45,72 @@ def move_disordered(): q1 = [Q1[i] for i in order] q2 = [Q2[i] for i in order] q3 = [Q3[i] for i in order] - g.trajectory.points = [ - JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)), - JointTrajectoryPoint(positions=q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)), - JointTrajectoryPoint(positions=q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)), - JointTrajectoryPoint(positions=q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))] - client.send_goal(g) - client.wait_for_result() - -def move_repeated(): - global joints_pos - g = FollowJointTrajectoryGoal() - g.trajectory = JointTrajectory() - g.trajectory.joint_names = JOINT_NAMES - - d = 2.0 - g.trajectory.points = [JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0))] - for i in range(10): - g.trajectory.points.append( - JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(d))) - d += 1 - g.trajectory.points.append( - JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(d))) - d += 1 - g.trajectory.points.append( - JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(d))) - d += 2 - client.send_goal(g) try: + joint_states = rospy.wait_for_message("joint_states", JointState) + joints_pos = joint_states.position + g.trajectory.points = [ + JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)), + JointTrajectoryPoint(positions=q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)), + JointTrajectoryPoint(positions=q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)), + JointTrajectoryPoint(positions=q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))] + client.send_goal(g) client.wait_for_result() except KeyboardInterrupt: client.cancel_goal() raise + except: + raise + +def move_repeated(): + g = FollowJointTrajectoryGoal() + g.trajectory = JointTrajectory() + g.trajectory.joint_names = JOINT_NAMES + try: + joint_states = rospy.wait_for_message("joint_states", JointState) + joints_pos = joint_states.position + d = 2.0 + g.trajectory.points = [JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0))] + for i in range(10): + g.trajectory.points.append( + JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(d))) + d += 1 + g.trajectory.points.append( + JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(d))) + d += 1 + g.trajectory.points.append( + JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(d))) + d += 2 + client.send_goal(g) + client.wait_for_result() + except KeyboardInterrupt: + client.cancel_goal() + raise + except: + raise def move_interrupt(): - global joints_pos g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() g.trajectory.joint_names = JOINT_NAMES - g.trajectory.points = [ - JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)), - JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)), - JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)), - JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))] - - client.send_goal(g) - time.sleep(2.0) - print "Interrupting" - client.send_goal(g) try: + joint_states = rospy.wait_for_message("joint_states", JointState) + joints_pos = joint_states.position + g.trajectory.points = [ + JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)), + JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)), + JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)), + JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))] + + client.send_goal(g) + time.sleep(2.0) + print "Interrupting" + client.send_goal(g) client.wait_for_result() except KeyboardInterrupt: client.cancel_goal() raise - - -def joint_subscriber(msg): - global joints_pos - joints_pos = msg.position + except: + raise def main(): global client @@ -110,7 +120,6 @@ def main(): print "Waiting for server..." client.wait_for_server() print "Connected to server" - rospy.Subscriber("joint_states", JointState, joint_subscriber) parameters = rospy.get_param(None) index = str(parameters).find('prefix') if (index > 0): From 0dc244bbe8946f5cd9f1e7d26501aa47e8c285dc Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Wed, 2 Mar 2016 17:04:11 +0100 Subject: [PATCH 19/19] Fixed a bug where tool0_controller position was all 0 for ur firmware version 1.8.xx --- src/robot_state_RT.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index 2074165..b170a04 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -364,8 +364,8 @@ void RobotStateRT::unpack(uint8_t * buf) { offset += sizeof(double) * 6; i_actual_ = unpackVector(buf, offset, 6); offset += sizeof(double) * 6; - if (version_ <= 1.8) { - if (version_ != 1.6) + if (version_ <= 1.9) { + if (version_ > 1.6) tool_accelerometer_values_ = unpackVector(buf, offset, 3); offset += sizeof(double) * (3 + 15); tcp_force_ = unpackVector(buf, offset, 6);