def move(self, left_shoulder, left_elbow, left_hand,
right_shoulder, right_elbow, right_hand):
"""
Creates and sends threads for Baxter's arm movement
"""
# Collects angles from each controller
angles = {'left': [], 'right': []}
self.human_to_baxter(left_shoulder, left_elbow, left_hand,
right_shoulder, right_elbow, right_hand, angles)
# Creates threads for each of Baxter's arms
left_queue = Queue.Queue()
right_queue = Queue.Queue()
left_thread = threading.Thread(target=self.move_thread,
args=('left', angles['left'], left_queue))
right_thread = threading.Thread(target=self.move_thread,
args=('right', angles['right'], right_queue))
left_thread.daemon = True
right_thread.daemon = True
left_thread.start()
right_thread.start()
dataflow.wait_for(lambda: \
not (left_thread.is_alive() or right_thread.is_alive()),
timeout=20.0,
rate=10)
left_thread.join()
right_thread.join()
def dance(self):
self.set_neutral()
done = False
while not done and not rospy.is_shutdown():
for move in self.moves:
angles = {
'left': self.all_moves(move + '_l'),
'right': self.all_moves(move + '_r')
}
print angles['left']
print angles['right']
left_queue = Queue.Queue()
right_queue = Queue.Queue()
left_thread = threading.Thread(target=self.move_thread,
args=('left', angles['left'],
left_queue))
right_thread = threading.Thread(target=self.move_thread,
args=('right', angles['right'],
right_queue))
left_thread.daemon = True
right_thread.daemon = True
left_thread.start()
right_thread.start()
dataflow.wait_for(lambda: \
not (left_thread.is_alive() or right_thread.is_alive()),
timeout=20.0,
rate=10)
left_thread.join()
right_thread.join()
rospy.sleep(2.0)
def __init__(self, component_id):
"""
@param component_id - unique id of analog component
"""
self._id = component_id
self._component_type = 'analog_io'
self._is_output = False
self._state = {}
type_ns = '/sdk/robot/' + self._component_type
topic_base = type_ns + '/' + self._id
self._sub_state = rospy.Subscriber(
topic_base + '/state',
AnalogIOState,
self._on_io_state)
dataflow.wait_for(
lambda: len(self._state.keys()) != 0,
timeout=2.0,
timeout_msg="Failed to get current analog_io state from %s" \
% (topic_base,),
)
# check if output-capable before creating publisher
if self._is_output:
self._pub_output = rospy.Publisher(
type_ns + '/command',
AnalogOutputCommand)
def dance(self):
self.set_neutral()
done = False
while not done and not rospy.is_shutdown():
for move in self.moves:
angles = {'left': self.all_moves(move + '_l'),
'right': self.all_moves(move + '_r')
}
print angles['left']
print angles['right']
left_queue = Queue.Queue()
right_queue = Queue.Queue()
left_thread = threading.Thread(target=self.move_thread,
args=('left', angles['left'], left_queue))
right_thread = threading.Thread(target=self.move_thread,
args=('right', angles['right'], right_queue))
left_thread.daemon = True
right_thread.daemon = True
left_thread.start()
right_thread.start()
dataflow.wait_for(lambda: \
not (left_thread.is_alive() or right_thread.is_alive()),
timeout=20.0,
rate=10)
left_thread.join()
right_thread.join()
rospy.sleep(2.0)
def command_nod(self, timeout=5.0):
"""
Command the head to nod once.
@param timeout (float) - Seconds to wait for the head to nod.
If 0, just command once and return. [0]
"""
self._pub_nod.publish(True)
if not timeout == 0:
# Wait for nod to initiate
dataflow.wait_for(
test=lambda: self.nodding(),
timeout=timeout,
rate=100,
timeout_msg="Failed to initiate head nod command",
body=lambda: self._pub_nod.publish(True)
)
# Wait for nod to complete
dataflow.wait_for(
test=lambda: not self.nodding(),
timeout=timeout,
rate=100,
timeout_msg="Failed to complete head nod command",
body=lambda: self._pub_nod.publish(True)
)
def __init__(self):
"""
Interface class for the head on the Baxter Robot.
Used to control the head pan angle and to enable/disable the head nod
action.
"""
self._state = {}
self._pub_pan = rospy.Publisher(
'/sdk/robot/head/command_head_pan',
baxter_msgs.msg.HeadPanCommand)
self._pub_nod = rospy.Publisher(
'/robot/head/command_head_nod',
std_msgs.msg.Bool)
self._sub_state = rospy.Subscriber(
'/sdk/robot/head/head_state',
baxter_msgs.msg.HeadState,
self._on_head_state)
dataflow.wait_for(
lambda: len(self._state) != 0,
timeout=5.0,
timeout_msg="Failed to get current head state from /sdk/robot/head/head_state",
)
def __init__(self):
self.status_changed = dataflow.Signal()
self._status = UpdateStatus()
self._avail_updates = UpdateSources()
self._update_sources = rospy.Subscriber(
'/usb/update_sources',
UpdateSources,
self._on_update_sources)
self._updater_status_sub = rospy.Subscriber(
'/updater/status',
UpdateStatus,
self._on_update_status)
self._updater_start = rospy.Publisher(
'/updater/start',
std_msgs.msg.String)
self._updater_stop = rospy.Publisher(
'/updater/stop',
std_msgs.msg.Empty)
dataflow.wait_for(
lambda: self._avail_updates.uuid != '',
timeout = 1.0,
timeout_msg = "Failed to get list of available updates")
def __init__(self, component_id):
"""
@param component_id - unique id of the digital component
"""
self._id = component_id
self._component_type = 'digital_io'
self._is_output = False
self._state = {}
type_ns = '/sdk/robot/' + self._component_type
topic_base = type_ns + '/' + self._id
self._sub_state = rospy.Subscriber(
topic_base + '/state',
DigitalIOState,
self._on_io_state)
dataflow.wait_for(
lambda: len(self._state.keys()) != 0,
timeout=2.0,
timeout_msg="Failed to get current digital_io state from %s" \
% (topic_base,),
)
# check if output-capable before creating publisher
if self._is_output:
self._pub_output = rospy.Publisher(
type_ns + '/command',
DigitalOutputCommand)
def __init__(self, gripper):
"""
Interface class for a gripper on the Baxter robot.
@param gripper - gripper to interface
"""
self.name = gripper
ns = '/robot/limb/' + self.name + '/accessory/gripper/'
sdkns = '/sdk' + ns
self._pub_enable = rospy.Publisher(
ns + 'set_enabled',
stdmsg.Bool)
self._pub_reset = rospy.Publisher(
ns + 'command_reset',
stdmsg.Bool)
self._pub_calibrate = rospy.Publisher(
ns + 'command_calibrate',
stdmsg.Empty)
self._pub_command = rospy.Publisher(
sdkns + 'command_set',
baxmsg.GripperCommand)
self._sub_identity = rospy.Subscriber(
sdkns + 'identity',
baxmsg.GripperIdentity,
self._on_gripper_identity)
self._sub_properties = rospy.Subscriber(
sdkns + 'properties',
baxmsg.GripperProperties,
self._on_gripper_properties)
self._sub_state = rospy.Subscriber(
sdkns + 'state',
baxmsg.GripperState,
self._on_gripper_state)
self._command = baxmsg.GripperCommand(
position=0.0,
force=30.0,
velocity=100.0,
holding=0.0,
deadZone=3.0)
self._identity = baxmsg.GripperIdentity()
self._properties = baxmsg.GripperProperties()
self._state = None
dataflow.wait_for(
lambda: not self._state is None,
timeout=5.0,
timeout_msg="Failed to get current gripper state from %s" % (sdkns + 'state'),
)
def _toggle_enabled(self, status):
pub = rospy.Publisher('/robot/set_super_enable', std_msgs.msg.Bool)
dataflow.wait_for(
test=lambda: self._state.enabled == status,
timeout=2.0 if status else 5.0,
timeout_msg="Failed to %sable robot" % ('en' if status else 'dis',),
body=lambda: pub.publish(status),
)
def reboot(self, timeout=2.0):
"""
Reboot the gripper
"""
self._pub_reset.publish(True)
dataflow.wait_for(
test=lambda: not self.calibrated(),
timeout=timeout,
body=lambda: self._pub_reset.publish(True)
)
def disable(self, timeout=2.0):
"""
Disable the gripper
"""
self._pub_enable.publish(False)
dataflow.wait_for(
test=lambda: not self.enabled(),
timeout=timeout,
body=lambda: self._pub_enable.publish(False)
)
def enable(self, timeout=2.0):
"""
Enable the gripper
"""
self._pub_enable.publish(True)
dataflow.wait_for(
test=lambda: self.enabled(),
timeout=timeout,
body=lambda: self._pub_enable.publish(True)
)
def reset(self, timeout=2.0):
"""
Reset the gripper
"""
self._pub_reset.publish(False)
dataflow.wait_for(
test=lambda: not self.error(),
timeout=timeout,
body=lambda: self._pub_reset.publish(False)
)
def _toggle_enabled(self, status):
pub = rospy.Publisher('/robot/set_super_enable', std_msgs.msg.Bool)
dataflow.wait_for(
test=lambda: self._state.enabled == status,
timeout=2.0 if status else 5.0,
timeout_msg="Failed to %sable robot" %
('en' if status else 'dis', ),
body=lambda: pub.publish(status),
)
def calibrate(self, timeout=5.0):
"""
Calibrate the gripper
"""
self._pub_calibrate.publish(stdmsg.Empty())
self.enable()
dataflow.wait_for(
test=lambda: self.calibrated(),
timeout=timeout,
body=lambda: self._pub_calibrate.publish(stdmsg.Empty())
)
def stop(self):
"""
Simulate an e-stop button being pressed. Robot must be reset to clear
the stopped state.
"""
pub = rospy.Publisher('/robot/set_super_stop', std_msgs.msg.Empty)
dataflow.wait_for(
test=lambda: self._state.stopped == True,
timeout=3.0,
timeout_msg="Failed to stop the robot",
body=lambda: pub.publish(),
)
def stop(self):
"""
Simulate an e-stop button being pressed. Robot must be reset to clear
the stopped state.
"""
pub = rospy.Publisher('/robot/set_super_stop', std_msgs.msg.Empty)
dataflow.wait_for(
test=lambda: self._state.stopped == True,
timeout=3.0,
timeout_msg="Failed to stop the robot",
body=lambda: pub.publish(),
)
def __init__(self):
self._state = None
topic = '/sdk/robot/state'
self._state_sub = rospy.Subscriber(topic,
baxter_msgs.msg.AssemblyState,
self._state_callback)
dataflow.wait_for(
lambda: not self._state is None,
timeout=2.0,
timeout_msg="Failed to get current robot state from %s" %
(topic, ),
)
def __init__(self):
self._state = None
topic = '/sdk/robot/state'
self._state_sub = rospy.Subscriber(
topic,
baxter_msgs.msg.AssemblyState,
self._state_callback)
dataflow.wait_for(
lambda: not self._state is None,
timeout=2.0,
timeout_msg="Failed to get current robot state from %s" % (topic,),
)
def __init__(self, limb):
"""
Interface class for a limb on the Baxter robot.
@param limb - limb to interface
"""
self.name = limb
self._joint_angle = {}
self._joint_velocity = {}
self._joint_effort = {}
self._cartesian_pose = {}
self._cartesian_velocity = {}
self._cartesian_effort = {}
self._joint_names = {
'left': ['left_s0', 'left_s1', 'left_e0', 'left_e1', \
'left_w0', 'left_w1', 'left_w2'],
'right': ['right_s0', 'right_s1', 'right_e0', 'right_e1', \
'right_w0', 'right_w1', 'right_w2']
}
ns = '/robot/limb/' + limb + '/'
sdkns = '/sdk' + ns
self._pub_joint_mode = rospy.Publisher(
ns + 'joint_command_mode',
baxter_msgs.msg.JointCommandMode)
self._pub_joint_position = rospy.Publisher(
ns + 'command_joint_angles',
baxter_msgs.msg.JointPositions)
self._pub_joint_velocity = rospy.Publisher(
ns + 'command_joint_velocities',
baxter_msgs.msg.JointVelocities)
self._last_state_time = None
self._state_rate = 0
joint_state_sub = rospy.Subscriber(
'/robot/joint_states',
sensor_msgs.msg.JointState,
self._on_joint_states)
cartesian_state_sub = rospy.Subscriber(
sdkns + 'endpoint/state',
baxter_msgs.msg.EndpointState,
self._on_endpoint_states)
dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0)
def __init__(self, limb):
"""
Interface class for a limb on the Baxter robot.
@param limb - limb to interface
"""
self.name = limb
self._joint_angle = {}
self._joint_velocity = {}
self._joint_effort = {}
self._cartesian_pose = {}
self._cartesian_velocity = {}
self._cartesian_effort = {}
self._joint_names = {
'left': ['left_s0', 'left_s1', 'left_e0', 'left_e1', \
'left_w0', 'left_w1', 'left_w2'],
'right': ['right_s0', 'right_s1', 'right_e0', 'right_e1', \
'right_w0', 'right_w1', 'right_w2']
}
ns = '/robot/limb/' + limb + '/'
sdkns = '/sdk' + ns
self._pub_joint_mode = rospy.Publisher(
ns + 'joint_command_mode', baxter_msgs.msg.JointCommandMode)
self._pub_joint_position = rospy.Publisher(
ns + 'command_joint_angles', baxter_msgs.msg.JointPositions)
self._pub_joint_velocity = rospy.Publisher(
ns + 'command_joint_velocities', baxter_msgs.msg.JointVelocities)
self._last_state_time = None
self._state_rate = 0
joint_state_sub = rospy.Subscriber('/robot/joint_states',
sensor_msgs.msg.JointState,
self._on_joint_states)
cartesian_state_sub = rospy.Subscriber(sdkns + 'endpoint/state',
baxter_msgs.msg.EndpointState,
self._on_endpoint_states)
dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0)
def openCamera(self):
if self._id == 'head_camera':
self._set_control_value(CameraControl.CAMERA_CONTROL_FLIP, True)
self._set_control_value(CameraControl.CAMERA_CONTROL_MIRROR, True)
ret = self._open_svc(self._id, self._settings)
if ret.err != 0:
raise OSError(ret.err, "Failed to open camera")
self._open = True
self._image_sub = rospy.Subscriber('/cameras/'+self._id+'/image', sensor_msgs.msg.Image, self._on_image_update)
dataflow.wait_for(
lambda: not (self._frameMsg) is None,
timeout=5.0,
timeout_msg="Failed to get current image",
)
def reset(self):
"""
Reset all joints. Trigger JRCP hardware to reset all faults. Disable
the robot.
"""
is_reset = lambda: self._state.enabled == False\
and self._state.stopped == False \
and self._state.error == False \
and self._state.estop_button == 0 \
and self._state.estop_source == 0
pub = rospy.Publisher('/robot/set_super_reset', std_msgs.msg.Empty)
dataflow.wait_for(
test=is_reset,
timeout=3.0,
timeout_msg="Failed to reset robot",
body=lambda: pub.publish(),
)
def reset(self):
"""
Reset all joints. Trigger JRCP hardware to reset all faults. Disable
the robot.
"""
is_reset = lambda: self._state.enabled == False\
and self._state.stopped == False \
and self._state.error == False \
and self._state.estop_button == 0 \
and self._state.estop_source == 0
pub = rospy.Publisher('/robot/set_super_reset', std_msgs.msg.Empty)
dataflow.wait_for(
test=is_reset,
timeout=3.0,
timeout_msg="Failed to reset robot",
body=lambda: pub.publish(),
)
def __init__(self, location):
if not location in self.__LOCATIONS:
raise AttributeError("Invalid Navigator name '%s'" % (location,))
self._id = location
self._state = None
self.button0_changed = dataflow.Signal()
self.button1_changed = dataflow.Signal()
self.button2_changed = dataflow.Signal()
self.wheel_changed = dataflow.Signal()
self._state_sub = rospy.Subscriber(
"/sdk/robot/itb/%s_itb/state" % (self._id,), baxter_msgs.msg.ITB, self._on_state
)
self._inner_led = digital_io.DigitalIO("%s_itb_light_inner" % (self._id,))
self._outer_led = digital_io.DigitalIO("%s_itb_light_outer" % (self._id,))
dataflow.wait_for(lambda: self._state != None)
def set_pan(self, angle, speed=100, timeout=10.0):
"""
Pan at the given speed to the desired angle.
@param angle (float) - Desired pan angle in radians.
@param speed (int) - Desired speed to pan at, range is 0-100 [100]
@param timeout (float) - Seconds to wait for the head to pan to the specified
angle. If 0, just command once and return. [0]
"""
msg = baxter_msgs.msg.HeadPanCommand(angle, speed)
self._pub_pan.publish(msg)
if not timeout == 0:
dataflow.wait_for(
lambda: abs(self.pan()-angle) <= settings.HEAD_PAN_ANGLE_TOLERANCE,
timeout=timeout,
rate=100,
timeout_msg="Failed to move head to pan command %f" % angle,
body=lambda: self._pub_pan.publish(msg)
)
def move_to_joint_positions(self, positions, timeout=15.0):
"""
@param positions dict({str:float}) - dictionary of joint_name:angle
@param timeout - seconds to wait for move to finish [15]
Commands the limb to the provided positions. Waits until the reported
joint state matches that specified.
"""
def genf(joint, angle):
def joint_diff():
return abs(angle - self._joint_angle[joint])
return joint_diff
diffs = [genf(j,a) for j,a in positions.items() if j in self._joint_angle]
dataflow.wait_for(
lambda: not any(diff() >= settings.JOINT_ANGLE_TOLERANCE for diff in diffs),
timeout=timeout,
rate=100,
body=lambda: self.set_joint_positions(positions)
)
def open_arms(self):
angles_l = [ 0.4, 0.2, -1.0, 0.4, 0.0, 0.4, 0.0]
angles_r = [-0.4, 0.2, 1.0, 0.4, 0.0, 0.4, 0.0]
left_queue = Queue.Queue()
right_queue = Queue.Queue()
left_thread = threading.Thread(
target=self.move_thread,
args=(self.left_arm,
dict(zip(self.left_arm.joint_names(), angles_l)),
left_queue)
)
right_thread = threading.Thread(
target=self.move_thread,
args=(self.right_arm,
dict(zip(self.right_arm.joint_names(), angles_r)),
right_queue)
)
left_thread.daemon = True
right_thread.daemon = True
left_thread.start()
right_thread.start()
dataflow.wait_for(
lambda: \
not (left_thread.is_alive() or right_thread.is_alive()),
timeout=20.0,
rate=10,
)
left_thread.join()
right_thread.join()
result = left_queue.get()
if not result == None:
raise left_queue.get()
result = right_queue.get()
if not result == None:
raise right_queue.get()
rospy.sleep(1.0)
def set_output(self, value, timeout=2.0):
"""
Control the state of the Digital Output.
@param value bool - new state {True, False} of the Output.
@param timeout (float) - Seconds to wait for the io to reflect command.
If 0, just command once and return. [0]
"""
if not self._is_output:
raise IOError(errno.EACCES, "Component is not an output [%s: %s]" % (self._component_type, self._id))
cmd = DigitalOutputCommand()
cmd.name = self._id
cmd.value = value
self._pub_output.publish(cmd)
if not timeout == 0:
dataflow.wait_for(
test=lambda: self.state() == value,
timeout=timeout,
rate=100,
timeout_msg=("Failed to command digital io to: %r" % (value,)),
body=lambda: self._pub_output.publish(cmd),
)
def close_arms(self):
angles_l = [-0.6, 0.2, -1.8, 0.4, 0.0, 0.4, 0.0]
angles_r = [0.6, 0.3, 1.8, 0.4, 0.0, 0.4, -0.3]
left_queue = Queue.Queue()
right_queue = Queue.Queue()
left_thread = threading.Thread(
target=self.move_thread,
args=(self.left_arm,
dict(zip(self.left_arm.joint_names(),
angles_l)), left_queue))
right_thread = threading.Thread(
target=self.move_thread,
args=(self.right_arm,
dict(zip(self.right_arm.joint_names(),
angles_r)), right_queue))
left_thread.daemon = True
right_thread.daemon = True
left_thread.start()
right_thread.start()
dataflow.wait_for(
lambda: \
not (left_thread.is_alive() or right_thread.is_alive()),
timeout=20.0,
rate=10,
)
left_thread.join()
right_thread.join()
result = left_queue.get()
if not result == None:
raise left_queue.get()
result = right_queue.get()
if not result == None:
raise right_queue.get()
rospy.sleep(1.0)
def move_to_joint_positions(self, positions, timeout=15.0):
"""
@param positions dict({str:float}) - dictionary of joint_name:angle
@param timeout - seconds to wait for move to finish [15]
Commands the limb to the provided positions. Waits until the reported
joint state matches that specified.
"""
def genf(joint, angle):
def joint_diff():
return abs(angle - self._joint_angle[joint])
return joint_diff
diffs = [
genf(j, a) for j, a in positions.items() if j in self._joint_angle
]
dataflow.wait_for(
lambda: not any(diff() >= settings.JOINT_ANGLE_TOLERANCE
for diff in diffs),
timeout=timeout,
rate=100,
body=lambda: self.set_joint_positions(positions))
def __init__(self, component_id):
"""
@param component_id - unique id of the digital component
"""
self._id = component_id
self._component_type = "digital_io"
self._is_output = False
self._state = {}
type_ns = "/sdk/robot/" + self._component_type
topic_base = type_ns + "/" + self._id
self._sub_state = rospy.Subscriber(topic_base + "/state", DigitalIOState, self._on_io_state)
dataflow.wait_for(
lambda: len(self._state.keys()) != 0,
timeout=2.0,
timeout_msg="Failed to get current digital_io state from %s" % (topic_base,),
)
# check if output-capable before creating publisher
if self._is_output:
self._pub_output = rospy.Publisher(type_ns + "/command", DigitalOutputCommand)
def set_output(self, value, timeout=2.0):
"""
Control the state of the Digital Output.
@param value bool - new state {True, False} of the Output.
@param timeout (float) - Seconds to wait for the io to reflect command.
If 0, just command once and return. [0]
"""
if not self._is_output:
raise IOError(errno.EACCES, "Component is not an output [%s: %s]" %
(self._component_type, self._id))
cmd = DigitalOutputCommand()
cmd.name = self._id
cmd.value = value
self._pub_output.publish(cmd)
if not timeout == 0:
dataflow.wait_for(
test=lambda: self.state() == value,
timeout=timeout,
rate=100,
timeout_msg=("Failed to command digital io to: %r" % (value,)),
body=lambda: self._pub_output.publish(cmd)
)
def _on_trajectory_action(self, goal):
joint_names = goal.trajectory.joint_names
trajectory_points = goal.trajectory.points
# Load parameters for trajectory
if not self._get_trajectory_parameters(joint_names):
self._server.set_aborted()
return
# Create a new discretized joint trajectory
num_points = len(trajectory_points)
if num_points == 0:
rospy.logerr("%s: Empty Trajectory" % (self._action_name, ))
self._server.set_aborted()
return
# If all time_from_start are zero,
# interject these based on default velocities
if all(pt.time_from_start.to_sec() == 0.0 for pt in trajectory_points):
last_point = self._get_current_position(joint_names)
move_time = 0.0
for point in trajectory_points:
diffs = map(operator.sub, point.positions, last_point)
diffs = map(operator.abs, diffs)
dflt_vel = [self._dflt_vel[jnt] for jnt in joint_names]
move_time = move_time + max(map(operator.div, diffs, dflt_vel))
point.time_from_start = rospy.Duration(move_time)
last_point = point.positions
def interp(a, b, pct):
return a + (b - a) * pct
def interp_positions(p1, p2, pct):
return map(interp, p1.positions, p2.positions,
[pct] * len(p1.positions))
end_time = trajectory_points[-1].time_from_start.to_sec()
control_rate = rospy.Rate(self._control_rate)
pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
# Wait for the specified execution time, if not provided use now
start_time = goal.trajectory.header.stamp.to_sec()
if start_time == 0.0:
start_time = rospy.get_time()
dataflow.wait_for(lambda: rospy.get_time() >= start_time,
timeout=float('inf'))
# Loop until end of trajectory time. Provide a single time step
# of the control rate past the end to ensure we get to the end.
now_from_start = rospy.get_time() - start_time
while now_from_start < end_time + (1.0 / self._control_rate):
idx = bisect.bisect(pnt_times, now_from_start)
if idx == 0:
# If our current time is before the first specified point
# in the trajectory, then we should interpolate between
# our current position and that point.
p1 = JointTrajectoryPoint()
p1.positions = self._get_current_position(joint_names)
else:
p1 = trajectory_points[idx - 1]
if idx != num_points:
p2 = trajectory_points[idx]
pct = ((now_from_start - p1.time_from_start.to_sec()) /
(p2.time_from_start - p1.time_from_start).to_sec())
point = interp_positions(p1, p2, pct)
else:
# If the current time is after the last trajectory point,
# just hold that position.
point = p1.positions
if not self._command_velocities(joint_names, point):
return
control_rate.sleep()
now_from_start = rospy.get_time() - start_time
# Keep trying to meet goal until goal_time constraint expired
last_point = trajectory_points[-1].positions
last_time = trajectory_points[-1].time_from_start.to_sec()
def check_goal_state():
for error in self._get_current_error(joint_names, last_point):
if (self._goal_error[error[0]] > 0
and self._goal_error[error[0]] < math.fabs(error[1])):
return error[0]
else:
return None
while ((rospy.get_time() < start_time + last_time + self._goal_time)
and check_goal_state()):
if not self._command_velocities(joint_names, last_point):
return
control_rate.sleep()
# Verify goal constraint
result = check_goal_state()
if result != None:
self._command_stop(goal.trajectory.joint_names)
rospy.logerr("%s: Exceeded Goal Threshold Error %s" % (
self._action_name,
result,
))
self._server.set_aborted()
else:
self._command_stop(goal.trajectory.joint_names)
self._server.set_succeeded()
class MoveArms(SmokeTest):
"""Move both arms to numerous joint positions.
"""
def __init__(self, name='MoveArms'):
super(MoveArms, self).__init__(name)
def start_test(self):
"""Runs MoveArms Smoke Test
"""
def move_thread(limb, angle, queue, timeout=15.0):
"""Threaded joint movement allowing for simultaneous joint moves
"""
try:
limb.move_to_joint_positions(angle, timeout)
queue.put(None)
except Exception, exception:
queue.put(traceback.format_exc())
queue.put(exception)
try:
print("Enabling robot...")
self._rs.enable()
print("Test: Create Limb Instances")
right = baxter_interface.Limb('right')
left = baxter_interface.Limb('left')
left_queue = Queue.Queue()
right_queue = Queue.Queue()
# Max Joint Range (s0, s1, e0, e1, w0, w1, w2)
# ( 1.701, 1.047, 3.054, 2.618, 3.059, 2.094, 3.059)
# Min Joint Range (e0, e1, s0, s1, w0, w1, w2)
# (-1.701, -2.147, -3.054, -0.050, -3.059, -1.571, -3.059)
joint_moves = (
[ 0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0],
[ 0.5, -0.8, 2.8, 0.15, 0.0, 1.9, 2.8],
[-0.1, -0.8,-1.0, 2.5, 0.0, -1.4, -2.8],
[ 0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0],
)
for move in joint_moves:
print(
"Test: Moving to Joint Positions: " + \
", ".join("%.2f" % x for x in move)
)
left_thread = threading.Thread(
target=move_thread,
args=(left, dict(zip(left.joint_names(), move)), left_queue)
)
right_thread = threading.Thread(
target=move_thread,
args=(right, dict(zip(right.joint_names(), move)), right_queue)
)
left_thread.daemon = True
right_thread.daemon = True
left_thread.start()
right_thread.start()
dataflow.wait_for(
lambda: \
not (left_thread.is_alive() or right_thread.is_alive()),
timeout=20.0,
rate=10,
)
left_thread.join()
right_thread.join()
result = left_queue.get()
if not result == None:
raise left_queue.get()
result = right_queue.get()
if not result == None:
raise right_queue.get()
rospy.sleep(1.0)
print("Disabling robot...")
self._rs.disable()
self.result[0] = True
rospy.sleep(5.0)
except:
self.return_failure(traceback.format_exc())
def _on_trajectory_action(self, goal):
joint_names = goal.trajectory.joint_names
trajectory_points = goal.trajectory.points
# Load parameters for trajectory
if not self._get_trajectory_parameters(joint_names):
self._server.set_aborted()
return
# Create a new discretized joint trajectory
num_points = len(trajectory_points)
if num_points == 0:
rospy.logerr("%s: Empty Trajectory" % (self._action_name,))
self._server.set_aborted()
return
# If all time_from_start are zero,
# interject these based on default velocities
if all(pt.time_from_start.to_sec() == 0.0 for pt in trajectory_points):
last_point = self._get_current_position(joint_names)
move_time = 0.0
for point in trajectory_points:
diffs = map(operator.sub, point.positions,
last_point)
diffs = map(operator.abs, diffs)
dflt_vel= [self._dflt_vel[jnt] for jnt in joint_names]
move_time = move_time + max(map(operator.div, diffs, dflt_vel))
point.time_from_start = rospy.Duration(move_time)
last_point = point.positions
def interp(a, b, pct):
return a + (b - a) * pct
def interp_positions(p1, p2, pct):
return map(interp, p1.positions, p2.positions, [pct] *
len(p1.positions))
end_time = trajectory_points[-1].time_from_start.to_sec()
control_rate = rospy.Rate(self._control_rate)
pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
# Wait for the specified execution time, if not provided use now
start_time = goal.trajectory.header.stamp.to_sec()
if start_time == 0.0:
start_time = rospy.get_time()
dataflow.wait_for(
lambda: rospy.get_time() >= start_time,
timeout=float('inf')
)
# Loop until end of trajectory time. Provide a single time step
# of the control rate past the end to ensure we get to the end.
now_from_start = rospy.get_time() - start_time
while now_from_start < end_time + (1.0 / self._control_rate):
idx = bisect.bisect(pnt_times, now_from_start)
if idx == 0:
# If our current time is before the first specified point
# in the trajectory, then we should interpolate between
# our current position and that point.
p1 = JointTrajectoryPoint()
p1.positions = self._get_current_position(joint_names)
else:
p1 = trajectory_points[idx - 1]
if idx != num_points:
p2 = trajectory_points[idx]
pct = ((now_from_start - p1.time_from_start.to_sec()) /
(p2.time_from_start - p1.time_from_start).to_sec())
point = interp_positions(p1, p2, pct)
else:
# If the current time is after the last trajectory point,
# just hold that position.
point = p1.positions
if not self._command_velocities(joint_names, point):
return
control_rate.sleep()
now_from_start = rospy.get_time() - start_time
# Keep trying to meet goal until goal_time constraint expired
last_point = trajectory_points[-1].positions
last_time = trajectory_points[-1].time_from_start.to_sec()
def check_goal_state():
for error in self._get_current_error(joint_names, last_point):
if (self._goal_error[error[0]] > 0
and self._goal_error[error[0]] < math.fabs(error[1])):
return error[0]
else:
return None
while ((rospy.get_time() < start_time + last_time + self._goal_time)
and check_goal_state()):
if not self._command_velocities(joint_names, last_point):
return
control_rate.sleep()
# Verify goal constraint
result = check_goal_state()
if result != None:
self._command_stop(goal.trajectory.joint_names)
rospy.logerr("%s: Exceeded Goal Threshold Error %s" %
(self._action_name, result,))
self._server.set_aborted()
else:
self._command_stop(goal.trajectory.joint_names)
self._server.set_succeeded()
updater.stop_update()
rospy.on_shutdown(on_shutdown)
updater.status_changed.connect(on_update_status)
try:
updater.command_update(uuid)
except OSError, e:
if e.errno == errno.EINVAL:
print(str(e))
return 1
raise
try:
dataflow.wait_for(
lambda: nl.done == True,
timeout = 5 * 60,
timeout_msg = "Timeout waiting for update to succeed")
except Exception, e:
if not (hasattr(e, 'errno') and e.errno == errno.ESHUTDOWN):
print (str(e))
nl.rc = 1
return nl.rc
def usage():
print """
%s [ARGUMENTS]
-h, --help This screen
-l, --list List available updates
-u, --update [UUID] Launch the given update
