def close(
client,
speed=0.1,
force=120,
block=True,
):
"""
Static function to close the gripper
Args:
client: `SimpleActionClient` instance connected to the action server holding a robotiq gripper
instance.
speed: [m/s] Motion speed in meters over seconds. Min value: 0.013[m/s] - Max value: 0.1[m/s]
force: [%] Force value to apply in gripper motion, see robotiq manuals to calculate an appropiate
gripping force value in Newtons.
block: Boolean indicating whether to lock the current thread until command has been reached or not.
"""
goal = CommandRobotiqGripperGoal()
goal.emergency_release = False
goal.stop = False
goal.position = 0.0
goal.speed = speed
goal.force = force
# Sends the goal to the gripper.
if block:
client.send_goal_and_wait(goal)
else:
client.send_goal(goal)
def goto(client, pos, speed=0.1, force=120, block=True):
"""
Static function to update the gripper command
Args:
client: `SimpleActionClient` instance connected to the action server holding a robotiq gripper
instance.
pos: [m] Position (distance in between fingers) in meters desired for the gripper.
speed: [m/s] Motion speed in meters over seconds. Min value: 0.013[m/s] - Max value: 0.1[m/s]
force: [%] Force value to apply in gripper motion, see robotiq manuals to calculate an appropiate
gripping force value in Newtons.
block: Boolean indicating whether to lock the current thread until command has been reached or not.
"""
goal = CommandRobotiqGripperGoal()
goal.emergency_release = False
goal.stop = False
goal.position = pos
goal.speed = speed
goal.force = force
# Sends the goal to the gripper.
if block:
client.send_goal(goal)
client.wait_for_result()
else:
client.send_goal(goal)
def operate_gripper():
action_name = rospy.get_param('~action_name', 'command_robotiq_action')
robotiq_client = actionlib.SimpleActionClient(action_name, CommandRobotiqGripperAction)
# Wait until grippers are ready to take command
robotiq_client.wait_for_server()
rospy.logwarn("Client test: Starting sending goals")
## Manually set all the parameters of the gripper goal state.
######################################################################################
goal = CommandRobotiqGripperGoal()
goal.emergency_release = False
goal.stop = False
goal.position = 0.00 #/m
goal.speed = 0.1 #m/s
goal.force = 5.0 #0-100%
# Sends the goal to the gripper.
robotiq_client.send_goal(goal)
# Block processing thread until gripper movement is finished, comment if waiting is not necesary.
robotiq_client.wait_for_result()
# Use pre-defined functions for robot gripper manipulation.
#####################################################################################
while not rospy.is_shutdown():
Robotiq.goto(robotiq_client, pos=0.00, speed=0.1, force=100 , block=True)
Robotiq.goto(robotiq_client, pos=0.04, speed=0.01, force=10)
Robotiq.goto(robotiq_client, pos=0.011, speed=0.01, force=0 , block=True)
Robotiq.goto(robotiq_client, pos=0.08, speed=0.11, force=200 , block=True)
# Robotiq.goto(robotiq_client, pos=0.06, speed=0.0, force=0)
# break
# Prints out the result of executing the action
return robotiq_client.get_result() # A FibonacciResult
def gripper_client(value):
if value > 0.14:
print "value over the limits value becomes 0.14"
value = 0.14
if value < 0.0:
print "value negative it becomes 0"
value = 0.0
robotiq_client = actionlib.SimpleActionClient('summit_xl/command_robotiq_action', CommandRobotiqGripperAction)
# Wait until the action server has been started and is listening for goals
robotiq_client.wait_for_server()
# Create a goal to send (to the action server)
goal = CommandRobotiqGripperGoal()
print value
goal.position = value # From 0.0 to 0.14
goal.speed= 0.1
goal.force= 5.0
robotiq_client.send_goal(goal)
# w8 for action to be executed
robotiq_client.wait_for_result()
return robotiq_client.get_result()
def execute_joint_trajectory_cb(self, goal):
rospy.loginfo("Trajectory received with %d points", len(goal.trajectory.points))
feedback = FollowJointTrajectoryFeedback()
result = FollowJointTrajectoryResult()
current_status = self._driver.get_current_gripper_status()
# Check trajectory joint names
joint_names = goal.trajectory.joint_names
if len(joint_names) != 1 and joint_names[0] != self._driver._joint_name :
msg = "Joint trajectory joints do not match gripper joint"
rospy.logerr(msg)
result.error_code = result.INVALID_JOINTS
result.error_string = msg
self._joint_trajectory_action_server.set_aborted(result)
return
# Check trajectory points
if len(goal.trajectory.points) == 0:
msg = "Ignoring empty trajectory "
rospy.logerr(msg)
result.error_code = result.INVALID_GOAL
result.error_string = msg
self._joint_trajectory_action_server.set_aborted(result)
return
# Process goal trajectory
self._processing_goal = True
self._is_stalled = False
goal_command = CommandRobotiqGripperGoal()
feedback.joint_names = goal.trajectory.joint_names
watchdog = rospy.Timer(rospy.Duration(goal.trajectory.points[-1].time_from_start.to_sec() * 1.5),
self._execution_timeout,
oneshot=True)
# Follow trajectory points
goal_trajectory_point = goal.trajectory.points[-1]
# Validate trajectory point
if len(goal_trajectory_point.positions) != 1:
result.error_code = result.INVALID_GOAL
result.error_string = "Invalid joint position on trajectory point "
self._joint_trajectory_action_server.set_aborted(result)
return
target_speed = goal_trajectory_point.velocities[0] if len(goal_trajectory_point.velocities) > 0 else 0.01
target_force = goal_trajectory_point.effort[0] if len(goal_trajectory_point.effort) > 0 else 0.1
goal_command.position = self._driver.from_radians_to_distance(goal_trajectory_point.positions[0])
goal_command.speed = abs(target_speed) # To-Do: Convert to rad/s
goal_command.force = target_force
# Send incoming command to gripper driver
self._driver.update_gripper_command(goal_command)
# Set feedback desired value
feedback.desired.positions = [goal_trajectory_point.positions[0]]
while not rospy.is_shutdown() and self._processing_goal and not self._is_stalled:
current_status = self._driver.get_current_gripper_status()
feedback.actual.positions = [self._driver.get_current_joint_position()]
error = abs(feedback.actual.positions[0] - feedback.desired.positions[0])
rospy.logwarn("Error : %.3f -- Actual: %.3f -- Desired: %.3f", error, self._driver.get_current_joint_position(), feedback.desired.positions[0])
feedback.error.positions = [error]
self._joint_trajectory_action_server.publish_feedback( feedback )
# Check for errors
if current_status.fault_status != 0 and not self._is_stalled:
self._is_stalled = True
self._processing_goal = False
rospy.logerr(msg)
result.error_code = -6
result.error_string = "Gripper fault status (gFLT): " + current_status.fault_status
self._joint_trajectory_action_server.set_aborted(result)
return
# Check if object was detected
if current_status.obj_detected:
watchdog.shutdown() # Stop timeout watchdog.
self._processing_goal = False
self._is_stalled = False
result.error_code = result.SUCCESSFUL
result.error_string = "Object detected/grasped"
self._joint_trajectory_action_server.set_succeeded(result)
return
# Check if current trajectory point was reached
if error < 0.005 :
break
# Entire trajectory was followed/reached
watchdog.shutdown()
rospy.logdebug(self._action_name + ": goal reached")
result.error_code = result.SUCCESSFUL
result.error_string = "Goal reached"
self._joint_trajectory_action_server.set_succeeded(result)
self._processing_goal = False
self._is_stalled = False