def emergency_release(client): """ Static function to trigger an emergency release motion. Args: client: `SimpleActionClient` instance connected to the action server holding a robotiq gripper instance. """ goal = CommandRobotiqGripperGoal() goal.emergency_release = True client.send_goal_and_wait(goal)
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 stop(client, block=True): """ Static function to stop gripper motion Args: client: `SimpleActionClient` instance connected to the action server holding a robotiq gripper instance. 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 # Sends the goal to the gripper. if block: client.send_goal_and_wait(goal) else: client.send_goal(goal)
def __init__(self, stroke=0.085, joint_name='finger_joint'): self._stroke = stroke self._joint_name = joint_name self._current_joint_pos = 0.0 self._prev_time = rospy.get_time() self._current_goal = CommandRobotiqGripperGoal() self._current_goal.position = self._stroke self._gripper_joint_state_pub = rospy.Publisher("/joint_states" , JointState, queue_size=10) self.is_ready = True self._is_moving = False self._max_joint_limit = 0.8 if( self._stroke == 0.140 ): self._max_joint_limit = 0.7
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 gripper_move(ctx, position, speed, force): ''' Moves the gripper to a target position (width). :param ctx A Context class instance. :param position The target width. [m] :param speed The target end-effector speed for the motion. [m/s] :param force The force to apply. [N] ''' global node_prefix goal = CommandRobotiqGripperGoal(position=position, speed=speed, force=force) rospy.loginfo('Moving gripper:\n{}'.format(goal)) ctx.gripper_move.send_goal(goal) ctx.gripper_move.wait_for_result() result = ctx.gripper_move.get_result() if not abs(result.requested_position - result.position) < 0.001: rospy.logerr(node_prefix + 'Couldn\'t move gripper') sys.exit(1)
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