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.
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
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/launch/ur10_bringup.launch b/launch/ur10_bringup.launch
index 97d5584..58f7abd 100644
--- a/launch/ur10_bringup.launch
+++ b/launch/ur10_bringup.launch
@@ -13,7 +13,7 @@
-
+
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/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/ur10_ros_control.launch b/launch/ur10_ros_control.launch
index ce6c1e1..3004def 100644
--- a/launch/ur10_ros_control.launch
+++ b/launch/ur10_ros_control.launch
@@ -1,20 +1,23 @@
+
-
+
-
+
+
+
@@ -22,6 +25,7 @@
+
diff --git a/launch/ur3_bringup.launch b/launch/ur3_bringup.launch
index 1b28f72..9e7986d 100644
--- a/launch/ur3_bringup.launch
+++ b/launch/ur3_bringup.launch
@@ -13,7 +13,7 @@
-
+
@@ -24,6 +24,7 @@
+
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/ur3_ros_control.launch b/launch/ur3_ros_control.launch
index 1c1cd89..7150955 100644
--- a/launch/ur3_ros_control.launch
+++ b/launch/ur3_ros_control.launch
@@ -1,3 +1,4 @@
+
@@ -9,6 +10,7 @@
+
@@ -23,6 +25,7 @@
+
diff --git a/launch/ur5_bringup.launch b/launch/ur5_bringup.launch
index 212b8e6..eeaa318 100644
--- a/launch/ur5_bringup.launch
+++ b/launch/ur5_bringup.launch
@@ -13,7 +13,7 @@
-
+
@@ -21,6 +21,7 @@
+
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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/ur5_ros_control.launch b/launch/ur5_ros_control.launch
index b81b017..d73ae7f 100644
--- a/launch/ur5_ros_control.launch
+++ b/launch/ur5_ros_control.launch
@@ -1,21 +1,23 @@
+
-
+
-
+
+
-
+
@@ -23,6 +25,7 @@
+
diff --git a/launch/ur_common.launch b/launch/ur_common.launch
index 565fa86..cc48d98 100644
--- a/launch/ur_common.launch
+++ b/launch/ur_common.launch
@@ -11,18 +11,20 @@
+
-
-
+
+
-
+
+
diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp
index 03921e6..b170a04 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);
@@ -132,7 +130,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;
}
@@ -321,12 +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;
}
+
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
time_ = RobotStateRT::ntohd(unpack_to);
offset += sizeof(double);
@@ -346,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);
diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp
index 70503e9..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,
@@ -181,7 +182,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;
@@ -213,6 +215,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);
@@ -245,8 +248,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();
@@ -294,15 +297,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);
}
diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp
index c95aa6b..e5a2d20 100644
--- a/src/ur_ros_wrapper.cpp
+++ b/src/ur_ros_wrapper.cpp
@@ -94,9 +94,11 @@ 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");
@@ -157,9 +159,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);
}
@@ -298,7 +301,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 +329,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 +487,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();
diff --git a/test_move.py b/test_move.py
new file mode 100755
index 0000000..fb6b0c5
--- /dev/null
+++ b/test_move.py
@@ -0,0 +1,146 @@
+#!/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]
+
+client = None
+
+def move1():
+ global joints_pos
+ 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
+ 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():
+ 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]
+ 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():
+ 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
+ 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
+ except:
+ raise
+
+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"
+ 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)])
+ 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()