def preempt_requested(self): result = GripperCommandResult() result.position = self.master_joint_position * 2.0 * self.master_joint_direction result.reached_goal = False self.stop_goal() self.gripper_command_action_server.set_preempted(result) self.status = IDLE_STATE
def _execute(self, goal): jointState = JointState() jointState.name = ['robotiq_85_left_knuckle_joint'] jointState.position = [goal.command.position] self._joint_state_pub.publish(jointState) result = GripperCommandResult() result.position = goal.command.position result.stalled = False result.reached_goal = True self._server.set_succeeded(result)
def _timed_out(self, te): rospy.loginfo('Gripper movement timed out') self._timer = None if not self._last_state: self._last_state = JointState() result = GripperCommandResult() self._as.set_aborted(result=self._state_to_feedback( self._last_state, result), text='Timed out')
def _handle_preempt(self): if self._timer: self._timer.shutdown() self._timer = None rospy.loginfo('Gripper movement preempted') if not self._last_state: self._last_state = JointState() result = GripperCommandResult() self._as.set_preempted(result=self._state_to_feedback( self._last_state, result), text='Preempted')
def execute_action(self, goal): print('------------------execute_action') print(goal) if goal.command.position > 0: success = self._open() else: success = self._close() if success: result = GripperCommandResult() result.position = goal.command.position result.effort = goal.command.max_effort result.reached_goal = True print(result) self.action_server.set_succeeded(result) print('------------------set_succeeded') else: rospy.loginfo('tool operation aboarted') self.action_server.set_aborted() print('------------------set_aborted')
def execute_cb_hand(self, command): print "hand" # -pi/3 used to display better in rviz self.jointStates[-1] = -pi / 3 # actual PWM value sent to servo motor to close the gripper self.jointPWMs[-1] = 2200 # send PWM self.spi_send_PWM() # send result to cient to finish the action protocol self.gripper_result = GripperCommandResult() self.gripper_result.reached_goal = True self._as_hand.set_succeeded(self.gripper_result)
def _generate_result_msg(self, goal): result = GripperCommandResult() result.position = self._stat.position result.effort = 0 result.stalled = False result.reached_goal = abs(self._stat.position - goal) < GRIP_THRESHOLD return result
def _state_update(self, state): if not self._as.is_active(): return rospy.logdebug('Got servo state update:\n{}'.format(state)) self._last_state = state feedback = GripperCommandFeedback() self._state_to_feedback(state, feedback) self._as.publish_feedback(feedback) if feedback.reached_goal: rospy.loginfo('Determined from state update that gripper goal has ' 'been reached') if self._timer: self._timer.shutdown() self._timer = None result = GripperCommandResult() self._as.set_succeeded(result=self._state_to_feedback( state, result), text='Reached goal') elif feedback.stalled: rospy.loginfo('Determined from state update that gripper has ' 'stalled') if self._timer: self._timer.shutdown() self._timer = None result = GripperCommandResult() self._as.set_aborted(result=self._state_to_feedback(state, result), text='Stalled') elif abs(state.load) > self._overload: rospy.loginfo('Determined from state update that gripper is ' 'blocked (overload protection)') if self._timer: self._timer.shutdown() self._timer = None self._command_pub.publish(state.current_pos) result = GripperCommandResult() self._as.set_aborted(result=self._state_to_feedback(state, result), text='Blocked')
def _std_res_msg(self, position_target, position_actual, effort_actual, stalled): result = GripperCommandResult() result.position = self.position_to_joint_conv(position_actual) result.effort = self.force_to_effort_conv(effort_actual) result.stalled = stalled result.reached_goal = abs(position_target - position_actual) < self._grip_threshold return result
def execute(self, goal): """Action server callback. Gripper has only two commands: fully open, fully close. Args: goal (GripperCommandGoal): action goal """ try: # gripper has only two commands: fully open, fully close close = goal.command.position > 1e-3 # we can choose from two effort modes low_force_mode = goal.command.max_effort < 1e-3 if close: self._gripper.close(low_force_mode, wait=False) else: self._gripper.open(low_force_mode, wait=False) rate = rospy.Rate(self._action_monitor_rate) while self._server.is_active(): rate.sleep() width = (1.0 - self._gripper.opening) * self._limit.upper force = self._limit.effort * (0.125 if low_force_mode else 1.0) if self._gripper.is_ready: reached_goal = abs(goal.command.position - width) < 0.001 stalled = not reached_goal result = GripperCommandResult( position=width, effort=force, stalled=stalled, reached_goal=reached_goal, ) self._server.set_succeeded(result) return feedback = GripperCommandFeedback( position=width, effort=force, stalled=False, reached_goal=False, ) self._server.publish_feedback(feedback) except Exception as ex: rospy.logerr('Griper commmand failed: %s', ex) self._server.set_aborted(text=str(ex))
def _generate_result_msg(self, goal): print goal, 'vs', self._stat.position result = GripperCommandResult() result.position = self._stat.position result.effort = 0 result.stalled = False result.reached_goal = abs(self._stat.position - goal) < self._grip_threshold return result
def __init__(self, gripper, scale_pos, reconfig_server): self._scale_pos = scale_pos self._dyn = reconfig_server self._ee = gripper + '_gripper' self._ns = 'robot/end_effector/' + self._ee + '/gripper_action' self._gripper = baxter_interface.Gripper(gripper, CHECK_VERSION) # Store Gripper Type self._type = self._gripper.type() if self._type == 'custom': msg = ("Stopping %s action server - %s gripper not capable of " "gripper actions" % (self._gripper.name, self._type)) rospy.logerr(msg) return # Verify Grippers Have No Errors and are Calibrated if self._gripper.error(): self._gripper.reset() if self._gripper.error(): msg = ("Stopping %s action server - Unable to clear error" % self._gripper.name) rospy.logerr(msg) return if not self._gripper.calibrated(): self._gripper.calibrate() if not self._gripper.calibrated(): msg = ("Stopping %s action server - Unable to calibrate" % self._gripper.name) rospy.logerr(msg) return # Action Server self._server = actionlib.SimpleActionServer( self._ns, GripperCommandAction, execute_cb=self._on_gripper_action, auto_start=False) self._action_name = rospy.get_name() self._server.start() # Action Feedback/Result self._fdbk = GripperCommandFeedback() self._result = GripperCommandResult() # Initialize Parameters self._prm = self._gripper.parameters() self._timeout = 5.0
def __init__(self): self._name_space = 'grip/gripper_action' # Start Action Server. self._server = actionlib.SimpleActionServer( self._name_space, GripperCommandAction, execute_cb=self._on_gripper_action, auto_start=False) self._action_name = rospy.get_name() self._server.start() # initialize a edo states in the action server self.states = EdoStates() # Action Feedback/Result self._feedback = GripperCommandFeedback() self._result = GripperCommandResult() self._timeout = 5.0 self._action_name = rospy.get_name() self._update_rate_spinner = rospy.Rate(20)
def __init__(self): self._ns = "/robot/gripper_action_server" self._gripper = intera_interface.Gripper() # Action Server self._server = actionlib.SimpleActionServer( self._ns, GripperCommandAction, execute_cb=self._on_gripper_action, auto_start=False) self._action_name = rospy.get_name() self._server.start() self._gripper.set_dead_zone("0.021") # Action Feedback/Result self.feedback_msg = GripperCommandFeedback() self.result_msg = GripperCommandResult() # Initialize Parameters self._timeout = 5.0
def __init__(self, controller_namespace, controllers): self.update_rate = 1000 self.controller_namespace = controller_namespace self.joint_names = [ctrl.joint_name for ctrl in controllers] # Get joint to controller dict self.joint_to_controller = {} for ctrl in controllers: self.joint_to_controller[ctrl.joint_name] = ctrl # Get port to joints dicts self.port_to_joints = {} for ctrl in controllers: if ctrl.port_namespace not in self.port_to_joints: self.port_to_joints[ctrl.port_namespace] = [] self.port_to_joints[ctrl.port_namespace].append(ctrl.joint_name) # Get get port to io (DynamixelIO) self.port_to_io = {} for ctrl in controllers: if ctrl.port_namespace in self.port_to_io: continue self.port_to_io[ctrl.port_namespace] = ctrl.dxl_io self.joint_states = dict( zip(self.joint_names, [ctrl.joint_state for ctrl in controllers])) self.num_joints = len(self.joint_names) self.joint_to_idx = dict(zip(self.joint_names, range(self.num_joints))) # Base msgs # Feedback and result base msg self.feedback = GripperCommandFeedback() self.result = GripperCommandResult() # Joint state base msg self.joint_state = JointState() self.joint_state.name = self.joint_names self.joint_state.position = [0.0] * self.num_joints self.joint_state.velocity = [0.0] * self.num_joints self.joint_state.effort = [0.0] * self.num_joints # Current max effort self.current_goal = 0.0 self.current_effort = 0.5 # Soft sensor self.left_side_pressure = UInt16() self.right_side_pressure = UInt16() self.sensor_effort = 0.0
def __init__(self): self._ns = 'franka_gripper/gripper_action' self._gripper = franka_interface.GripperInterface( gripper_joint_names=['panda_finger_joint1', 'panda_finger_joint2']) # Action Server self._server = actionlib.SimpleActionServer( self._ns, GripperCommandAction, execute_cb=self._on_gripper_action, auto_start=False) self._action_name = "GripperActionServer" self._server.start() # Action Feedback/Result self._fdbk = GripperCommandFeedback() self._result = GripperCommandResult() # Initialize Parameters self._timeout = 5.0
def __init__(self, reconfig_server): self._dyn = reconfig_server self._ee = 'gripper' self._ns = 'crustcrawler/end_effector/' + self._ee + '/gripper_action' self._gripper = crustcrawler_interface.Gripper() # Verify Grippers Have No Errors and are Calibrated if self._gripper.error(): self._gripper.reset() if self._gripper.error(): msg = ("Stopping %s action server - Unable to clear error" % self._gripper.name) rospy.logerr(msg) return if not self._gripper.calibrated(): self._gripper.calibrate() if not self._gripper.calibrated(): msg = ("Stopping %s action server - Unable to calibrate" % self._gripper.name) rospy.logerr(msg) return # Action Server self._server = actionlib.SimpleActionServer( self._ns, GripperCommandAction, execute_cb=self._on_gripper_action, auto_start=False) self._action_name = rospy.get_name() self._server.start() # Action Feedback/Result self._fdbk = GripperCommandFeedback() self._result = GripperCommandResult() # Initialize Parameters #self._prm = self._gripper.parameters() self._timeout = 5.0
def gripper_action_execute(self, goal): rospy.loginfo("Execute goal: position=%.1f, max_effort=%.1f" % (goal.command.position, goal.command.max_effort)) if goal.command.max_effort == 0.0: rospy.loginfo("Release torque: start") self.gripper.release() rospy.loginfo("Release torque: done") else: rospy.loginfo("Go to position: start") self.gripper.goto_position(goal.command.position, goal.command.max_effort) rospy.loginfo("Go to position: done") result = GripperCommandResult() result.position = goal.command.position #not necessarily the current position of the gripper if the gripper did not reach its goal position. result.effort = goal.command.max_effort result.stalled = False result.reached_goal = True self.action_server.set_succeeded(result)
def gripper_action_execute(self, goal): rospy.loginfo( "Execute goal: position=%.3f, grip_max=%d max_effort=%.3f" % (goal.command.position, grip_max, goal.command.max_effort)) if goal.command.max_effort == 0.0: rospy.loginfo("Release torque: start") for servo in self.gripper.servos: set_torque_mode(servo, False) result = GripperCommandResult() result.position = goal.command.position result.effort = 0.0 result.stalled = False result.reached_goal = True self.action_server.set_succeeded(result) rospy.loginfo("Release torque: done") return if goal.command.position == 0.0: rospy.loginfo("Close: start") closing_torque = goal.command.max_effort rospy.loginfo("Using closing torque %.2f" % closing_torque) self.gripper.close(closing_torque) result = GripperCommandResult() result.position = goal.command.position result.effort = 13.0 result.stalled = False result.reached_goal = True self.action_server.set_succeeded(result) rospy.loginfo("Close: done") return rospy.loginfo("Go to position: start") #servo_position = servo_position_from_gap(goal.command.position) servo_position = servo_position_from_gap(goal.command.position) rospy.loginfo("Target position: %.3f (%d)" % (goal.command.position, servo_position)) closing_torque = int( goal.command.max_effort) # TODO: need a more accurate calculation self.gripper.set_max_effort( closing_torque ) # essentially sets velocity of movement, but also sets max_effort for initial half second of grasp. self.gripper.goto_position(int(servo_position)) # sets torque to keep gripper in position, but does not apply torque if there is no load. This does not provide continuous grasping torque. holding_torque = min(torque_hold, closing_torque) self.gripper.set_max_effort(holding_torque) # result = GripperCommandResult() result.position = goal.command.position #not necessarily the current position of the gripper if the gripper did not reach its goal position. result.effort = goal.command.max_effort result.stalled = False result.reached_goal = True self.action_server.set_succeeded(result) rospy.loginfo("Go to position: done")
def __init__(self, prefix="", gripper="", interface='eth0', jaco_ip="10.66.171.15", dof="", rate=100.0): self._alive = False self.init_success = True self._action_name = rospy.get_name() self._prefix = prefix # Action Feedback/Result if ("kg2" == gripper) or ("rq85" == gripper): self.gripper_stall_force = 20.0 self.gripper_dead_zone = 0.01 elif("kg3" == gripper): self.gripper_stall_force = 30.0 self.gripper_dead_zone = 0.01 self._last_gripper_pos = 0.165 self._gripper_stall_to = 0.7 self._gripper_pos_stall = False self._last_movement_time = rospy.get_time() self.dof = dof self._planner_homing = False """ Define the joint names """ if ("6dof" == dof): self._joint_names = [self._prefix+'_shoulder_pan_joint', self._prefix+'_shoulder_lift_joint', self._prefix+'_elbow_joint', self._prefix+'_wrist_1_joint', self._prefix+'_wrist_2_joint', self._prefix+'_wrist_3_joint'] self._body_joints = ["right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", "right_wrist_1_joint", "right_wrist_2_joint", "right_wrist_3_joint", "left_elbow_joint", "left_shoulder_lift_joint", "left_shoulder_pan_joint", "left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"] self._homed = [-2.135, -0.227, -1.478, -2.083, 1.445, 1.321, 2.135, 0.227, 1.478, 2.083, -1.445, -1.321, 0.25, 0.0, 0.0] elif ("7dof" == dof): self._joint_names = [self._prefix + '_shoulder_pan_joint', self._prefix + '_shoulder_lift_joint', self._prefix + '_arm_half_joint', self._prefix + '_elbow_joint', self._prefix + '_wrist_spherical_1_joint', self._prefix + '_wrist_spherical_2_joint', self._prefix + '_wrist_3_joint'] self._body_joints = ["right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint", "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"] self._homed = [-1.5, -0.2, -0.15, -2.0, 2.0, -1.24, -1.1, 1.5, 0.2, 0.15, 2.0, -2.0, 1.24, 1.1, 0.25, 0, 0] else: rospy.logerr("DoF needs to be set 6 or 7, cannot start MovoArmJTAS") return """ Controller parameters from arguments, messages, and dynamic reconfigure """ self._trajectory_control_rate = rate # Hz self._goal_time = 0.0 self._stopped_velocity = 0.0 self._goal_error = dict() self._path_thresh = dict() self._traj_smoother = TrajectorySmoother(rospy.get_name(),self._prefix) self._ctl = SIArmController(self._prefix,gripper,interface,jaco_ip, dof) self._ctl.Pause() self._estop_delay = 0 self.home_arm_sub = rospy.Subscriber('/movo/home_arms', Bool, self._home_arms) self.home_arm_pub = rospy.Publisher('/movo/arms_are_homed', Bool, queue_size=1) self._arms_homing = False if not self._ctl.init_success: rospy.logerr("Failed to initialize controller, make sure the serial number exists") self.clean_shutdown() self.init_success = False return self.estop = False self._fdbk = FollowJointTrajectoryFeedback() self._result = FollowJointTrajectoryResult() #self._dyn = reconfig_server self._ns = '/movo/%s_arm_controller'%self._prefix self._fjt_ns = self._ns + '/follow_joint_trajectory' self._server = actionlib.SimpleActionServer( self._fjt_ns, FollowJointTrajectoryAction, execute_cb=self._on_trajectory_action, auto_start=False) self._alive = True self._movo_status_sub = rospy.Subscriber("/movo/feedback/status",Status,self._update_movo_status) self._server.start() # Action Server self._gripper_server = actionlib.SimpleActionServer( '/movo/%s_gripper_controller/gripper_cmd'%self._prefix, GripperCommandAction, execute_cb=self._on_gripper_action, auto_start=False) self._gripper_server.start() self._gripper_action_name = '/movo/%s_gripper_controller/gripper_cmd'%self._prefix # Action Feedback/Result self._gripper_fdbk = GripperCommandFeedback() self._gripper_result = GripperCommandResult() self._gripper_timeout = 6.0 self._ctl.api.InitFingers()
def _fail_res_msg(self): result = GripperCommandResult() result.reached_goal = False return result
def goal_reached(self): result = GripperCommandResult() result.position = self.desired_position result.reached_goal = True self.gripper_command_action_server.set_succeeded(result) self.status = IDLE_STATE
def gripper_action_execute(self, goal): rospy.loginfo("Execute goal: position=%.3f, grip_max=%d max_effort=%.3f"% (goal.command.position, grip_max, goal.command.max_effort)) if goal.command.max_effort == 0.0: rospy.loginfo("Release torque: start") for servo in self.gripper.servos: set_torque_mode(servo, False) result = GripperCommandResult() result.position = goal.command.position result.effort = 0.0 result.stalled = False result.reached_goal = True self.action_server.set_succeeded(result) rospy.loginfo("Release torque: done") return if goal.command.position == 0.0: rospy.loginfo("Close: start") closing_torque = goal.command.max_effort rospy.loginfo("Using closing torque %.2f"%closing_torque) self.gripper.close(closing_torque) result = GripperCommandResult() result.position = goal.command.position result.effort = 13.0 result.stalled = False result.reached_goal = True self.action_server.set_succeeded(result) rospy.loginfo("Close: done") return rospy.loginfo("Go to position: start") #servo_position = servo_position_from_gap(goal.command.position) servo_position = servo_position_from_gap(goal.command.position) rospy.loginfo("Target position: %.3f (%d)"%(goal.command.position, servo_position)) closing_torque = int(goal.command.max_effort) # TODO: need a more accurate calculation self.gripper.set_max_effort(closing_torque) # essentially sets velocity of movement, but also sets max_effort for initial half second of grasp. self.gripper.goto_position(int(servo_position)) # sets torque to keep gripper in position, but does not apply torque if there is no load. This does not provide continuous grasping torque. holding_torque = min(torque_hold, closing_torque) self.gripper.set_max_effort(holding_torque) # result = GripperCommandResult() result.position = goal.command.position #not necessarily the current position of the gripper if the gripper did not reach its goal position. result.effort = goal.command.max_effort result.stalled = False result.reached_goal = True self.action_server.set_succeeded(result) rospy.loginfo("Go to position: done")
def actionCb(self, goal): result = GripperCommandResult() feedback = GripperCommandFeedback() """ Take an input command of width to open gripper. """ rospy.loginfo('Gripper Controller action goal received: position %f, max_effort %f', goal.command.position, goal.command.max_effort) # send command to gripper if not self.model.setCommand(goal.command): self.server.set_aborted() rospy.loginfo('Gripper Controller: Aborted.') return # register progress so we can guess if the gripper is stalled; our buffer # must contain up to: stalled_time / joint_states period position values if len(self.state_cb_times) == self.state_cb_times.maxlen: T = sum(self.state_cb_times[i] - self.state_cb_times[i-1] \ for i in xrange(1, len(self.state_cb_times) - 1)) \ /(len(self.state_cb_times) - 1) progress = collections.deque(maxlen=round(self.stalled_time/T)) else: self.server.set_aborted() rospy.logerr('Gripper Controller: no messages from joint_states topic received') return diff_at_start = round(abs(goal.command.position - self.current_position), 3) # keep watching for gripper position... while True: if self.server.is_preempt_requested(): self.server.set_preempted() rospy.loginfo('Gripper Controller: Preempted.') return # synchronize with the joints state callbacks; self.state_cb_event.wait() self.state_cb_event.clear() # break when we have reached the goal position... diff = abs(goal.command.position - self.current_position) if diff < self._EPSILON_OPENING_DIFF_: result.reached_goal = True break # ...or when the gripper is exerting beyond max effort if goal.command.max_effort and self.current_effort >= goal.command.max_effort: # stop moving to prevent damaging the gripper goal.command.position = self.current_position self.model.setCommand(goal.command) rospy.logerr('Gripper Controller: max effort reached at position %f (%f >= %f)', self.current_position, self.current_effort, goal.command.max_effort) result.stalled = True break # ...or when progress stagnates, probably signaling that the gripper is exerting max effort and not moving progress.append(round(diff, 3)) # round to millimeter to neglect tiny motions of the stalled gripper if len(progress) == progress.maxlen and progress.count(progress[0]) == len(progress): if progress[0] == diff_at_start: # we didn't move at all! is the gripper connected? self.server.set_aborted() rospy.logerr('Gripper Controller: gripper not moving; is the servo connected?') return # buffer full with all-equal positions -> gripper stalled goal.command.position = self.current_position self.model.setCommand(goal.command) rospy.logerr('Gripper Controller: gripper stalled at position %f', self.current_position) result.stalled = True break # publish feedback feedback.position = self.current_position feedback.effort = self.current_effort self.server.publish_feedback(feedback) # publish one last feedback and the result (identical) feedback.position = self.current_position feedback.effort = self.current_effort feedback.reached_goal = result.reached_goal feedback.stalled = result.stalled self.server.publish_feedback(feedback) result.position = self.current_position result.effort = self.current_effort self.server.set_succeeded(result) rospy.loginfo('Gripper Controller: Succeeded (%s)', 'goal reached' if result.reached_goal else 'stalled')