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 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 _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 _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 _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 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 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 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')