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)