def set_output(self, value, timeout=2.0):
"""
Control the state of the Digital Output.
Use this function for finer control over the wait_for timeout.
@type value: bool
@param value: new state {True, False} of the Output.
@type timeout: float
@param timeout: 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:
baxter_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 set_pan(self, angle, speed=100, timeout=10.0):
"""
Pan at the given speed to the desired angle.
@type angle: float
@param angle: Desired pan angle in radians.
@type speed: int
@param speed: Desired speed to pan at, range is 0-100 [100]
@type timeout: float
@param timeout: Seconds to wait for the head to pan to the
specified angle. If 0, just command once and
return. [10]
"""
msg = HeadPanCommand(angle, speed)
self._pub_pan.publish(msg)
if not timeout == 0:
baxter_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 __init__(self, limb):
"""
Constructor.
@type limb: str
@param limb: limb to interface
"""
self.name = limb
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._cartesian_pose = dict()
self._cartesian_velocity = dict()
self._cartesian_effort = dict()
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 + '/'
self._command_msg = JointCommand()
self._pub_speed_ratio = rospy.Publisher(
ns + 'set_speed_ratio',
Float64,
latch=True)
self._pub_joint_cmd = rospy.Publisher(
ns + 'joint_command',
JointCommand,
tcp_nodelay=True)
self._pub_joint_cmd_timeout = rospy.Publisher(
ns + 'joint_command_timeout',
Float64,
latch=True)
_cartesian_state_sub = rospy.Subscriber(
ns + 'endpoint_state',
EndpointState,
self._on_endpoint_states,
queue_size=1,
tcp_nodelay=True)
joint_state_topic = 'robot/joint_states'
_joint_state_sub = rospy.Subscriber(
joint_state_topic,
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
err_msg = ("%s limb init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
baxter_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
timeout_msg=err_msg)
def __init__(self, component_id):
"""
Constructor.
@param component_id: unique id of analog component
"""
self._id = component_id
self._component_type = 'analog_io'
self._is_output = False
self._state = dict()
type_ns = '/robot/' + self._component_type
topic_base = type_ns + '/' + self._id
self._sub_state = rospy.Subscriber(topic_base + '/state',
AnalogIOState, self._on_io_state)
baxter_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,
queue_size=10)
def __init__(self, versioned=False):
"""
Version checking capable constructor.
@type versioned: bool
@param versioned: True to check robot software version
compatibility on initialization. False (default) to ignore.
The compatibility of robot versions to SDK (baxter_interface)
versions is defined in the L{baxter_interface.VERSIONS_SDK2ROBOT}.
By default, the class does not check, but all examples do. The
example behavior can be overridden by changing the value of
L{baxter_interface.CHECK_VERSION} to False.
"""
self._state = None
state_topic = 'robot/state'
self._state_sub = rospy.Subscriber(state_topic,
AssemblyState,
self._state_callback
)
if versioned and not self.version_check():
sys.exit(1)
baxter_dataflow.wait_for(
lambda: not self._state is None,
timeout=2.0,
timeout_msg=("Failed to get robot state on %s" %
(state_topic,)),
)
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 = dict()
self._pub_pan = rospy.Publisher(
'/robot/head/command_head_pan',
HeadPanCommand)
self._pub_nod = rospy.Publisher(
'/robot/head/command_head_nod',
Bool)
state_topic = '/robot/head/head_state'
self._sub_state = rospy.Subscriber(
state_topic,
HeadState,
self._on_head_state)
baxter_dataflow.wait_for(
lambda: len(self._state) != 0,
timeout=5.0,
timeout_msg=("Failed to get current head state from %s" %
(state_topic,)),
)
def __init__(self, component_id):
"""
Constructor.
@param component_id: unique id of the digital component
"""
self._id = component_id
self._component_type = 'digital_io'
self._is_output = False
self._state = None
self.state_changed = baxter_dataflow.Signal()
type_ns = '/robot/' + self._component_type
topic_base = type_ns + '/' + self._id
self._sub_state = rospy.Subscriber(
topic_base + '/state',
DigitalIOState,
self._on_io_state)
baxter_dataflow.wait_for(
lambda: self._state != None,
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,
queue_size=10)
def __init__(self, versioned=False):
"""
Version checking capable constructor.
@type versioned: bool
@param versioned: True to check robot software version
compatibility on initialization. False (default) to ignore.
The compatibility of robot versions to SDK (baxter_interface)
versions is defined in the L{baxter_interface.VERSIONS_SDK2ROBOT}.
By default, the class does not check, but all examples do. The
example behavior can be overridden by changing the value of
L{baxter_interface.CHECK_VERSION} to False.
"""
self._state = None
state_topic = 'robot/state'
self._state_sub = rospy.Subscriber(state_topic,
AssemblyState,
self._state_callback
)
if versioned and not self.version_check():
sys.exit(1)
baxter_dataflow.wait_for(
lambda: not self._state is None,
timeout=2.0,
timeout_msg=("Failed to get robot state on %s" %
(state_topic,)),
)
def __init__(self):
"""
Constructor.
"""
self._state = dict()
self._pub_pan = rospy.Publisher(
'/robot/head/command_head_pan',
HeadPanCommand,
queue_size=10)
self._pub_nod = rospy.Publisher(
'/robot/head/command_head_nod',
Bool,
queue_size=10)
state_topic = '/robot/head/head_state'
self._sub_state = rospy.Subscriber(
state_topic,
HeadState,
self._on_head_state)
baxter_dataflow.wait_for(
lambda: len(self._state) != 0,
timeout=5.0,
timeout_msg=("Failed to get current head state from %s" %
(state_topic,)),
)
def __init__(self, limb):
"""
Constructor.
@type limb: str
@param limb: limb to interface
"""
self.name = limb
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._cartesian_pose = dict()
self._cartesian_velocity = dict()
self._cartesian_effort = dict()
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 + '/'
self._command_msg = JointCommand()
self._pub_speed_ratio = rospy.Publisher(ns + 'set_speed_ratio',
Float64,
latch=True)
self._pub_joint_cmd = rospy.Publisher(ns + 'joint_command',
JointCommand,
tcp_nodelay=True)
self._pub_joint_cmd_timeout = rospy.Publisher(ns +
'joint_command_timeout',
Float64,
latch=True)
_cartesian_state_sub = rospy.Subscriber(ns + 'endpoint_state',
EndpointState,
self._on_endpoint_states,
queue_size=1,
tcp_nodelay=True)
joint_state_topic = 'robot/joint_states'
_joint_state_sub = rospy.Subscriber(joint_state_topic,
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
err_msg = ("%s limb init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
baxter_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
timeout_msg=err_msg)
def __init__(self):
self.status_changed = baxter_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,
queue_size=10)
self._updater_stop = rospy.Publisher(
'/updater/stop',
std_msgs.msg.Empty,
queue_size=10)
baxter_dataflow.wait_for(
lambda: self._avail_updates.uuid != '',
timeout=5.0,
timeout_msg="Failed to get list of available updates"
)
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()
baxter_dataflow.wait_for(
lambda: not (left_thread.is_alive() or
right_thread.is_alive()),
timeout=20.0,
timeout_msg=("Timeout while waiting for arm move threads"
" to finish"),
rate=10,
)
left_thread.join()
right_thread.join()
def move_to_joint_positions(self, positions, joint_tolerance=0.015,speed_tolerance=0.015,timeout=10.0):
"""
Commands the limb to the provided positions.
:param positions: joint_name:angle command
:type positions: dict({str:float})
:param joint_tolerance: Desired maximum goal tolerance in radians
:type joint_tolerance: float
:param timeout: seconds to wait for move to finish
:type timeout: float
Waits until the reported joint state matches that specified.
"""
self._cancel=False
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]
baxter_dataflow.wait_for(
lambda: (all(diff() < joint_tolerance
for diff in diffs) or self._cancel),
timeout=timeout,
timeout_msg=("%s limb failed to reach commanded joint positions" %
(self.name.capitalize(),)),
rate=DEFAULT_RATE,
body=lambda: self.set_joint_positions(positions)
)
return True
def __init__(self):
self.status_changed = baxter_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,
queue_size=10)
self._updater_stop = rospy.Publisher(
'/updater/stop',
std_msgs.msg.Empty,
queue_size=10)
baxter_dataflow.wait_for(
lambda: self._avail_updates.uuid != '',
timeout=5.0,
timeout_msg="Failed to get list of available updates"
)
def __init__(self, component_id):
"""
Constructor.
@param component_id: unique id of analog component
"""
self._id = component_id
self._component_type = 'analog_io'
self._is_output = False
self._state = dict()
type_ns = '/robot/' + self._component_type
topic_base = type_ns + '/' + self._id
self._sub_state = rospy.Subscriber(
topic_base + '/state',
AnalogIOState,
self._on_io_state)
baxter_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,
queue_size=10)
def set_output(self, value, timeout=2.0):
"""
Control the state of the Analog Output.
@type value: uint16
@param value: new state of the Output.
@type timeout: float
@param timeout: 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 = AnalogOutputCommand()
cmd.name = self._id
cmd.value = value
self._pub_output.publish(cmd)
if not timeout == 0:
baxter_dataflow.wait_for(
test=lambda: self.state() == value,
timeout=timeout,
rate=100,
timeout_msg=("Failed to command analog io to: %d" % (value,)),
body=lambda: self._pub_output.publish(cmd)
)
def __init__(self):
"""
Constructor.
"""
self._state = dict()
self._pub_pan = rospy.Publisher(
'/robot/head/command_head_pan',
HeadPanCommand)
self._pub_nod = rospy.Publisher(
'/robot/head/command_head_nod',
Bool)
state_topic = '/robot/head/head_state'
self._sub_state = rospy.Subscriber(
state_topic,
HeadState,
self._on_head_state)
baxter_dataflow.wait_for(
lambda: len(self._state) != 0,
timeout=5.0,
timeout_msg=("Failed to get current head state from %s" %
(state_topic,)),
)
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 = baxter_dataflow.Signal()
self.button1_changed = baxter_dataflow.Signal()
self.button2_changed = baxter_dataflow.Signal()
self.wheel_changed = baxter_dataflow.Signal()
nav_state_topic = 'robot/itb/%s_itb/state' % (self._id,)
self._state_sub = rospy.Subscriber(
nav_state_topic,
ITBState,
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,))
init_err_msg = ("Navigator init failed to get current state from %s" %
(nav_state_topic,))
baxter_dataflow.wait_for(lambda: self._state != None,
timeout_msg=init_err_msg)
def command_nod(self, timeout=5.0):
"""
Command the head to nod once.
@type timeout: float
@param timeout: 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
baxter_dataflow.wait_for(
test=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
baxter_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(False)
)
def __init__(self, location):
"""
Constructor.
@type location: str
@param location: body location (prefix) of Navigator to control.
Valid locations are in L{Navigator.__LOCATIONS}::
left, right, torso_left, torso_right
"""
if not location in self.__LOCATIONS:
raise AttributeError("Invalid Navigator name '%s'" % (location,))
self._id = location
self._state = None
self.button0_changed = baxter_dataflow.Signal()
self.button1_changed = baxter_dataflow.Signal()
self.button2_changed = baxter_dataflow.Signal()
self.wheel_changed = baxter_dataflow.Signal()
nav_state_topic = 'robot/itb/%s_itb/state' % (self._id,)
self._state_sub = rospy.Subscriber(
nav_state_topic,
ITBState,
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,))
init_err_msg = ("Navigator init failed to get current state from %s" %
(nav_state_topic,))
baxter_dataflow.wait_for(lambda: self._state != None,
timeout_msg=init_err_msg)
def command_nod(self, timeout=5.0):
"""
Command the head to nod once.
@type timeout: float
@param timeout: 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
baxter_dataflow.wait_for(
test=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
baxter_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(False))
def MoveToJointPositions(limb, moves, queue, write = True):
try:
for move in moves:
thread = threading.Thread(
target=move_thread,
args=(limb,move, queue, write)
)
if (move.values()):
thread.daemon = True
thread.start()
baxter_dataflow.wait_for(
lambda: not (thread.is_alive()),
timeout=20.0,
timeout_msg=("Timeout while waiting for %s move thread"
" to finish" % limb.name),
rate=10,
)
thread.join()
result = queue.get()
if not result is None:
raise queue.get()
rospy.sleep(1.0)
except Exception, exception:
queue.put(traceback.format_exc())
queue.put(exception)
def __init__(self, location):
"""
Constructor.
@type location: str
@param location: body location (prefix) of Navigator to control.
Valid locations are in L{Navigator.__LOCATIONS}::
left, right, torso_left, torso_right
"""
if not location in self.__LOCATIONS:
raise AttributeError("Invalid Navigator name '%s'" % (location,))
self._id = location
self._state = None
self.button0_changed = baxter_dataflow.Signal()
self.button1_changed = baxter_dataflow.Signal()
self.button2_changed = baxter_dataflow.Signal()
self.wheel_changed = baxter_dataflow.Signal()
nav_state_topic = 'robot/itb/%s_itb/state' % (self._id,)
self._state_sub = rospy.Subscriber(
nav_state_topic,
ITBState,
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,))
init_err_msg = ("Navigator init failed to get current state from %s" %
(nav_state_topic,))
baxter_dataflow.wait_for(lambda: self._state != None,
timeout_msg=init_err_msg)
def set_output(self, value, timeout=2.0):
"""
Control the state of the Analog Output.
@type value: uint16
@param value: new state of the Output.
@type timeout: float
@param timeout: 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 = AnalogOutputCommand()
cmd.name = self._id
cmd.value = value
self._pub_output.publish(cmd)
if not timeout == 0:
baxter_dataflow.wait_for(
test=lambda: self.state() == value,
timeout=timeout,
rate=100,
timeout_msg=("Failed to command analog io to: %d" % (value, )),
body=lambda: self._pub_output.publish(cmd))
def set_output(self, value, timeout=2.0):
"""
Control the state of the Digital Output.
Use this function for finer control over the wait_for timeout.
@type value: bool
@param value: new state {True, False} of the Output.
@type timeout: float
@param timeout: 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:
baxter_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 __init__(self, component_id):
"""
Constructor.
@param component_id: unique id of the digital component
"""
self._id = component_id
self._component_type = 'digital_io'
self._is_output = False
self._state = None
self.state_changed = baxter_dataflow.Signal()
type_ns = '/robot/' + self._component_type
topic_base = type_ns + '/' + self._id
self._sub_state = rospy.Subscriber(topic_base + '/state',
DigitalIOState, self._on_io_state)
baxter_dataflow.wait_for(
lambda: self._state != None,
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,
queue_size=10)
def __init__(self):
self._state = None
state_topic = 'robot/state'
self._state_sub = rospy.Subscriber(state_topic, AssemblyState,
self._state_callback)
baxter_dataflow.wait_for(
lambda: not self._state is None,
timeout=2.0,
timeout_msg=("Failed to get robot state on %s" % (state_topic, )),
)
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', Empty, queue_size=10)
baxter_dataflow.wait_for(
test=lambda: self._state.stopped == True,
timeout=3.0,
timeout_msg="Failed to stop the robot",
body=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', Empty)
baxter_dataflow.wait_for(
test=lambda: self._state.stopped == True,
timeout=3.0,
timeout_msg="Failed to stop the robot",
body=pub.publish,
)
def _toggle_enabled(self, status):
pub = rospy.Publisher('robot/set_super_enable', Bool)
baxter_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),
)
rospy.loginfo("Robot %s", ('Enabled' if status else 'Disabled'))
def move_to_joint_positions(self, positions, joint_tolerance=0.015,speed_tolerance=0.015,timeout=10.0,stop_condition=None):
"""
Commands the limb to the provided positions.
:param positions: joint_name:angle command
:type positions: dict({str:float})
:param joint_tolerance: Desired maximum goal tolerance in radians
:type joint_tolerance: float
:param timeout: seconds to wait for move to finish
:type timeout: float
Waits until the reported joint state matches that specified.
"""
if self.__stop:
return False
self._cancel=False
if stop_condition is None:
stop_condition=lambda arm: False
cmd = self.joint_angles()
def filtered_cmd():
# First Order Filter - 0.2 Hz Cutoff
for joint in positions.keys():
# cmd[joint] = 0.012488 * positions[joint] + 0.98751 * cmd[joint]
cmd[joint] = 0.5 * positions[joint] + 0.5 * cmd[joint]
return cmd
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]
self.set_joint_positions(filtered_cmd())
baxter_dataflow.wait_for(
lambda: (all(diff() < joint_tolerance
for diff in diffs) or self._cancel or stop_condition(self)),
timeout=timeout,
timeout_msg=("%s limb failed to reach commanded joint positions" %
(self.name.capitalize(),)),
rate=DEFAULT_RATE,
body=lambda: self.set_joint_positions(filtered_cmd())
)
if self._cancel:
return False
return True
def command(self,
cmd,
block=False,
test=lambda: True,
timeout=0.0,
args=None):
"""
Raw command call to directly control gripper.
@type cmd: str
@param cmd: string of known gripper commands
@type block: bool
@param block: command is blocking or non-blocking [False]
@type test: func
@param test: test function for command validation
@type timeout: float
@param timeout: timeout in seconds for command evaluation
@type args: dict({str:float})
@param args: dictionary of parameter:value
"""
ee_cmd = EndEffectorCommand()
ee_cmd.id = self.hardware_id()
ee_cmd.command = cmd
ee_cmd.sender = self._cmd_sender % (cmd, )
ee_cmd.sequence = self._inc_cmd_sequence()
ee_cmd.args = ''
if args != None:
ee_cmd.args = JSONEncoder().encode(args)
seq_test = lambda: (self._state.command_sender == ee_cmd.sender and
(self._state.command_sequence == ee_cmd.sequence or
self._state.command_sequence == 0))
self._cmd_pub.publish(ee_cmd)
if block:
finish_time = rospy.get_time() + timeout
cmd_seq = baxter_dataflow.wait_for(
test=seq_test,
timeout=timeout,
raise_on_error=False,
body=lambda: self._cmd_pub.publish(ee_cmd))
if not cmd_seq:
seq_msg = (("Timed out on gripper command acknowledgement for"
" %s:%s") % (self.name, ee_cmd.command))
rospy.logdebug(seq_msg)
time_remain = max(0.5, finish_time - rospy.get_time())
return baxter_dataflow.wait_for(
test=test,
timeout=time_remain,
raise_on_error=False,
body=lambda: self._cmd_pub.publish(ee_cmd))
else:
return True
def move_to_joint_positions(self,
limb,
positions,
timeout=15.0,
move_rate=0.982,
threshold=settings.JOINT_ANGLE_TOLERANCE,
test=None):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter to smooth the movement.
@type positions: dict({str:float})
@param positions: joint_name:angle command
@type timeout: float
@param timeout: seconds to wait for move to finish [15]
@type threshold: float
@param threshold: position threshold in radians across each joint when
move is considered successful [0.008726646]
@param test: optional function returning True if motion must be aborted
"""
cmd = limb.joint_angles()
def filtered_cmd():
# First Order Filter - 0.2 Hz Cutoff
for joint in positions.keys():
# cmd[joint] = 0.012488 * positions[joint] + 0.98751 * cmd[joint]
cmd[joint] = (
1 - move_rate) * positions[joint] + move_rate * cmd[joint]
return cmd
def genf(joint, angle):
def joint_diff():
return abs(angle - limb._joint_angle[joint])
return joint_diff
diffs = [
genf(j, a) for j, a in positions.items() if j in limb._joint_angle
]
limb.set_joint_positions(filtered_cmd())
baxter_dataflow.wait_for(
test=lambda: callable(test) and test() == True or \
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg=("%s limb failed to reach commanded joint positions" %
(limb.name.capitalize(),)),
rate=100,
raise_on_error=False,
body=lambda: limb.set_joint_positions(filtered_cmd())
)
def __init__(self):
self._state = None
state_topic = 'robot/state'
self._state_sub = rospy.Subscriber(state_topic,
AssemblyState,
self._state_callback
)
baxter_dataflow.wait_for(
lambda: not self._state is None,
timeout=2.0,
timeout_msg=("Failed to get robot state on %s" %
(state_topic,)),
)
def command(self, cmd, block=False, test=lambda: True,
timeout=0.0, args=None):
"""
Raw command call to directly control gripper.
@type cmd: str
@param cmd: string of known gripper commands
@type block: bool
@param block: command is blocking or non-blocking [False]
@type test: func
@param test: test function for command validation
@type timeout: float
@param timeout: timeout in seconds for command evaluation
@type args: dict({str:float})
@param args: dictionary of parameter:value
"""
ee_cmd = EndEffectorCommand()
ee_cmd.id = self.hardware_id()
ee_cmd.command = cmd
ee_cmd.sender = self._cmd_sender % (cmd,)
ee_cmd.sequence = self._inc_cmd_sequence()
ee_cmd.args = ''
if args != None:
ee_cmd.args = JSONEncoder().encode(args)
seq_test = lambda: (self._state.command_sender == ee_cmd.sender and
(self._state.command_sequence == ee_cmd.sequence
or self._state.command_sequence == 0))
self._cmd_pub.publish(ee_cmd)
if block:
finish_time = rospy.get_time() + timeout
cmd_seq = baxter_dataflow.wait_for(
test=seq_test,
timeout=timeout,
raise_on_error=False,
body=lambda: self._cmd_pub.publish(ee_cmd)
)
if not cmd_seq:
seq_msg = (("Timed out on gripper command acknowledgement for"
" %s:%s") % (self.name, ee_cmd.command))
rospy.logdebug(seq_msg)
time_remain = max(0.5, finish_time - rospy.get_time())
return baxter_dataflow.wait_for(
test=test,
timeout=time_remain,
raise_on_error=False,
body=lambda: self._cmd_pub.publish(ee_cmd)
)
else:
return True
def __init__(self, gripper):
"""
Interface class for a gripper on the Baxter Research Robot.
@param gripper: robot limb <left/right> on which the gripper
is mounted
"""
self.name = gripper + '_gripper'
self._cmd_sender = rospy.get_name() + '_%s'
self._cmd_sequence = 0
ns = '/robot/end_effector/' + self.name + "/"
self._state = None
self._prop = EndEffectorProperties()
self._parameters = dict()
self._cmd_pub = rospy.Publisher(ns + 'command', EndEffectorCommand)
self._state_sub = rospy.Subscriber(
ns + 'state',
EndEffectorState,
self._on_gripper_state
)
self._prop_sub = rospy.Subscriber(
ns + 'properties',
EndEffectorProperties,
self._on_gripper_prop
)
# Wait for the gripper state message to be populated
baxter_dataflow.wait_for(
lambda: not self._state is None,
timeout=5.0,
timeout_msg=("Failed to get state from %s" %
(ns + 'state',))
)
# Wait for the gripper type to be populated
baxter_dataflow.wait_for(
lambda: not self.type() is None,
timeout=5.0,
timeout_msg=("Failed to get properties from %s" %
(ns + 'properties',))
)
self.set_parameters(defaults=True)
def reset_custom_state(self, timeout=2.0):
"""
Resets default state for custom grippers
@return: True if custom gripper state reset successfully
@rtype: bool
"""
state_true = EndEffectorState.STATE_TRUE
state_unknown = EndEffectorState.STATE_UNKNOWN
# Create default state message
state_msg = EndEffectorState()
for idx, attr in enumerate(state_msg.__slots__):
if 'int' in state_msg._slot_types[idx]:
setattr(state_msg, attr, state_unknown)
setattr(state_msg, 'enabled', state_true)
self._state_pub.publish(state_msg)
# Verify state reset successfully
test = lambda: (self._state.enabled == state_true and
self._state.calibrated == state_unknown and
self._state.ready == state_unknown and
self._state.position == 0.0
)
return baxter_dataflow.wait_for(
test=test,
timeout=timeout,
raise_on_error=False,
body=lambda: self._state_pub.publish(state_msg)
)
def reset_custom_properties(self, timeout=2.0):
"""
Resets default properties for custom grippers
@return: True if custom gripper properties reset successfully
@rtype: bool
"""
default_id = 131073
default_ui_type = EndEffectorProperties.PASSIVE_GRIPPER
default_manufacturer = 'Rethink Research Robot'
default_product = 'SDK End Effector'
# Create default properties message
prop_msg = EndEffectorProperties(
id=default_id,
ui_type=default_ui_type,
manufacturer=default_manufacturer,
product=default_product,
)
for idx, attr in enumerate(prop_msg.__slots__):
if prop_msg._slot_types[idx] == 'bool':
setattr(prop_msg, attr, True)
self._prop_pub.publish(prop_msg)
# Verify properties reset successfully
test = lambda: (self._prop.id == default_id and self._prop.ui_type ==
default_ui_type and self._prop.manufacturer ==
default_manufacturer and self._prop.product ==
default_product)
return baxter_dataflow.wait_for(
test=test,
timeout=timeout,
raise_on_error=False,
body=lambda: self._prop_pub.publish(prop_msg))
def reset_custom_state(self, timeout=2.0):
"""
Resets default state for custom grippers
@return: True if custom gripper state reset successfully
@rtype: bool
"""
state_true = EndEffectorState.STATE_TRUE
state_unknown = EndEffectorState.STATE_UNKNOWN
# Create default state message
state_msg = EndEffectorState()
for idx, attr in enumerate(state_msg.__slots__):
if 'int' in state_msg._slot_types[idx]:
setattr(state_msg, attr, state_unknown)
setattr(state_msg, 'enabled', state_true)
self._state_pub.publish(state_msg)
# Verify state reset successfully
test = lambda: (self._state.enabled == state_true and self._state.
calibrated == state_unknown and self._state.ready ==
state_unknown and self._state.position == 0.0)
return baxter_dataflow.wait_for(
test=test,
timeout=timeout,
raise_on_error=False,
body=lambda: self._state_pub.publish(state_msg))
def reset_custom_properties(self, timeout=2.0):
"""
Resets default properties for custom grippers
@return: True if custom gripper properties reset successfully
@rtype: bool
"""
default_id = 131073
default_ui_type = EndEffectorProperties.PASSIVE_GRIPPER
default_manufacturer = 'Rethink Research Robot'
default_product = 'SDK End Effector'
# Create default properties message
prop_msg = EndEffectorProperties(
id=default_id,
ui_type=default_ui_type,
manufacturer=default_manufacturer,
product=default_product,
)
for idx, attr in enumerate(prop_msg.__slots__):
if prop_msg._slot_types[idx] == 'bool':
setattr(prop_msg, attr, True)
self._prop_pub.publish(prop_msg)
# Verify properties reset successfully
test = lambda: (self._prop.id == default_id and
self._prop.ui_type == default_ui_type and
self._prop.manufacturer == default_manufacturer and
self._prop.product == default_product
)
return baxter_dataflow.wait_for(
test=test,
timeout=timeout,
raise_on_error=False,
body=lambda: self._prop_pub.publish(prop_msg)
)
def move_to_joint_positions(self, positions, timeout=15.0,
threshold=settings.JOINT_ANGLE_TOLERANCE,
test=None):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter to smooth the movement.
@type positions: dict({str:float})
@param positions: joint_name:angle command
@type timeout: float
@param timeout: seconds to wait for move to finish [15]
@type threshold: float
@param threshold: position threshold in radians across each joint when
move is considered successful [0.008726646]
@param test: optional function returning True if motion must be aborted
"""
cmd = self.joint_angles()
def filtered_cmd():
# First Order Filter - 0.2 Hz Cutoff
for joint in positions.keys():
cmd[joint] = 0.012488 * positions[joint] + 0.98751 * cmd[joint]
return cmd
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]
self.set_joint_positions(filtered_cmd())
baxter_dataflow.wait_for(
test=lambda: callable(test) and test() == True or \
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg=("%s limb failed to reach commanded joint positions" %
(self.name.capitalize(),)),
rate=100,
raise_on_error=False,
body=lambda: self.set_joint_positions(filtered_cmd())
)
def obtainTorque():
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
lj = left.joint_names()
rj = right.joint_names()
sampleGripper('left')
return
move = [0.5, -0.8, 2.8, 0.15, 0.0, 1.9, 2.8]
left_queue = Queue.Queue()
try:
#print "move:",move
#print "Test: Moving to Joint Positions: ",
#print ", ".join("%.2f" % x for x in move)
#print dict(zip(left.joint_names(), move))
left_thread = threading.Thread(target=move_thread,
args=(left,
dict(zip(left.joint_names(),
move)), left_queue))
left_thread.daemon = True
left_thread.start()
baxter_dataflow.wait_for(
lambda: not (left_thread.is_alive()),
timeout=20.0,
timeout_msg=("Timeout while waiting for arm move threads"
" to finish"),
rate=10)
left_thread.join()
result = left_queue.get()
#print result
if not result is None:
raise left_queue.get()
rospy.sleep(1.0)
except Exception:
print 'Failure'
print kin.forward_position_kinematics()
print kin.jacobian_transpose()
def reset(self):
"""
Reset all joints. Trigger JRCP hardware to reset all faults. Disable
the robot.
"""
error_estop = """\
E-Stop is ASSERTED. Disengage E-Stop and then reset the robot.
"""
error_nonfatal = """Non-fatal Robot Error on reset.
Robot reset cleared stopped state and robot can be enabled, but a non-fatal
error persists. Check diagnostics or rethink.log for more info.
"""
error_env = """Failed to reset robot.
Please verify that the ROS_IP or ROS_HOSTNAME environment variables are set
and resolvable. For more information please visit:
http://sdk.rethinkrobotics.com/wiki/RSDK_Shell#Initialize
"""
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', Empty)
if (self._state.stopped and
self._state.estop_button == AssemblyState.ESTOP_BUTTON_PRESSED):
rospy.logfatal(error_estop)
raise IOError(errno.EREMOTEIO, "Failed to Reset: E-Stop Engaged")
rospy.loginfo("Resetting robot...")
try:
baxter_dataflow.wait_for(
test=is_reset,
timeout=3.0,
timeout_msg=error_env,
body=pub.publish
)
except OSError, e:
if e.errno == errno.ETIMEDOUT:
if self._state.error == True and self._state.stopped == False:
rospy.logwarn(error_nonfatal)
return False
raise
def reset(self):
"""
Reset all joints. Trigger JRCP hardware to reset all faults. Disable
the robot.
"""
error_estop = """\
E-Stop is ASSERTED. Disengage E-Stop and then reset the robot.
"""
error_nonfatal = """Non-fatal Robot Error on reset.
Robot reset cleared stopped state and robot can be enabled, but a non-fatal
error persists. Check diagnostics or rethink.log for more info.
"""
error_env = """Failed to reset robot.
Please verify that the ROS_IP or ROS_HOSTNAME environment variables are set
and resolvable. For more information please visit:
http://sdk.rethinkrobotics.com/wiki/RSDK_Shell#Initialize
"""
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', Empty, queue_size=10)
if (self._state.stopped and
self._state.estop_button == AssemblyState.ESTOP_BUTTON_PRESSED):
rospy.logfatal(error_estop)
raise IOError(errno.EREMOTEIO, "Failed to Reset: E-Stop Engaged")
rospy.loginfo("Resetting robot...")
try:
baxter_dataflow.wait_for(
test=is_reset,
timeout=3.0,
timeout_msg=error_env,
body=pub.publish
)
except OSError as e:
if e.errno == errno.ETIMEDOUT:
if self._state.error == True and self._state.stopped == False:
rospy.logwarn(error_nonfatal)
return False
raise
def _move_arm(self, joint_angles):
# Gets differences between desired and actual joint angles
def genf(joint, angle):
def joint_diff():
return abs(angle - self._left_arm.joint_angle(joint))
return joint_diff
diffs = [genf(j, a) for j, a in joint_angles.items()]
# Move arm until in correct position
self._left_arm.set_joint_positions(joint_angles, raw=True)
baxter_dataflow.wait_for(lambda:
(all(diff() < 0.008726646 for diff in diffs)),
timeout=2.0,
rate=100,
raise_on_error=False,
body=lambda: self._left_arm.
set_joint_positions(joint_angles, raw=True))
def _move_to_positions(limb, pos_dict):
'''
Moves the limb to the desired positions.
@param limb - The baxter_interface.Limb object to move
@param pos_dict - Dictionary of joint -> angle positions in degrees
Note, to change how fast it goes, you can call
>>> limb.set_joint_position_speed(ratio)
where ratio is a number between 0 and 1 with 1 being pretty fast and 0
being stopped.
'''
# Convert to radians first thing
for key in pos_dict:
pos_dict[key] = _deg_to_rad(pos_dict[key])
cmd = limb.joint_angles()
def filtered_cmd():
# First Order Filter - 0.2 Hz Cutoff
for joint, value in pos_dict.iteritems():
cmd[joint] = 0.07 * value + 0.93 * cmd[joint]
return cmd
diff = lambda joint, angle: abs(angle - limb.joint_angle(joint))
#threshold = baxter_interface.settings.JOINT_ANGLE_TOLERANCE
# We want a bigger threshold so that it doesn't wiggle at the end, but
# says "good enough" and goes to the next point
threshold = 0.02
timeout = 15.0 # seconds
# Otherwise, go there without the filter
#limb.set_joint_positions(filtered_cmd())
limb.set_joint_positions(pos_dict)
baxter_dataflow.wait_for(
lambda: (all(diff(j, a) < threshold for j, a in pos_dict.iteritems())),
timeout=timeout,
rate=100,
raise_on_error=False,
body=lambda: limb.set_joint_positions(pos_dict)
)
def __init__(self, gripper):
"""
Interface class for a gripper on the Baxter Research Robot.
@param gripper: robot limb <left/right> on which the gripper
is mounted
"""
self.name = gripper + '_gripper'
self._cmd_sender = rospy.get_name() + '_%s'
self._cmd_sequence = 0
ns = '/robot/end_effector/' + self.name + "/"
self._state = None
self._prop = EndEffectorProperties()
self._parameters = dict()
self._cmd_pub = rospy.Publisher(ns + 'command', EndEffectorCommand)
self._state_sub = rospy.Subscriber(ns + 'state', EndEffectorState,
self._on_gripper_state)
self._prop_sub = rospy.Subscriber(ns + 'properties',
EndEffectorProperties,
self._on_gripper_prop)
# Wait for the gripper state message to be populated
baxter_dataflow.wait_for(lambda: not self._state is None,
timeout=5.0,
timeout_msg=("Failed to get state from %s" %
(ns + 'state', )))
# Wait for the gripper type to be populated
baxter_dataflow.wait_for(
lambda: not self.type() is None,
timeout=5.0,
timeout_msg=("Failed to get properties from %s" %
(ns + 'properties', )))
self.set_parameters(defaults=True)
def ThreadMovements(left, right, left_queue, right_queue, left_angles, right_angles):
try:
left_thread = threading.Thread(
target=move_thread,
args=(left,left_angles,left_queue)
)
right_thread = threading.Thread(
target=move_thread,
args=(right,right_angles,right_queue)
)
left_thread.daemon = True
right_thread.daemon = True
left_thread.start()
right_thread.start()
baxter_dataflow.wait_for(
lambda: not (left_thread.is_alive() or
right_thread.is_alive()),
timeout=20.0,
timeout_msg=("Timeout while waiting for arm move threads"
" to finish"),
rate=10,
)
left_thread.join()
right_thread.join()
result = left_queue.get()
if not result is None:
raise left_queue.get()
result = right_queue.get()
if not result is None:
raise right_queue.get()
rospy.sleep(0.001)
except Exception:
print 'Failure'
def move(self, joints):
# rospy.loginfo('move')
left_queue = Queue.Queue()
right_queue = Queue.Queue()
left_thread = threading.Thread(target=self.move_thread,
args=('left', joints['left'], left_queue))
right_thread = threading.Thread(target=self.move_thread,
args=('right', joints['right'], right_queue))
left_thread.daemon = True
right_thread.daemon = True
left_thread.start()
right_thread.start()
baxter_dataflow.wait_for(
lambda: not (left_thread.is_alive() or
right_thread.is_alive()),
timeout=20.0,
timeout_msg=("Timeout while waiting for arm move threads"
" to finish"),
rate=10,
)
left_thread.join()
right_thread.join()
def move(self, joints):
# rospy.loginfo('move')
left_queue = Queue.Queue()
right_queue = Queue.Queue()
left_thread = threading.Thread(target=self.move_thread,
args=('left', joints['left'],
left_queue))
right_thread = threading.Thread(target=self.move_thread,
args=('right', joints['right'],
right_queue))
left_thread.daemon = True
right_thread.daemon = True
left_thread.start()
right_thread.start()
baxter_dataflow.wait_for(
lambda: not (left_thread.is_alive() or right_thread.is_alive()),
timeout=20.0,
timeout_msg=("Timeout while waiting for arm move threads"
" to finish"),
rate=10,
)
left_thread.join()
right_thread.join()
def set_pan(self, angle, speed=1.0, timeout=10.0, scale_speed=False):
"""
Pan at the given speed to the desired angle.
@type angle: float
@param angle: Desired pan angle in radians.
@type speed: int
@param speed: Desired speed to pan at, range is 0-1.0 [1.0]
@type timeout: float
@param timeout: Seconds to wait for the head to pan to the
specified angle. If 0, just command once and
return. [10]
@param scale_speed: Scale speed to pan at by a factor of 100,
to use legacy range between 0-100 [100]
"""
if scale_speed:
cmd_speed = speed / 100.0
else:
cmd_speed = speed
if (cmd_speed < HeadPanCommand.MIN_SPEED_RATIO
or cmd_speed > HeadPanCommand.MAX_SPEED_RATIO):
rospy.logerr(
("Commanded Speed, ({0}), outside of valid range"
" [{1}, {2}]").format(cmd_speed,
HeadPanCommand.MIN_SPEED_RATIO,
HeadPanCommand.MAX_SPEED_RATIO))
msg = HeadPanCommand(angle, cmd_speed, True)
self._pub_pan.publish(msg)
if not timeout == 0:
baxter_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):
"""
Commands the limb to the provided positions.
@type positions: dict({str:float})
@param positions: joint_name:angle command
@type timeout: float
@param timeout: seconds to wait for move to finish [15]
Waits until the reported joint state matches that specified.
"""
cmd = self.joint_angles()
def filtered_cmd():
# First Order Filter - 0.2 Hz Cutoff
for joint in positions.keys():
cmd[joint] = 0.012488 * positions[joint] + 0.98751 * cmd[joint]
return cmd
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
]
baxter_dataflow.wait_for(
lambda:
(all(diff() < settings.JOINT_ANGLE_TOLERANCE for diff in diffs)),
timeout=timeout,
timeout_msg=("%s limb failed to reach commanded joint positions" %
(self.name.capitalize(), )),
rate=100,
body=lambda: self.set_joint_positions(filtered_cmd()))
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 = dict()
self._pub_pan = rospy.Publisher('/robot/head/command_head_pan',
HeadPanCommand)
self._pub_nod = rospy.Publisher('/robot/head/command_head_nod', Bool)
state_topic = '/robot/head/head_state'
self._sub_state = rospy.Subscriber(state_topic, HeadState,
self._on_head_state)
baxter_dataflow.wait_for(
lambda: len(self._state) != 0,
timeout=5.0,
timeout_msg=("Failed to get current head state from %s" %
(state_topic, )),
)
def set_pan(self, angle, speed=1.0, timeout=10.0, scale_speed=False):
"""
Pan at the given speed to the desired angle.
@type angle: float
@param angle: Desired pan angle in radians.
@type speed: int
@param speed: Desired speed to pan at, range is 0-1.0 [1.0]
@type timeout: float
@param timeout: Seconds to wait for the head to pan to the
specified angle. If 0, just command once and
return. [10]
@param scale_speed: Scale speed to pan at by a factor of 100,
to use legacy range between 0-100 [100]
"""
if scale_speed:
cmd_speed = speed / 100.0
else:
cmd_speed = speed
if cmd_speed < HeadPanCommand.MIN_SPEED_RATIO or cmd_speed > HeadPanCommand.MAX_SPEED_RATIO:
rospy.logerr(
("Commanded Speed, ({0}), outside of valid range" " [{1}, {2}]").format(
cmd_speed, HeadPanCommand.MIN_SPEED_RATIO, HeadPanCommand.MAX_SPEED_RATIO
)
)
msg = HeadPanCommand(angle, cmd_speed, True)
self._pub_pan.publish(msg)
if not timeout == 0:
baxter_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):
"""
Commands the limb to the provided positions.
@type positions: dict({str:float})
@param positions: joint_name:angle command
@type timeout: float
@param timeout: seconds to wait for move to finish [15]
Waits until the reported joint state matches that specified.
"""
cmd = self.joint_angles()
def filtered_cmd():
# First Order Filter - 0.2 Hz Cutoff
for joint in positions.keys():
cmd[joint] = 0.012488 * positions[joint] + 0.98751 * cmd[joint]
return cmd
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]
baxter_dataflow.wait_for(
lambda: (all(diff() < settings.JOINT_ANGLE_TOLERANCE
for diff in diffs)),
timeout=timeout,
timeout_msg=("%s limb failed to reach commanded joint positions" %
(self.name.capitalize(),)),
rate=100,
body=lambda: self.set_joint_positions(filtered_cmd())
)
def set_pan(self, angle, speed=100, timeout=10.0):
"""
Pan at the given speed to the desired angle.
@type angle: float
@param angle: Desired pan angle in radians.
@type speed: int
@param speed: Desired speed to pan at, range is 0-100 [100]
@type timeout: float
@param timeout: Seconds to wait for the head to pan to the
specified angle. If 0, just command once and
return. [10]
"""
msg = HeadPanCommand(angle, speed)
self._pub_pan.publish(msg)
if not timeout == 0:
baxter_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 _on_trajectory_action(self, goal):
joint_names = goal.trajectory.joint_names
trajectory_points = goal.trajectory.points
# Load parameters for trajectory
self._get_trajectory_parameters(joint_names, goal)
# 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
rospy.loginfo("%s: Executing requested joint trajectory" % (self._action_name,))
rospy.logdebug("Trajectory Points: {0}".format(trajectory_points))
control_rate = rospy.Rate(self._control_rate)
dimensions_dict = self._determine_dimensions(trajectory_points)
if num_points == 1:
# Add current position as trajectory point
first_trajectory_point = JointTrajectoryPoint()
first_trajectory_point.positions = self._get_current_position(joint_names)
# To preserve desired velocities and accelerations, copy them to the first
# trajectory point if the trajectory is only 1 point.
if dimensions_dict["velocities"]:
first_trajectory_point.velocities = deepcopy(trajectory_points[0].velocities)
if dimensions_dict["accelerations"]:
first_trajectory_point.accelerations = deepcopy(trajectory_points[0].accelerations)
first_trajectory_point.time_from_start = rospy.Duration(0)
trajectory_points.insert(0, first_trajectory_point)
num_points = len(trajectory_points)
# Force Velocites/Accelerations to zero at the final timestep
# if they exist in the trajectory
# Remove this behavior if you are stringing together trajectories,
# and want continuous, non-zero velocities/accelerations between
# trajectories
if dimensions_dict["velocities"]:
trajectory_points[-1].velocities = [0.0] * len(joint_names)
if dimensions_dict["accelerations"]:
trajectory_points[-1].accelerations = [0.0] * len(joint_names)
# Compute Full Bezier Curve Coefficients for all 7 joints
pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
try:
b_matrix = self._compute_bezier_coeff(joint_names, trajectory_points, dimensions_dict)
except Exception as ex:
rospy.logerr(
("{0}: Failed to compute a Bezier trajectory for {1}" ' arm with error "{2}: {3}"').format(
self._action_name, self._name, type(ex).__name__, ex
)
)
self._server.set_aborted()
return
# 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()
baxter_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.
# Keep track of current indices for spline segment generation
now_from_start = rospy.get_time() - start_time
end_time = trajectory_points[-1].time_from_start.to_sec()
while now_from_start < end_time and not rospy.is_shutdown():
# Acquire Mutex
now = rospy.get_time()
now_from_start = now - start_time
idx = bisect.bisect(pnt_times, now_from_start)
# Calculate percentage of time passed in this interval
if idx >= num_points:
cmd_time = now_from_start - pnt_times[-1]
t = 1.0
elif idx >= 0:
cmd_time = now_from_start - pnt_times[idx - 1]
t = cmd_time / (pnt_times[idx] - pnt_times[idx - 1])
else:
cmd_time = 0
t = 0
point = self._get_bezier_point(b_matrix, idx, t, cmd_time, dimensions_dict)
# Command Joint Position, Velocity, Acceleration
command_executed = self._command_joints(joint_names, point, start_time, dimensions_dict)
self._update_feedback(deepcopy(point), joint_names, now_from_start)
# Release the Mutex
if not command_executed:
return
control_rate.sleep()
# Keep trying to meet goal until goal_time constraint expired
last = trajectory_points[-1]
last_time = trajectory_points[-1].time_from_start.to_sec()
end_angles = dict(zip(joint_names, last.positions))
def check_goal_state():
for error in self._get_current_error(joint_names, last.positions):
if self._goal_error[error[0]] > 0 and self._goal_error[error[0]] < math.fabs(error[1]):
return error[0]
if (
self._stopped_velocity > 0.0
and max([abs(cur_vel) for cur_vel in self._get_current_velocities(joint_names)])
> self._stopped_velocity
):
return False
else:
return True
while now_from_start < (last_time + self._goal_time) and not rospy.is_shutdown():
if not self._command_joints(joint_names, last, start_time, dimensions_dict):
return
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names, now_from_start)
control_rate.sleep()
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names, now_from_start)
# Verify goal constraint
result = check_goal_state()
if result is True:
rospy.loginfo("%s: Joint Trajectory Action Succeeded for %s arm" % (self._action_name, self._name))
self._result.error_code = self._result.SUCCESSFUL
self._server.set_succeeded(self._result)
elif result is False:
rospy.logerr("%s: Exceeded Max Goal Velocity Threshold for %s arm" % (self._action_name, self._name))
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
else:
rospy.logerr("%s: Exceeded Goal Threshold Error %s for %s arm" % (self._action_name, result, self._name))
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
self._command_stop(goal.trajectory.joint_names, end_angles, start_time, dimensions_dict)