def process_grasp_action(self, msg):
gripper_command = WubbleGripperGoal()
if msg.goal == GraspHandPostureExecutionGoal.GRASP:
rospy.loginfo('Received GRASP request')
# if not msg.goal.grasp.position:
# msg = 'wubble gripper grasp execution: position vector empty in requested grasp'
# rospy.logerr(msg)
# self.action_server.set_aborted(text=msg)
# return
#
gripper_command.command = WubbleGripperGoal.CLOSE_GRIPPER
gripper_command.torque_limit = 0.4
gripper_command.dynamic_torque_control = True
gripper_command.pressure_upper = 1900.0
gripper_command.pressure_lower = 1800.0
elif msg.goal == GraspHandPostureExecutionGoal.PRE_GRASP:
rospy.loginfo('Received PRE_GRASP request')
gripper_command.command = WubbleGripperGoal.OPEN_GRIPPER
gripper_command.torque_limit = 0.6
elif msg.goal == GraspHandPostureExecutionGoal.RELEASE:
rospy.loginfo('Received RELEASE request')
gripper_command.command = WubbleGripperGoal.OPEN_GRIPPER
gripper_command.torque_limit = 0.6
else:
msg = 'wubble gripper grasp execution: unknown goal code (%d)' % msg.goal
rospy.logerr(msg)
self.action_server.set_aborted(text=msg)
self.gripper_action_client.send_goal(gripper_command)
self.gripper_action_client.wait_for_result()
self.action_server.set_succeeded()
def gentle_close_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.GRASP
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
goal = WubbleGripperGoal()
goal.command = WubbleGripperGoal.CLOSE_GRIPPER
goal.torque_limit = 0.0
goal.dynamic_torque_control = False
self.gripper_controller.send_goal(goal)
self.gripper_controller.wait_for_result()
def gentle_close_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.GRASP
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
goal = WubbleGripperGoal()
goal.command = WubbleGripperGoal.CLOSE_GRIPPER
goal.torque_limit = 0.0
goal.dynamic_torque_control = False
self.gripper_controller.send_goal(goal)
self.gripper_controller.wait_for_result()
def process_grasp_action(self, msg):
gripper_command = WubbleGripperGoal()
if msg.goal == GraspHandPostureExecutionGoal.GRASP:
rospy.loginfo('Received GRASP request')
# if not msg.goal.grasp.position:
# msg = 'wubble gripper grasp execution: position vector empty in requested grasp'
# rospy.logerr(msg)
# self.action_server.set_aborted(text=msg)
# return
#
gripper_command.command = WubbleGripperGoal.CLOSE_GRIPPER
gripper_command.torque_limit = 0.4
gripper_command.dynamic_torque_control = True
gripper_command.pressure_upper = 1900.0
gripper_command.pressure_lower = 1800.0
elif msg.goal == GraspHandPostureExecutionGoal.PRE_GRASP:
rospy.loginfo('Received PRE_GRASP request')
gripper_command.command = WubbleGripperGoal.OPEN_GRIPPER
gripper_command.torque_limit = 0.6
elif msg.goal == GraspHandPostureExecutionGoal.RELEASE:
rospy.loginfo('Received RELEASE request')
gripper_command.command = WubbleGripperGoal.OPEN_GRIPPER
gripper_command.torque_limit = 0.6
else:
msg = 'wubble gripper grasp execution: unknown goal code (%d)' % msg.goal
rospy.logerr(msg)
self.action_server.set_aborted(text=msg)
self.gripper_action_client.send_goal(gripper_command)
self.gripper_action_client.wait_for_result()
self.action_server.set_succeeded()
