def __init__(self): self.name = 'gripper' self._cmd_sender = rospy.get_name() + '_%s' self._cmd_sequence = 0 ns = 'crustcrawler/end_effector/' + self.name + "/" self._state = None self.on_gripping_changed = crustcrawler_dataflow.Signal() self.on_moving_changed = crustcrawler_dataflow.Signal() self._parameters = dict() self._cmd_pub = rospy.Publisher(ns + 'command', EndEffectorCommand, queue_size=10) self._state_pub = rospy.Publisher(ns + 'set_state', EndEffectorState, latch=True, queue_size=10) self._state_sub = rospy.Subscriber(ns + 'state', EndEffectorState, self._on_gripper_state) # Wait for the gripper state message to be populated crustcrawler_dataflow.wait_for( lambda: not self._state is None, timeout=5.0, timeout_msg=("Failed to get state from %s" % (ns + 'state', )))
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 = crustcrawler_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 crustcrawler_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 crustcrawler 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()) crustcrawler_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 crustcrawler_dataflow.wait_for( test=test, timeout=timeout, raise_on_error=False, body=lambda: self._state_pub.publish(state_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 6 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() crustcrawler_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)
def __init__(self): """ Constructor. """ self.name = 'crustcrawler' 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 = { 'crustcrawler': ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'] } ns = '/crustcrawler/' 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_1_cmd = rospy.Publisher( ns + 'joint_1_position_controller/command', Float64, tcp_nodelay=True, queue_size=1) self._pub_joint_2_cmd = rospy.Publisher( ns + 'joint_2_position_controller/command', Float64, tcp_nodelay=True, queue_size=1) self._pub_joint_3_cmd = rospy.Publisher( ns + 'joint_3_position_controller/command', Float64, tcp_nodelay=True, queue_size=1) self._pub_joint_4_cmd = rospy.Publisher( ns + 'joint_4_position_controller/command', Float64, tcp_nodelay=True, queue_size=1) self._pub_joint_5_cmd = rospy.Publisher( ns + 'joint_5_position_controller/command', Float64, tcp_nodelay=True, queue_size=1) self._pub_joint_6_cmd = rospy.Publisher( ns + 'joint_6_position_controller/command', Float64, 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 = 'crustcrawler/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) crustcrawler_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') #crustcrawler_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0, # timeout_msg=err_msg) print(self._joint_angle)