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 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 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 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 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 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)
