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