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
