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 = rbt_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)
rbt_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, 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)
rbt_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 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:
rbt_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_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:
rbt_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 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 = rbt_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 rbt_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,
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())
rbt_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 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 rbt_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 rbt_baxter_dataflow.wait_for(
test=test,
timeout=timeout,
raise_on_error=False,
body=lambda: self._prop_pub.publish(prop_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()
rbt_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()
and self.robot_is_enabled()):
# 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() and self.robot_is_enabled()):
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)
def __init__(self, gripper, versioned=False):
"""
Version-checking capable constructor.
@type gripper: str
@param gripper: robot limb <left/right> on which the gripper
is mounted
@type versioned: bool
@param versioned: True if Gripper firmware version should be checked
on initialization. [False]
The gripper firmware versions are checked against the version
compatibility list in L{rbt_baxter_interface.VERSIONS_SDK2GRIPPER}.
The compatibility list is updated in each SDK release.
By default, this interface class will not check versions,
but all examples using Grippers in baxter_examples pass a True
and will check. This behavior can be overridden by setting
L{rbt_baxter_interface.CHECK_VERSION} to False.
"""
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(id=-1) # initialize w/unused value
self.on_type_changed = rbt_baxter_dataflow.Signal()
self.on_gripping_changed = rbt_baxter_dataflow.Signal()
self.on_moving_changed = rbt_baxter_dataflow.Signal()
self._parameters = dict()
self._cmd_pub = rospy.Publisher(ns + 'command',
EndEffectorCommand,
queue_size=10)
self._prop_pub = rospy.Publisher(ns + 'rsdk/set_properties',
EndEffectorProperties,
latch=True,
queue_size=10)
self._state_pub = rospy.Publisher(ns + 'rsdk/set_state',
EndEffectorState,
latch=True,
queue_size=10)
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
rbt_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
rbt_baxter_dataflow.wait_for(
lambda: not self.type() is None,
timeout=5.0,
timeout_msg=("Failed to get properties from %s" %
(ns + 'properties', )))
if versioned and self.type() == 'electric':
if not self.version_check():
sys.exit(1)
self.set_parameters(defaults=True)
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,
queue_size=10)
self._pub_joint_cmd = rospy.Publisher(ns + 'joint_command',
JointCommand,
tcp_nodelay=True,
queue_size=1)
self._pub_joint_cmd_timeout = rospy.Publisher(ns +
'joint_command_timeout',
Float64,
latch=True,
queue_size=10)
_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)
rbt_baxter_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
timeout_msg=err_msg)
err_msg = ("%s limb init failed to get current endpoint_state "
"from %s") % (self.name.capitalize(), ns + 'endpoint_state')
rbt_baxter_dataflow.wait_for(
lambda: len(self._cartesian_pose.keys()) > 0, timeout_msg=err_msg)
