def create_spaced_downward_grasps(xyz, cloud_in, num_grasps=20,
num_axis_attempts = 5):
x_angle = 0
y_angle = np.pi / 2
grasps = []
orig_x, orig_y, orig_z = xyz
for z_angle in np.linspace(-np.pi, np.pi, num_grasps):
#for x in np.linspace(orig_x - 0.01, orig_x + 0.01, num_axis_attempts):
#for y in np.linspace(orig_y - 0.01, orig_y + 0.01, num_axis_attempts):
#for z in np.linspace(orig_z - 0.01, orig_z + 0.01, num_axis_attempts):
x = orig_x
y = orig_y
z = orig_z
g = Grasp()
p = Pose()
p.position.x = x
p.position.y = y
p.position.z = z + 0.17
q = transformations.quaternion_from_euler(x_angle, y_angle, z_angle)
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
g.grasp_pose = p
grasps.append(g)
return grasps
def refine_and_grasp(self,
name,
arms_to_try,
table_frame_x_offset=0.0,
table_frame_y_offset=0.0,
desired_grasp_pose=None):
# table_frame_y_offset is positive if the robot moved to its right (meaning the table y position is greater in the robot's base frame)
object_to_grab = self.refine_and_and_get_graspable_object(
name, table_frame_x_offset, table_frame_y_offset)
if object_to_grab == None:
rospy.logwarn(
'no object with name was found in broad detection list: ' +
str(name))
return (0, -1, None)
for whicharm in arms_to_try:
self.pick_and_place_manager.check_preempted()
if desired_grasp_pose is not None:
desired_grasp = Grasp()
desired_grasp.grasp_pose = desired_grasp_pose
desired_grasps = [desired_grasp]
else:
desired_grasps = None
grasplogger = EventLoggerClient('grasp')
(result, arm_used, grasp_pose
) = self.pick_and_place_manager.grasp_object_and_check_success(
object_to_grab, whicharm, desired_grasps)
if result == 'succeeded':
grasplogger.stops()
rospy.loginfo('BTO: grasp success.')
return (whicharm, result, grasp_pose)
grasplogger.stopf()
rospy.logwarn('BTO: grasp failed')
return (whicharm, result, grasp_pose)
def place_click(self):
if (self.pickup_result):
grasp = self.pickup_result.grasp
self.pickupservice.pre_grasp_exec(grasp, 10.0)
else:
self.pickupservice.grasp_release_exec(10.0)
initial_pose = PoseStamped()
initial_pose.header.stamp = rospy.get_rostime()
initial_pose.header.frame_id = "/world" #"/fixed"
#fake hand to world pose
initial_pose.pose.position.x = 0.4 #self.box_pose.pose.position.x+0.0
initial_pose.pose.position.y = 0.091 # self.box_pose.pose.position.y-0.2
initial_pose.pose.position.z = 1.25 #self.box_pose.pose.position.z+0.05
q = transformations.quaternion_about_axis(-0.05, (0, 0, 1))
initial_pose.pose.orientation.x = 0.41 #self.box_pose.pose.orientation.x#q[0]
initial_pose.pose.orientation.y = 0.695 #self.box_pose.pose.orientation.y#q[1]
initial_pose.pose.orientation.z = 0.52 #self.box_pose.pose.orientation.z#q[2]
initial_pose.pose.orientation.w = 0.284 #self.box_pose.pose.orientation.w#q[3]
executed_grasp = Grasp()
executed_grasp.grasp_pose = copy.deepcopy(initial_pose.pose)
#fake obj world origin pose
initial_pose.pose.position.x = 0.43
initial_pose.pose.position.y = 0.149
initial_pose.pose.position.z = 1.088
initial_pose.pose.orientation.x = 0.0 #self.box_pose.pose.orientation.x#q[0]
initial_pose.pose.orientation.y = 0.0 #self.box_pose.pose.orientation.y#q[1]
initial_pose.pose.orientation.z = 0.567 #self.box_pose.pose.orientation.z#q[2]
initial_pose.pose.orientation.w = 0.824 #self.box_pose.pose.orientation.w#q[3]
#fake graspable object
graspable_object = GraspableObject()
graspable_object.reference_frame_id = "/world"
mypotentialmodels = DatabaseModelPose()
mypotentialmodels.model_id = 18744
mypotentialmodels.confidence = 1.0
mypotentialmodels.pose = copy.deepcopy(initial_pose)
graspable_object.potential_models.append(mypotentialmodels)
#fake obj world destination pose
initial_pose.pose.position.x = 0.43
initial_pose.pose.position.y = 0.0
initial_pose.pose.position.z = 1.088
initial_pose.pose.orientation.x = 0.0 #self.box_pose.pose.orientation.x#q[0]
initial_pose.pose.orientation.y = 0.0 #self.box_pose.pose.orientation.y#q[1]
initial_pose.pose.orientation.z = 0.567 #self.box_pose.pose.orientation.z#q[2]
initial_pose.pose.orientation.w = 0.824 #self.box_pose.pose.orientation.w#q[3]
list_of_poses = self.compute_list_of_poses(initial_pose,
graspable_object,
executed_grasp)
def build_simple_pickup_goal(grasp_joint_state, pre_grasp_joint_state, fingers_config):
# Adjust finger config un param server
rospy.set_param('/estirabot/skills/grasp/fingers_configuration', fingers_config)
g = Grasp()
g.grasp_posture = grasp_joint_state
g.pre_grasp_posture = pre_grasp_joint_state
r = PickupGoal()
r.desired_grasps.append(g)
return r
def grasp_release_exec(self,timeout_sec=10.0):
hand_posture_goal_ = GraspHandPostureExecutionGoal()
hand_posture_goal_.grasp = Grasp()
hand_posture_goal_.grasp.pre_grasp_posture.name= [ "FFJ0", "FFJ3", "FFJ4", "LFJ0", "LFJ3", "LFJ4", "LFJ5", "MFJ0", "MFJ3", "MFJ4", "RFJ0", "RFJ3", "RFJ4", "THJ1", "THJ2", "THJ3", "THJ4", "THJ5", "WRJ1", "WRJ2"]
hand_posture_goal_.grasp.pre_grasp_posture.position = [0]*(18)
# do release action
hand_posture_goal_.goal = GraspHandPostureExecutionGoal.RELEASE
# call hand_posture_exec action lib
rospy.loginfo("Trying to release...")
return self.hand_posture_exec(hand_posture_goal_,timeout_sec)
def plan_point_cluster_grasps(self, target, arm_name):
error_code = GraspPlanningErrorCode()
grasps = []
#get the hand joint names from the param server (loaded from yaml config file)
joint_names_dict = rospy.get_param('~joint_names')
pregrasp_joint_angles_dict = rospy.get_param('~pregrasp_joint_angles')
grasp_joint_angles_dict = rospy.get_param('~grasp_joint_angles')
pregrasp_joint_efforts_dict = rospy.get_param('~pregrasp_joint_efforts')
grasp_joint_efforts_dict = rospy.get_param('~grasp_joint_efforts')
if not arm_name:
arm_name = joint_names_dict.keys()[0]
rospy.logerr("point cluster grasp planner: missing arm_name in request! Using "+arm_name)
try:
hand_joints = joint_names_dict[arm_name]
except KeyError:
arm_name = joint_names_dict.keys()[0]
rospy.logerr("arm_name "+arm_name+" not found! Using joint names from "+arm_name)
hand_joints = joint_names_dict[arm_name]
pregrasp_joint_angles = pregrasp_joint_angles_dict[arm_name]
grasp_joint_angles = grasp_joint_angles_dict[arm_name]
pregrasp_joint_efforts = pregrasp_joint_efforts_dict[arm_name]
grasp_joint_efforts = grasp_joint_efforts_dict[arm_name]
#hand_joints = rospy.get_param('/hand_description/'+arm_name+'/hand_joints')
rospy.loginfo("hand_joints:"+str(hand_joints))
#find the cluster bounding box and relevant frames, and transform the cluster
init_start_time = time.time()
if len(target.cluster.points) > 0:
self.pcgp.init_cluster_grasper(target.cluster)
cluster_frame = target.cluster.header.frame_id
else:
self.pcgp.init_cluster_grasper(target.region.cloud)
cluster_frame = target.region.cloud.header.frame_id
if len(cluster_frame) == 0:
rospy.logerr("region.cloud.header.frame_id was empty!")
error_code.value = error_code.OTHER_ERROR
return (grasps, error_code)
init_end_time = time.time()
#print "init time: %.3f"%(init_end_time - init_start_time)
#plan grasps for the cluster (returned in the cluster frame)
grasp_plan_start_time = time.time()
(pregrasp_poses, grasp_poses, gripper_openings, qualities, pregrasp_dists) = self.pcgp.plan_point_cluster_grasps()
grasp_plan_end_time = time.time()
#print "total grasp planning time: %.3f"%(grasp_plan_end_time - grasp_plan_start_time)
#add error code to service
error_code.value = error_code.SUCCESS
grasp_list = []
if pregrasp_poses == None:
error_code.value = error_code.OTHER_ERROR
return (grasps, error_code)
#fill in the list of grasps
for (grasp_pose, quality, pregrasp_dist) in zip(grasp_poses, qualities, pregrasp_dists):
pre_grasp_joint_state = self.create_joint_state(hand_joints, pregrasp_joint_angles, pregrasp_joint_efforts)
grasp_joint_state = self.create_joint_state(hand_joints, grasp_joint_angles, grasp_joint_efforts)
#if the cluster isn't in the same frame as the graspable object reference frame,
#transform the grasps to be in the reference frame
if cluster_frame == target.reference_frame_id:
transformed_grasp_pose = grasp_pose
else:
transformed_grasp_pose = change_pose_stamped_frame(self.pcgp.tf_listener,
stamp_pose(grasp_pose, cluster_frame),
target.reference_frame_id).pose
if self.pcgp.pregrasp_just_outside_box:
min_approach_distance = pregrasp_dist
else:
min_approach_distance = max(pregrasp_dist-.05, .05)
grasp_list.append(Grasp(pre_grasp_posture=pre_grasp_joint_state, grasp_posture=grasp_joint_state,
grasp_pose=transformed_grasp_pose, success_probability=quality,
desired_approach_distance = pregrasp_dist, min_approach_distance = min_approach_distance))
#if requested, randomize the first few grasps
if self.randomize_grasps:
first_grasps = grasp_list[:30]
random.shuffle(first_grasps)
shuffled_grasp_list = first_grasps + grasp_list[30:]
grasps = shuffled_grasp_list
else:
grasps = grasp_list
return (grasps, error_code)
def generate_grasps(box,
num_grasps=4,
desired_approach_distance = 0.10,
min_approach_distance = 0.05,
effort = 50):
"""
Generates grasps
Parameters:
box: bounding box to genereate grasps for
num_grasps: number of grasps to generate, spaced equally around the box
desired_approach_distance: how far the pre-grasp should ideally be away from the grasp
min_approach_distance: how much distance between pre-grasp and grasp must actually be feasible for the grasp not to be rejected
Return: a list of grasps
"""
rospy.loginfo("Generating grasps")
grasps = []
for i in range(num_grasps):
g = Grasp()
# align z-axis rotation to box
euler = transformations.euler_from_quaternion([
box.pose.pose.orientation.x,
box.pose.pose.orientation.y,
box.pose.pose.orientation.z,
box.pose.pose.orientation.w])
rot_ang = euler[2]
# apply transformations based on rotations
rot_ang += i * (2 * math.pi) / num_grasps
t1 = transformations.translation_matrix([
box.pose.pose.position.x,
box.pose.pose.position.y,
box.pose.pose.position.z + \
box.box_dims.z * 0.05])
r1 = transformations.rotation_matrix(rot_ang, [0, 0, 1])
box_width = max(box.box_dims.x, box.box_dims.y)
t2 = transformations.translation_matrix([-(box_width/2 + 0.12), 0, 0])
mat = transformations.concatenate_matrices(t1, r1, t2)
translation = transformations.translation_from_matrix(mat)
g.grasp_pose.position.x = translation[0]
g.grasp_pose.position.y = translation[1]
g.grasp_pose.position.z = translation[2]
q = transformations.quaternion_from_matrix(mat)
g.grasp_pose.orientation.x = q[0]
g.grasp_pose.orientation.y = q[1]
g.grasp_pose.orientation.z = q[2]
g.grasp_pose.orientation.w = q[3]
g.desired_approach_distance = desired_approach_distance
g.min_approach_distance = min_approach_distance
pre_grasp_posture = JointState()
pre_grasp_posture.header.stamp = rospy.Time.now()
pre_grasp_posture.header.frame_id = 'base_link'
pre_grasp_posture.name = ['r_wrist_flex_link']
pre_grasp_posture.position = [0.8]
pre_grasp_posture.effort = [effort]
g.pre_grasp_posture = pre_grasp_posture
grasp_posture = JointState()
grasp_posture.header.stamp = rospy.Time.now()
grasp_posture.header.frame_id = 'base_link'
grasp_posture.name = ['r_wrist_flex_link']
grasp_posture.position = [0.45]
grasp_posture.effort = [effort]
g.grasp_posture = grasp_posture
grasps.append(g)
np.random.shuffle(grasps)
return grasps
def pick(self,pickup_goal):
#prepare result
pickresult = PickupResult()
#get grasps for the object
# fill up a grasp planning request
grasp_planning_req = GraspPlanningRequest()
grasp_planning_req.arm_name = pickup_goal.arm_name
grasp_planning_req.target = pickup_goal.target
object_to_attach = pickup_goal.collision_object_name
# call grasp planning service
grasp_planning_res = self.grasp_planning_service_.call(grasp_planning_req)
#print grasp_planning_res
# process grasp planning result
if (grasp_planning_res.error_code.value != grasp_planning_res.error_code.SUCCESS):
rospy.logerr("No grasp found for this object, we will generate some, but only when the node is ready for that !")
pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
return pickresult
else:
rospy.loginfo("Got "+ str(len(grasp_planning_res.grasps)) +" grasps for this object")
# for each grasp, generate rotational symmetric grasps around the object (this is already in the DB for the CokeCan but should be removed and done online)
#for each grasp, check path from pre-grasp pose to grasp pose first and then check motion to pre-grasp pose
motion_plan_res=GetMotionPlanResponse()
grasp_to_execute_=Grasp()
for index, grasp in enumerate(grasp_planning_res.grasps):
# extract grasp_pose
grasp_pose_ = PoseStamped()
grasp_pose_.header.frame_id = "/world";
grasp_pose_.pose = grasp.grasp_pose
grasp_pose_.pose.position.y=grasp_pose_.pose.position.y-0.01 #-0.01 in sim, +0.03 in real #cheating here
# copy the grasp_pose as a pre-grasp_pose
pre_grasp_pose_ = copy.deepcopy(grasp_pose_)
# add desired_approach_distance along the approach vector. above the object to plan pre-grasp pose
# currently add this to Z because approach vector needs to be computed somehow first (TODO)
pre_grasp_pose_.pose.position.z = pre_grasp_pose_.pose.position.z + 0.05
# for distance from 0 (grasp_pose) to desired_approach distance (pre_grasp_pose) test IK/Collision and save result
# decompose this in X steps depending on distance to do and max speed
interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(pre_grasp_pose_, grasp_pose_, False)
# check the result (depending on number of steps etc...)
if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS):
number_of_interpolated_steps=0
# check if one approach trajectory is feasible
for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
if traj_error_code.val!=1:
rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val))
break
else:
number_of_interpolated_steps=interpolation_index
# if trajectory is feasible then plan reach motion to pre-grasp pose
if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
rospy.loginfo("Grasp number "+str(index)+" approach is possible, checking motion plan to pre-grasp")
#print interpolated_motion_plan_res
# check and plan motion to this pre_grasp_pose
motion_plan_res = self.plan.plan_arm_motion( pickup_goal.arm_name, "jointspace", pre_grasp_pose_ )
#if this pre-grasp pose is successful do not test others
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
rospy.loginfo("Grasp number "+str(index)+" is possible, executing it")
# copy the grasp to execute for the following steps
grasp_to_execute_ = copy.deepcopy(grasp)
break
else:
rospy.logerr("Grasp number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
else:
rospy.logerr("Grasp number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
# execution part
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
#put hand in pre-grasp posture
if self.pre_grasp_exec(grasp_to_execute_)<0:
#QMessageBox.warning(self, "Warning",
# "Pre-grasp action failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
#go there
# filter the trajectory
filtered_traj = self.filter_traj_(motion_plan_res)
self.display_traj_( filtered_traj )
# reach pregrasp pose
if self.send_traj_( filtered_traj )<0:
#QMessageBox.warning(self, "Warning",
# "Reach trajectory execution failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
#time.sleep(20) # TODO use actionlib here
time.sleep(self.simdelay) # TODO use actionlib here
# approach
if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0:
#QMessageBox.warning(self, "Warning",
# "Approach trajectory execution failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
time.sleep(self.simdelay) # TODO use actionlib here
#grasp
if self.grasp_exec(grasp_to_execute_)<0:
#QMessageBox.warning(self, "Warning",
# "Grasp action failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
time.sleep(self.simdelay) # TODO use actionlib here
#attach the collision object to the hand (should be cleaned-up)
rospy.loginfo("Now we attach the object")
att_object = AttachedCollisionObject()
att_object.link_name = "palm"
att_object.object.id = object_to_attach
att_object.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
att_object.object.header.frame_id = "palm"
att_object.object.header.stamp = rospy.Time.now()
object = Shape()
object.type = Shape.CYLINDER
object.dimensions.append(.03)
object.dimensions.append(0.1)
pose = Pose()
pose.position.x = 0.0
pose.position.y = -0.06
pose.position.z = 0.06
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1
att_object.object.shapes.append(object)
att_object.object.poses.append(pose);
att_object.touch_links= ["ffdistal","mfdistal","rfdistal","lfdistal","thdistal","ffmiddle","mfmiddle","rfmiddle","lfmiddle","thmiddle","ffproximal","mfproximal","rfproximal","lfproximal","thproximal","palm","lfmetacarpal","thbase"]
self.att_object_in_map_pub_.publish(att_object)
rospy.loginfo("Attach object published")
else:
rospy.logerr("None of the grasps tested is possible")
pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
return pickresult
pickresult.manipulation_result.value = ManipulationResult.SUCCESS
pickresult.grasp= grasp_to_execute_
return pickresult
def go_to_mechanism_and_grasp_(self, suitcase):
# compute the full trajectory
semi_circle = self.compute_semi_circle_traj_(suitcase)
# compute the pregrasp and grasp
grasp = Grasp()
grasp_pose_ = PoseStamped()
# the grasp is the first item of the semi circle
grasp_pose_ = semi_circle[0]
# copy the grasp_pose as a pre-grasp_pose
pre_grasp_pose_ = copy.deepcopy(grasp_pose_)
# add desired_approach_distance along the approach vector. above the object to plan pre-grasp pose
# TODO: use the suitcase axis to approach from the perpendicular
pre_grasp_pose_.pose.position.x = pre_grasp_pose_.pose.position.x - self.APPROACH_DISTANCE
# TODO: find better postures
grasp.pre_grasp_posture.name = [
"FFJ0",
"FFJ3",
"FFJ4",
"LFJ0",
"LFJ3",
"LFJ4",
"LFJ5",
"MFJ0",
"MFJ3",
"MFJ4",
"RFJ0",
"RFJ3",
"RFJ4",
"THJ1",
"THJ2",
"THJ3",
"THJ4",
"THJ5",
"WRJ1",
"WRJ2",
]
grasp.pre_grasp_posture.position = [0.0] * 18
grasp.pre_grasp_posture.position[grasp.pre_grasp_posture.name.index("THJ4")] = 58.0
grasp.pre_grasp_posture.position[grasp.pre_grasp_posture.name.index("THJ5")] = -50.0
grasp.grasp_posture.name = [
"FFJ0",
"FFJ3",
"FFJ4",
"LFJ0",
"LFJ3",
"LFJ4",
"LFJ5",
"MFJ0",
"MFJ3",
"MFJ4",
"RFJ0",
"RFJ3",
"RFJ4",
"THJ1",
"THJ2",
"THJ3",
"THJ4",
"THJ5",
"WRJ1",
"WRJ2",
]
grasp.grasp_posture.position = [0.0] * 18
grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ1")] = 57.0
grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ2")] = 30.0
grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ3")] = -15.0
grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ4")] = 58.0
grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ5")] = 40.0
grasp.grasp_pose = grasp_pose_.pose
# for distance from 0 (grasp_pose) to desired_approach distance (pre_grasp_pose) test IK/Collision and save result
# decompose this in X steps depending on distance to do and max speed
motion_plan_res = GetMotionPlanResponse()
interpolated_motion_plan_res = self.execution.plan.get_interpolated_ik_motion_plan(
pre_grasp_pose_, grasp_pose_, False
)
# check the result (depending on number of steps etc...)
if interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS:
number_of_interpolated_steps = 0
# check if one approach trajectory is feasible
for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
if traj_error_code.val != 1:
rospy.logerr(
"One unfeasible approach-phase step found at "
+ str(interpolation_index)
+ "with val "
+ str(traj_error_code.val)
)
break
else:
number_of_interpolated_steps = interpolation_index
# if trajectory is feasible then plan reach motion to pre-grasp pose
if number_of_interpolated_steps + 1 == len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
rospy.loginfo("Grasp number approach is possible, checking motion plan to pre-grasp")
# print interpolated_motion_plan_res
# check and plan motion to this pre_grasp_pose
motion_plan_res = self.execution.plan.plan_arm_motion("right_arm", "jointspace", pre_grasp_pose_)
# execution part
if motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS:
# put hand in pre-grasp posture
if self.execution.pre_grasp_exec(grasp) < 0:
rospy.logerr("Failed to go in pregrasp.")
sys.exit()
# go there
# filter the trajectory
filtered_traj = self.execution.filter_traj_(motion_plan_res)
self.execution.display_traj_(filtered_traj)
# reach pregrasp pose
if self.execution.send_traj_(filtered_traj) < 0:
time.sleep(20) # TODO use actionlib here
# approach
if self.execution.send_traj_(interpolated_motion_plan_res.trajectory.joint_trajectory) < 0:
rospy.logerr("Failed to approach.")
sys.exit()
time.sleep(20) # TODO use actionlib here
# grasp
if self.execution.grasp_exec(grasp) < 0:
rospy.logerr("Failed to grasp.")
sys.exit()
time.sleep(20) # TODO use actionlib here
else:
# Failed, don't return the computed traj
return None
# return the full traj
return semi_circle
def pick_object(self, object_to_pick, collision_support_surface_name):
self.server.publish_feedback(
PickObjectFeedback(1, "Finding object bounding box"))
self.object_name = object_to_pick.model_description.name
self.object = object_to_pick
graspable_object = self.object.graspable_object
# draw a bounding box around the selected object
(box_pose, box_dims) = self.call_find_cluster_bounding_box(
graspable_object.cluster)
if box_pose == None:
return (1, None, None)
box_mat = pose_to_mat(box_pose.pose)
rospy.logerr("box_pose %f %f %f q: %f %f %f %f",
box_pose.pose.position.x, box_pose.pose.position.y,
box_pose.pose.position.z, box_pose.pose.orientation.x,
box_pose.pose.orientation.y, box_pose.pose.orientation.z,
box_pose.pose.orientation.w)
box_ranges = [[-box_dims.x / 2, -box_dims.y / 2, -box_dims.z / 2],
[box_dims.x / 2, box_dims.y / 2, box_dims.z / 2]]
self.box_pose = box_pose
self.draw_functions.draw_rviz_box(box_mat,
box_ranges,
'/world',
ns='bounding box',
color=[0, 0, 1],
opaque=0.25,
duration=60)
if self.server.is_preempt_requested():
self.server.publish_feedback(
PickObjectFeedback(50, "Pick action cancelled"))
self.server.set_preempted()
return (1, None, None)
self.server.publish_feedback(
PickObjectFeedback(10, "Calling pick up service"))
# call the pickup service
res = self.pickup(graspable_object, self.object.graspable_object_name,
self.object_name, collision_support_surface_name)
initial_pose = PoseStamped()
initial_pose.header.stamp = rospy.get_rostime()
initial_pose.header.frame_id = "/world"
initial_pose.pose.position.x = self.box_pose.pose.position.x
initial_pose.pose.position.y = self.box_pose.pose.position.y
initial_pose.pose.position.z = self.box_pose.pose.position.z - box_dims.z / 2 # graspable object is from bottom but bounding box is at center !
initial_pose.pose.orientation.x = self.box_pose.pose.orientation.x
initial_pose.pose.orientation.y = self.box_pose.pose.orientation.y
initial_pose.pose.orientation.z = self.box_pose.pose.orientation.z
initial_pose.pose.orientation.w = self.box_pose.pose.orientation.w
if res == 0: #correctly picked up
executed_grasp = self.pickup_result.grasp
else:
executed_grasp = Grasp()
return (res, initial_pose, executed_grasp)