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