Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 10:00:48 +02:00
Code Releases Activity

Merge pull request #2 from ThomasTimm/master

Browse Source 
Merge updates from original driver
This commit is contained in:
Simon Jansen
2016-03-18 10:08:07 +01:00
parent dd8849384a 0dc244bbe8
commit 96d478ac2c
19 changed files with 333 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
README.md
View File

@@ -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.

12
config/ur5_controllers.yaml
View File

@@ -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

1
include/ur_modern_driver/ur_driver.h
View File

@@ -49,6 +49,7 @@ private:
bool reverse_connected_;
double servoj_time_;
bool executing_traj_;
double firmware_version_;
public:
UrRealtimeCommunication* rt_interface_;
UrCommunication* sec_interface_;

2
launch/ur10_bringup.launch
View File

@@ -13,7 +13,7 @@
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="10.0"/>
<arg name="prefix" default="" />
<!-- robot model -->
<include file="$(find ur_description)/launch/ur10_upload.launch">
<arg name="limited" value="$(arg limited)"/>

30
launch/ur10_bringup_compatible.launch Normal file
View File

@@ -0,0 +1,30 @@
<?xml version="1.0"?>
<!--
Universal robot ur10 launch. Loads ur10 robot description (see ur_common.launch
for more info)
Usage:
ur10_bringup.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="10.0"/>
<!-- robot model -->
<include file="$(find ur_description)/launch/ur10_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<!-- ur common -->
<include file="$(find ur_modern_driver)/launch/ur_common.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="servoj_time" value="0.08" />
</include>
</launch>

4
launch/ur10_bringup_joint_limited.launch
View File

@@ -11,12 +11,14 @@
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="20.0"/>
<arg name="max_payload" default="10.0"/>
<arg name="prefix" default="" />
<include file="$(find ur_modern_driver)/launch/ur10_bringup.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="limited" value="true"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" value="$(arg prefix)" />
</include>
</launch>

8
launch/ur10_ros_control.launch
View File

@@ -1,20 +1,23 @@
<?xml version="1.0"?>
<launch>
<!-- GDB functionality -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<arg name="robot_ip"/>
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="10.0"/>
<arg name="max_payload" default="3.0"/>
<arg name="prefix" default="" />
<arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
<!-- robot model -->
<include file="$(find ur_description)/launch/ur10_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<!-- Load hardware interface -->
<node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
@@ -22,6 +25,7 @@
<param name="max_payload" type="double" value="$(arg max_payload)"/>
<param name="max_velocity" type="double" value="$(arg max_velocity)"/>
<param name="use_ros_control" type="bool" value="True"/>
<arg name="prefix" value="$(arg prefix)" />
</node>
<!-- Load controller settings -->

3
launch/ur3_bringup.launch
View File

@@ -13,7 +13,7 @@
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="3.0"/>
<arg name="prefix" default="" />
<!-- robot model -->
<include file="$(find ur_description)/launch/ur3_upload.launch">
<arg name="limited" value="$(arg limited)"/>
@@ -24,6 +24,7 @@
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" value="$(arg prefix)" />
</include>
</launch>

4
launch/ur3_bringup_joint_limited.launch
View File

@@ -11,12 +11,14 @@
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="10.0"/>
<arg name="max_payload" default="3.0"/>
<arg name="prefix" default="" />
<include file="$(find ur_modern_driver)/launch/ur3_bringup.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="limited" value="true"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" value="$(arg prefix)" />
</include>
</launch>

3
launch/ur3_ros_control.launch
View File

@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
<!-- GDB functionality -->
@@ -9,6 +10,7 @@
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="3.0"/>
<arg name="prefix" default="" />
<arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
<!-- robot model -->
<include file="$(find ur_description)/launch/ur3_upload.launch">
@@ -23,6 +25,7 @@
<param name="max_payload" type="double" value="$(arg max_payload)"/>
<param name="max_velocity" type="double" value="$(arg max_velocity)"/>
<param name="use_ros_control" type="bool" value="True"/>
<arg name="prefix" value="$(arg prefix)" />
</node>
<!-- Load controller settings -->

