def __init__(self):
# Initialize publishers
# Pull all params off param server
self.grasp_approach_tran_frame = rospy.get_param(
"grasp_approach_tran_frame")
self.world_frame = rospy.get_param("world_frame")
self.arm_move_group_name = rospy.get_param("arm_move_group_name")
self.gripper_move_group_name = rospy.get_param(
"gripper_move_group_name")
self.analyzer_planner_id = rospy.get_param("analyzer_planner_id")
self.executor_planner_id = rospy.get_param("executor_planner_id")
self.allowed_analyzing_time = rospy.get_param("allowed_analyzing_time")
self.allowed_execution_time = rospy.get_param("allowed_execution_time")
# Initialize ros service interfaces
self.grasping_controller = curpp.MoveitPickPlaceInterface(
arm_name=self.arm_move_group_name,
gripper_name=self.gripper_move_group_name,
grasp_approach_tran_frame=self.grasp_approach_tran_frame,
analyzer_planner_id=self.analyzer_planner_id,
execution_planner_id=self.executor_planner_id,
allowed_analyzing_time=self.allowed_analyzing_time,
allowed_execution_time=self.allowed_execution_time)
self.scene = moveit_commander.PlanningSceneInterface()
self.world_manager_client = world_manager.world_manager_client.WorldManagerClient(
)
self.tf_listener = tf.TransformListener()
import curpp
import rospy
import grasit_interface.msg
import geometry_msgs.msg
import tf
grasping_controller = curpp.MoveitPickPlaceInterface(
arm_name="arm",
gripper_name="gripper",
grasp_approach_tran_frame="/approach_tran",
analyzer_planner_id='[PRMkConfigDefault]',
execution_planner_id='[BiRRTkConfigDefault]',
allowed_analyzing_time=2,
allowed_execution_time=8)
object_name = "block0"
graspit_grasp = graspit_interface.msg.Grasp()
tf_listener = tf.TransformListener()
pre_grasp_approach_direction = geometry_msgs.msg.Vector3Stamped()
pre_grasp_approach_direction.header.frame_id = rospy.get_param(
"pre_grasp_approach_direction_frame_id")
pre_grasp_approach_direction.vector.x = rospy.get_param(
"pre_grasp_approach_direction_x")
pre_grasp_approach_direction.vector.y = rospy.get_param(
"pre_grasp_approach_direction_y")
pre_grasp_approach_direction.vector.z = rospy.get_param(
"pre_grasp_approach_direction_z")
post_grasp_retreat_direction = geometry_msgs.msg.Vector3Stamped()
post_grasp_retreat_direction.header.frame_id = rospy.get_param(