def __init__(self, side='r', ctrl_mng=None, moveit_cmdr=None):
'''
Constructor
'''
if side == "right" or side == "r":
arm = "right_arm"
self.side = "r"
elif side == 'left' or side == 'l':
arm = "left_arm"
self.side = "l"
else:
rospy.logerr("Side " + side + " is not supported")
raise
#self.tf_listener = tf.TransformListener() if not tf_listener else tf_listener
self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
self.ctrl_mng.switch_controllers([self.side + '_arm_controller'],
[self.side + '_cart'])
self.moveit_cmdr = MoveGroupCommander(
arm) if not moveit_cmdr else moveit_cmdr
self.gripper = Gripper(arm)
self.pose_pub = rospy.Publisher('/r_cart/command_pose', PoseStamped)
self.eef_frame = self.side + "_wrist_roll_link"
self.reference_frame = "base_link"
self.approach_dist = 0.100
self.lift_dist = 0.110
self.step_size = 0.003
self.moveit_cmdr.set_pose_reference_frame(self.reference_frame)
return (contact_point1, contact_point2, normal1, normal2, \\
uncertainty1, uncertainty2)
"""
# Reference frame
reference_frame_id = 'base_link'
#services
rospy.wait_for_service('tabletop_octomap')
rospy.wait_for_service('get_probabilistic_pointcloud')
rospy.wait_for_service('plan_point_cluster_grasp')
rospy.wait_for_service('set_point_cluster_grasp_params')
rospy.wait_for_service('evaluate_point_cluster_grasps')
rospy.wait_for_service('find_pretouch')
# Controller switcher
cm_client = ControllerManagerClient()
cm_client.switch_controllers(['r_arm_controller'], ['r_cart'])
# MoveIt! Commander
moveit_cmd = MoveGroupCommander("right_arm")
moveit_cmd.set_pose_reference_frame(reference_frame_id)
pose = Pose()
pose.position.x = 0.258
pose.position.y = -0.614
pose.position.z = 1.017
pose.orientation.w = 0.720
pose.orientation.x = 0.059
pose.orientation.y = 0.153
pose.orientation.z = 0.674
moveit_cmd.go(pose, wait=True)
def __init__(self,
side='r',
tf_listener=None,
tf_broadcaster=None,
ctrl_mng=None,
moveit_cmdr=None):
if side == "right" or side == "r":
self.arm = "right_arm"
self.side = "r"
elif side == 'left' or side == 'l':
self.arm = "left_arm"
self.side = "l"
else:
rospy.logerr("Side " + side + " is not supported")
raise
self.tf_listener = tf.TransformListener(
) if not tf_listener else tf_listener
#init a TF transform broadcaster for the object frame
self.tf_broadcaster = tf.TransformBroadcaster(
) if not tf_broadcaster else tf_broadcaster
self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
#the pretouch sensor frame
self.pretouch_sensor_frame_id = '/r_gripper_l_finger_tip_pretouch_frame'
#Gripper client and open the gripper
rospy.loginfo('open the ' + side + '_gripper')
self.gripper = Gripper(self.arm)
self.gripper.open()
#controller_magager_client
self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
self.ctrl_mng.switch_controllers([self.side + '_arm_controller'],
[self.side + '_cart'])
self.count = 0
#PoseStamped command publisher for jt controller
self.pose_pub = rospy.Publisher('/r_cart/command_pose', PoseStamped)
self.eef_frame = self.side + "_wrist_roll_link"
self.reference_frame = "base_link"
# MoveIt! Commander
self.moveit_cmd = MoveGroupCommander(
self.arm) if not moveit_cmdr else moveit_cmdr
self.moveit_cmd.set_pose_reference_frame(self.reference_frame)
self.move_arm_to_side() # Move the arm to the sidea
self.step_size = 0.0002
self.move_step_mat = np.matrix(
translation_matrix(np.array([self.step_size, 0.0, 0.0])))
#pickup action server
#self.pickup_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_pickup', PickupAction)
#rospy.loginfo("waiting for PickupAction Server")
#self.pickup_client.wait_for_server()
#rospy.loginfo("PickupAction Server Connected")
#service client to /add_point service
self.add_point_client = rospy.ServiceProxy('add_point', AddPoint)
#draw_functions object for drawing stuff in rviz
self.draw_functions = draw_functions.DrawFunctions(
'pretouch_executor_markers')
#the transform from wrist_frame to the center of the fingers
self.gripper_to_r_finger = np.eye(4)
self.gripper_to_r_finger[0][
3] = 0.1615 #x-translation from wrist_frame (0.1615)
self.gripper_to_r_finger[1][
3] = -0.0400 #y-translation from wrist_frame (0.0425)
self.gripper_to_l_finger = np.eye(4)
self.gripper_to_l_finger[0][
3] = 0.1615 #x-translation from wrist_frame (0.1615)
self.gripper_to_l_finger[1][
3] = 0.0400 #y-translation from wrist_frame (-0.0425)
