def __init__(self): self._clientR = actionlib.SimpleActionClient( "/sciurus17/controller1/right_hand_controller/gripper_cmd", GripperCommandAction) self._clientL = actionlib.SimpleActionClient( "/sciurus17/controller2/left_hand_controller/gripper_cmd", GripperCommandAction) self._goalR = GripperCommandGoal() self._goalL = GripperCommandGoal() # Wait 10 Seconds for the gripper action server to start or exit self._clientR.wait_for_server(rospy.Duration(5.0)) if not self._clientR.wait_for_server(rospy.Duration(5.0)): rospy.logerr("Exiting - Gripper R Action Server Not Found") rospy.signal_shutdown("Action Server not found") sys.exit(1) self.clear() # Wait 10 Seconds for the gripper action server to start or exit self._clientL.wait_for_server(rospy.Duration(5.0)) if not self._clientL.wait_for_server(rospy.Duration(5.0)): rospy.logerr("Exiting - Gripper L Action Server Not Found") rospy.signal_shutdown("Action Server not found") sys.exit(1) self.clear()
def command_gripper(self, arm, position, max_effort, blocking, timeout): goal = GripperCommandGoal() goal.command.position = position goal.command.max_effort = max_effort arm = self.get_arm(arm) client = "%s_gripper" % arm return self.send_command(client, goal, blocking, timeout)
def __init__(self, namespace='', timeout=5.0, attach_plugin=False, attach_link='robot::wrist_3_link'): self.ns = utils.solve_namespace(namespace) if attach_plugin: # gazebo_ros link attacher self.attach_link = attach_link self.attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach) self.detach_srv = rospy.ServiceProxy('/link_attacher_node/detach', Attach) rospy.logdebug('Waiting for service: {0}'.format(self.attach_srv.resolved_name)) rospy.logdebug('Waiting for service: {0}'.format(self.detach_srv.resolved_name)) self.attach_srv.wait_for_service() self.detach_srv.wait_for_service() # Gripper action server action_server = self.ns + 'gripper_controller/gripper_cmd' self._client = actionlib.SimpleActionClient(action_server, GripperCommandAction) self._goal = GripperCommandGoal() rospy.logdebug('Waiting for [%s] action server' % action_server) server_up = self._client.wait_for_server(timeout=rospy.Duration(timeout)) if not server_up: rospy.logerr('Timed out waiting for Gripper Command' ' Action Server to connect. Start the action server' ' before running this node.') raise rospy.ROSException('GripperCommandAction timed out: {0}'.format(action_server)) rospy.logdebug('Successfully connected to [%s]' % action_server) rospy.loginfo('GripperCommandAction initialized. ns: {0}'.format(self.ns))
def open_gripper(self): print("Opening Gripper ...") goal = GripperCommandGoal() goal.command.position = 0.2 # Fill in the goal here self.client.send_goal(goal) self.client.wait_for_result(rospy.Duration.from_sec(5.0))
def set_trajectory_ee(self, action): """ Helper function. Wraps an action vector of joint angles into a JointTrajectory message. The velocities, accelerations, and effort do not control the arm motion """ # Set up a trajectory message to publish. for the end effector ee_target = EeTrajRequest() quat = quaternion_from_euler(0, 1.571, action[3], 'sxyz') ee_target.pose.orientation.x = quat[0] ee_target.pose.orientation.y = quat[1] ee_target.pose.orientation.z = quat[2] ee_target.pose.orientation.w = quat[3] ee_target.pose.position.x = action[0] ee_target.pose.position.y = action[1] ee_target.pose.position.z = action[2] if self.check_ee_valid_pose(action): result = self.ee_traj_callback(ee_target) goal = GripperCommandGoal() goal.command.position = 0.8 if action[4] >= 0.0 else 0.0 goal.command.max_effort = -1.0 #THIS NEEDS TO BE CHANGEDDDDDD self.gripper_client.send_goal(goal) self.gripper_client.wait_for_result() return True
def gripper_action(self, gripper_type, gripper_position): name_space = '/' + gripper_type + '_gripper_controller/gripper_action' self.gripper_client = SimpleActionClient(name_space, GripperCommandAction) gripper_goal = GripperCommandGoal() gripper_goal.command.position = gripper_position gripper_goal.command.max_effort = 30.0 self.gripper_client.send_goal(gripper_goal)
def main(): """Main function.""" parser = argparse.ArgumentParser( 'Send gripper width commands to a GripperActionServer') parser.add_argument('width', type=float, help='the width (in metres) to set the gripper to') parser.add_argument( '-n', '--namespace', type=str, default='/gripper_command', help='the namespace containing the GripperCommand action') args = parser.parse_args(rospy.myargv()[1:]) rospy.init_node('gripper_command_client') client = actionlib.SimpleActionClient(args.namespace, GripperCommandAction) client.wait_for_server() goal = GripperCommandGoal() goal.command.position = args.width client.send_goal(goal, feedback_cb=_feedback_cb) client.wait_for_result() print('[Result] State: {}'.format(states[client.get_state()])) print('[Result] Status: {}'.format(client.get_goal_status_text())) print('[Result] Position: {}'.format(client.get_result().position)) print('[Result] Effort: {}'.format(client.get_result().effort)) print('[Result] Stalled: {}'.format(client.get_result().stalled)) print('[Result] Reached goal: {}'.format(client.get_result().reached_goal))
def main(): global gazebo_model_states OBJECT_NAME = "wood_cube_5cm" # name of object to grab] sub_model_states = rospy.Subscriber("gazebo/model_states", ModelStates, callback, queue_size=1) arm = moveit_commander.MoveGroupCommander("arm") arm.set_max_velocity_scaling_factor(0.4) gripper = actionlib.SimpleActionClient( "crane_x7/gripper_controller/gripper_cmd", GripperCommandAction) gripper.wait_for_server() gripper_goal = GripperCommandGoal() gripper_goal.command.max_effort = 4.0 # setGripperGoal(gripper,gripper_goal, 1.5707) rospy.sleep(1.0) # setGripperGoal(gripper,gripper_goal, 0) while True: goHome(arm) position, orientation, yaw = getObjectCoordinates(OBJECT_NAME) setGripperGoal(gripper, gripper_goal, 1.5707) moveArm(arm, position.x, position.y, 0.12, yaw) setGripperGoal(gripper, gripper_goal, 0) x = input("x ") y = input("y ") moveArm(arm, x, y, 0.2, yaw) setGripperGoal(gripper, gripper_goal, 1.2)
def grasp2(self, width, force): ''' width, epsilon_inner, epsilon_outer, speed, force ''' grasp_goal = GripperCommandGoal() grasp_goal.command.position = float(width) grasp_goal.command.max_effort = float(force) grasp_msg = GripperCommandActionGoal(goal=grasp_goal) self.generic_grasp_client.publish(grasp_msg)
def move_hand(positions, blocking=True): client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) if len(positions) != 1: return False, "Wrong joint number", positions if not client.wait_for_server(): success = False reason = 'failed to connect to action server' return success, reason, positions rospy.loginfo("Connected to Finger server") client.send_goal( GripperCommandGoal( GripperCommand(position=positions[0], max_effort=100))) try: client.wait_for_result() except KeyboardInterrupt: rospy.loginfo("Program interrupted, pre-empting goal") client.cancel_all_goals() success = False reason = 'Program interrupted from keyboard' return success, reason, positions result = client.get_result() positions = [result.position] success = True reason = None return success, reason, positions
def call_gripper_action(position): global gripper_action_client if not gripper_action_client: gripper_action_client = actionlib.SimpleActionClient( "/gripper_controller/gripper_cmd", GripperCommandAction) gripper_action_client.wait_for_server(rospy.Duration(1)) goal = GripperCommandGoal(command=GripperCommand(position=position)) gripper_action_client.send_goal(goal)
def hard_close(self): rospy.loginfo("argripper_interface: hard close") goal = GripperCommandGoal() goal.command.position = 0.0 goal.command.max_effort = 100 # >0 to 100 self._client.send_goal_and_wait(goal) rospy.loginfo("argripper_interface: hard close done") self._grip_value = self.GRIP_MIN
def close(self, max_effort): rospy.loginfo(f"argripper_interface: close, effort {max_effort:.1f}") goal = GripperCommandGoal() goal.command.position = 0.0 goal.command.max_effort = max_effort self._client.send_goal_and_wait(goal) rospy.loginfo("argripper_interface: close done") self._grip_value = self.GRIP_MIN
def execute_trajectory(self, trajectory): goal_position = trajectory.points[-1].positions print '==========', goal_position command_goal = GripperCommandGoal() command_goal.command.position = goal_position[0]+goal_position[1] command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this self.client.send_goal(command_goal) self.client.wait_for_result()
def release(self): rospy.loginfo("ezgripper_interface: release") goal = GripperCommandGoal() goal.command.position = 0.0 # not dependent on position goal.command.max_effort = 0.0 # max_effort = 0.0 releases all torque on motor self._client.send_goal_and_wait(goal) rospy.loginfo("ezgripper_interface: release done") self._grip_value = self._grip_min
def __init__(self): self.client = actionlib.SimpleActionClient( "gripper_controller/gripper_action", GripperCommandAction) rospy.loginfo("Waiting for gripper_action...") self.move_group = MoveGroupInterface("arm_with_torso", "base_link") self.goal = GripperCommandGoal() self.client.wait_for_server()
def open(self): rospy.loginfo("ezgripper_interface: open") goal = GripperCommandGoal() goal.command.position = 1.0 # 100% range(0.01 to 1.0) goal.command.max_effort = 20.0 # >0 to 100 self._client.send_goal_and_wait(goal) rospy.loginfo("ezgripper_interface: open done") self._grip_value = self._grip_max
def goto_position(self, grip_position = 0.05, grip_effort = 20.0): # position in % 0.01 to 1.0, effort in % 0 to 100 rospy.loginfo("ezgripper_interface: goto position %.3f" %grip_position) goal = GripperCommandGoal() goal.command.position = grip_position # range(0.01 to 1.0) goal.command.max_effort = grip_effort # >0 to 100, if 0.0, torque is released self._client.send_goal_and_wait(goal) rospy.loginfo("ezgripper_interface: open done") self._grip_value = grip_position
def close(self, max_effort): rospy.loginfo("ezgripper_interface: close") goal = GripperCommandGoal() goal.command.position = 0.0 goal.command.max_effort = max_effort self._client.send_goal_and_wait(goal) rospy.loginfo("ezgripper_interface: hard close done") self._grip_value = self._grip_min
def soft_close(self): rospy.loginfo("ezgripper_interface: soft close") goal = GripperCommandGoal() goal.command.position = 0.0 goal.command.max_effort = 10.0 # >0 to 100 self._client.send_goal_and_wait(goal) rospy.loginfo("ezgripper_interface: soft close done") self._grip_value = self._grip_min
def main(): global gazebo_model_states sub_model_states = rospy.Subscriber("gazebo/model_states", ModelStates, callback, queue_size=1) arm = moveit_commander.MoveGroupCommander("arm") arm.set_max_velocity_scaling_factor(0.4) gripper = actionlib.SimpleActionClient("crane_x7/gripper_controller/gripper_cmd", GripperCommandAction) gripper.wait_for_server() gripper_goal = GripperCommandGoal() gripper_goal.command.max_effort = 4.0 rospy.sleep(1.0) count = 0 while True: gripper_goal.command.position = 0.0 gripper.send_goal(gripper_goal) gripper.wait_for_result(rospy.Duration(1.0)) arm.set_named_target("home") arm.go() rospy.sleep(1.0) print "count_number" print count % 3 target_pose = Pose() if count % 3 == 0: target_pose.position.x = 0.2 target_pose.position.y = 0.2 target_pose.position.z = 0.0 q = quaternion_from_euler(math.pi/2, 0.0, math.pi/2) elif count % 3 == 1: target_pose.position.x = 0.1 target_pose.position.y = 0.2 target_pose.position.z = 0.2 q = quaternion_from_euler(0.0, 0.0, math.pi/2) else: target_pose.position.x = -0.3 target_pose.position.y = 0.0 target_pose.position.z = 0.2 q = quaternion_from_euler(math.pi/2, 0.0, 0.0) target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) #arm.go() if arm.go() is False: print "buri buri" continue rospy.sleep(1.0) count += 1
def __init__(self): self.client = actionlib.SimpleActionClient( 'panda/franka_gripper_node/gripper_action', GripperCommandAction) print "Waiting for gripper server..." self.client.wait_for_server() print "Connected to gripper server" self.goal = GripperCommandGoal() self.joint_names = ['panda_finger_joint1', 'panda_finger_joint2']
def change_state(self, value, wait): gripper_goal = GripperCommandGoal() gripper_goal.command.position = value gripper_goal.command.max_effort = 30.0 self.gripper_client.send_goal(gripper_goal) if wait: self.gripper_client.wait_for_result() if not self.gripper_client.get_result().reached_goal: time.sleep(1)
def open_gripper(self): client = actionlib.SimpleActionClient( 'gripper_controller/gripper_action', GripperCommandAction) client.wait_for_server() close_cmmnd = GripperCommandGoal() close_cmmnd.command.position = 0.115 close_cmmnd.command.max_effort = 100 client.send_goal(close_cmmnd) client.wait_for_result()
def move_gripper(self, gripper_x, max_effort, timeout=5.0): gripper_goal = GripperCommandGoal() gripper_goal.command.max_effort = max_effort gripper_goal.command.position = gripper_x self.gripper_client.send_goal(gripper_goal) result = self.gripper_client.wait_for_result(rospy.Duration(timeout)) return result
def close(self, max_effort=MAX_EFFORT): """Closes the gripper. Args: max_effort: The maximum effort, in Newtons, to use. Note that this should not be less than 35N, or else the gripper may not close. """ goal = GripperCommandGoal() goal.command.position = self.CLOSED_POS goal.command.max_effort = max_effort self._client.send_goal_and_wait(goal, rospy.Duration(10))
def close_step(self): self._grip_value = self._grip_value - self._grip_step if self._grip_value < self._grip_min: self._grip_value = self._grip_min rospy.loginfo("ezgripper_interface: goto position %.3f"%self._grip_value) goal = GripperCommandGoal() goal.command.position = self._grip_value goal.command.max_effort = 25.0 self._client.send_goal_and_wait(goal) rospy.loginfo("ezgripper_interface: goto position done")
def open_step(self): self._grip_value = self._grip_value + self._grip_step if self._grip_value > self._grip_max: self._grip_value = self._grip_max rospy.loginfo("ezgripper_interface: goto position %.3f"%self._grip_value) goal = GripperCommandGoal() goal.command.position = self._grip_value goal.command.max_effort = 50.0 self._client.send_goal_and_wait(goal) rospy.loginfo("ezgripper_interface: goto position done")
def close_step(self): self._grip_value = self._grip_value - self._grip_step self._grip_value = max(self._grip_value, self.GRIP_MIN) rospy.loginfo( f"argripper_interface: goto position {self._grip_value:.3f}") goal = GripperCommandGoal() goal.command.position = self._grip_value goal.command.max_effort = 50.0 self._client.send_goal_and_wait(goal) rospy.loginfo("argripper_interface: goto position done")
def set_position(self, position): goal = GripperCommandGoal() goal.command.max_effort = self.max_effort goal.command.position = position # Fill in the goal here self.client.send_goal(goal) self.client.wait_for_result(rospy.Duration.from_sec(5.0)) res = self.client.get_result() with self._lock: self.position = res.position