3
launch/ur5_bringup.launch
View File

@@ -13,7 +13,7 @@
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="5.0"/>
<arg name="prefix" default="" />
<!-- robot model -->
<include file="$(find ur_description)/launch/ur5_upload.launch">
<arg name="limited" value="$(arg limited)"/>
@@ -21,6 +21,7 @@
<!-- ur common -->
<include file="$(find ur_modern_driver)/launch/ur_common.launch">
<arg name="prefix" value="$(arg prefix)" />
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>

30
launch/ur5_bringup_compatible.launch Normal file
View File

@@ -0,0 +1,30 @@
<?xml version="1.0"?>
<!--
Universal robot ur5 launch. Loads ur5 robot description (see ur_common.launch
for more info)
Usage:
ur5_bringup.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="5.0"/>
<!-- robot model -->
<include file="$(find ur_description)/launch/ur5_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<!-- ur common -->
<include file="$(find ur_modern_driver)/launch/ur_common.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="servoj_time" value="0.08" />
</include>
</launch>

6
launch/ur5_bringup_joint_limited.launch
View File

@@ -11,12 +11,14 @@
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="10.0"/>
<arg name="max_payload" default="5.0"/>
<arg name="prefix" default="" />
<include file="$(find ur_modern_driver)/launch/ur5_bringup.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="limited" value="true"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" value="$(arg prefix)" />
</include>
</launch>

9
launch/ur5_ros_control.launch
View File

@@ -1,21 +1,23 @@
<?xml version="1.0"?>
<launch>
<!-- GDB functionality -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<arg name="robot_ip"/>
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="5.0"/>
<arg name="max_payload" default="3.0"/>
<arg name="prefix" default="" />
<arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
<!-- robot model -->
<include file="$(find ur_description)/launch/ur5_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<!-- Load hardware interface -->
<node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
@@ -23,6 +25,7 @@
<param name="max_payload" type="double" value="$(arg max_payload)"/>
<param name="max_velocity" type="double" value="$(arg max_velocity)"/>
<param name="use_ros_control" type="bool" value="True"/>
<arg name="prefix" value="$(arg prefix)" />
</node>
<!-- Load controller settings -->

8
launch/ur_common.launch
View File

@@ -11,18 +11,20 @@
<arg name="robot_ip" />
<arg name="min_payload" />
<arg name="max_payload" />
<arg name="prefix" default="" />
<arg name="servoj_time" default="0.008" />
<arg name="base_frame" default="base"/>
<arg name="tool_frame" default="tool0_controller"/>
<arg name="base_frame" default="$(arg prefix)base_link" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<!-- The max_velocity parameter is only used for debugging in the ur_driver. It's not related to actual velocity limits -->
<arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- driver -->
<node name="ur_driver" pkg="ur_modern_driver" type="ur_driver" output="screen">
<!-- copy the specified IP address to be consistant with ROS-Industrial spec. -->
<param name="prefix" type="str" value="$(arg prefix)" />
<param name="robot_ip_address" type="str" value="$(arg robot_ip)" />
<param name="min_payload" type="double" value="$(arg min_payload)" />
<param name="max_payload" type="double" value="$(arg max_payload)" />

34
src/robot_state_RT.cpp
View File

@@ -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<double> RobotStateRT::getQTarget() {
std::vector<double> RobotStateRT::getQdTarget() {
std::vector<double> 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);

34
src/ur_driver.cpp
View File

@@ -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<double> 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);
}

36
src/ur_ros_wrapper.cpp
View File

@@ -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<double> timestamps;
std::vector<std::vector<double> > 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<double> 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<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*goal_handle_.getGoal();

146
test_move.py Executable file
View File

@@ -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()