def get_collision_free_surface_pose(good_bodies, robot, obj,
max_trials = 500,
use_general_grasps = True):
env = robot.GetEnv()
robot_pose = robot.GetTransform()
torso_angle = robot.GetJoint("torso_lift_joint").GetValues()[0]
manip = robot.GetActiveManipulator()
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot,iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
grasping_poses = generate_manip_above_surface(obj)
env.GetCollisionChecker().SetCollisionOptions(openravepy.CollisionOptions.Contacts)
collision = env.CheckCollision(robot)
sol, _ = check_reachable(good_bodies, env, obj, manip, grasping_poses)
isreachable = sol is not None
min_torso, max_torso = utils.get_pr2_torso_limit(robot)
num_trial = 0
with robot:
while ((collision) or (not isreachable)) and (num_trial < max_trials):
num_trial +=1
torso_angle = move_random_torso(robot, min_torso, max_torso)
robot_pose = generate_random_pos(robot, obj)
robot.SetTransform(robot_pose)
report = openravepy.CollisionReport()
collision = env.CheckCollision(robot, report=report)
if not collision:
grasping_poses = generate_manip_above_surface(obj)
sol, _ = check_reachable(good_bodies, env, obj, manip, grasping_poses)
isreachable = sol is not None
else:
continue
if (sol is None) or collision:
raise GraspingPoseError("No collision free grasping pose found within %d steps" % max_trials)
else:
return (robot_pose, sol, torso_angle)
def get_collision_free_ik_pose(good_bodies, robot, object_to_grasp,
ik_pose,
max_trials = 100,
only_reachable=False
):
"""Returns the position from where the robot can reach a position (in
cartesian coordinates). The active manipulator is used.
Parameters:
robot: an OpenRave robot instance
ik_pose: a 4x4 matrix with the desired 6D pose
max_trials: how many attempts before giving up
use_general_grasps: f True, don't calculate actual grasp points, but use
a pre-generated list. It is much faster if a grasping model has not been
generated.
Returns:
(robot_pose, sol, torso_angle): the robot position (as a transformation matrix),
the active manipulator angles and the torso joint angle from where the robot
can grasp an object.
Raises GraspingPoseError if no valid solution is found.
"""
env = robot.GetEnv()
robot_pose = robot.GetTransform()
torso_angle = robot.GetJoint("torso_lift_joint").GetValues()[0]
manip = robot.GetActiveManipulator()
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot,iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
env.GetCollisionChecker().SetCollisionOptions(openravepy.CollisionOptions.Contacts)
collision = env.CheckCollision(robot)
sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, [ik_pose], only_reachable)
isreachable = sol is not None
min_torso, max_torso = utils.get_pr2_torso_limit(robot)
num_trial = 0
xyz = ik_pose[:3, 3]
with robot:
while ((collision) or (not isreachable)) and (num_trial < max_trials):
num_trial +=1
torso_angle = move_random_torso(robot, min_torso, max_torso)
robot_pose = generate_random_pos(robot, xyz)
robot.SetTransform(robot_pose)
report = openravepy.CollisionReport()
collision = env.CheckCollision(robot, report=report)
if not collision:
sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, [ik_pose], only_reachable)
isreachable = sol is not None
else:
continue
if (sol is None) or collision:
e = GraspingPoseError("No collision free IK pose found within %d steps" % max_trials)
raise e
else:
return (robot_pose, sol, torso_angle)
def get_torso_grasping_pose(good_bodies, robot,
object_to_grasp,
max_trials = 100,
use_general_grasps = True,
):
"""Returns the torso height from where the robot can grasp an object.
Parameters:
robot: an OpenRave robot instance
object_to_grasp: a KinBody that the robot should grasp
max_trials: how many attempts before giving up
use_general_grasps: f True, don't calculate actual grasp points, but use
a pre-generated list. It is much faster if a grasping model has not been
generated.
Returns:
(sol, torso_angle): ,
the active manipulator angles and the torso joint angle from where the robot
can grasp an object.
Raises GraspingPoseError if no valid solution is found.
"""
env = robot.GetEnv()
robot_pose = robot.GetTransform()
torso_angle = robot.GetJoint("torso_lift_joint").GetValues()[0]
manip = robot.GetActiveManipulator()
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot,iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
grasping_poses = generate_grasping_pose(robot, object_to_grasp,
use_general_grasps)
openravepy.raveLogInfo("I've got %d grasping poses" % len(grasping_poses))
env.GetCollisionChecker().SetCollisionOptions(openravepy.CollisionOptions.Contacts)
collision = env.CheckCollision(robot)
sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, grasping_poses)
isreachable = sol is not None
min_torso, max_torso = utils.get_pr2_torso_limit(robot)
num_trial = 0
with robot:
while ((collision) or (not isreachable)) and (num_trial < max_trials):
num_trial +=1
torso_angle = move_random_torso(robot, min_torso, max_torso)
report = openravepy.CollisionReport()
collision = env.CheckCollision(robot, report=report)
if not collision:
grasping_poses = generate_grasping_pose(robot,
object_to_grasp,
use_general_grasps)
sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, grasping_poses)
isreachable = sol is not None
else:
continue
if (sol is None) or collision:
e = GraspingPoseError("No collision free grasping pose found within %d steps" % max_trials)
raise e
else:
return (sol, torso_angle)
def get_occluding_objects(good_bodies, robot,
object_to_grasp,
max_trials = 0,
just_one_attempt = False,
return_pose = False,
body_filter = None
):
"""Generates a list of all the objects that prevent the robot from reaching
a target object. Several (up to max_trials) attempts are performed to grasp
Parameters:
robot: a openravepy.Robot instance
object_to_grasp: a openravepy.KinBody instance representing the object to grasp
Returns:
a list of sets of objects
"""
if just_one_attempt != return_pose:
raise ValueError("If asking for a return poes then set just_one_attempt to True")
env = robot.GetEnv()
robot_pose = robot.GetTransform()
manip = robot.GetActiveManipulator()
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot,iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
min_torso, max_torso = utils.get_pr2_torso_limit(robot)
num_trial = 0
collisions_list = []
with robot:
while num_trial < max_trials:
num_trial +=1
# sample a base pose
torso_angle = generate_reaching_poses.move_random_torso(robot,
min_torso,
max_torso)
robot_pose = generate_reaching_poses.generate_random_pos(robot,
object_to_grasp)
robot.SetTransform(robot_pose)
report = openravepy.CollisionReport()
collision = env.CheckCollision(robot, report=report)
if not collision:
openravepy.raveLogInfo("Got a position not in collision")
#sample a gripper pose
grasping_poses = generate_reaching_poses.generate_grasping_pose(robot,
object_to_grasp,
use_general_grasps = True,
checkik=False)
openravepy.raveLogInfo("Got %d grasping poses" % len(grasping_poses))
#check if gripper pose is reachable from base pose
#use robot's base pose to transform precomputed
#gripper poses into the robot's frame of reference
sol, collisions = generate_reaching_poses.check_reachable(good_bodies,
env, object_to_grasp, manip, grasping_poses, only_reachable = True)
if sol is None:
print "Finding collisions: trial {0} No sol from base pose to gripper pose".\
format(num_trial)
goodCollisions = False
if body_filter is not None:
goodCollisions = filter(body_filter, collisions)
print "good collisions: " + repr(goodCollisions)
print "all collisions: " + repr(collisions)
if sol is not None and goodCollisions:
print "Sol from base pose to gripper pose found in trial {0}".\
format(num_trial)
openravepy.raveLogInfo("Getting the list of collisions")
with robot:
#robot.SetDOFValues(sol, robot.GetActiveManipulator().GetArmIndices());
#collisions_list.append(utils.get_all_collisions(robot, env))
collisions_list.append(collisions)
if just_one_attempt:
if return_pose:
return (robot_pose, sol, torso_angle, collisions_list)
else:
return collisions_list
if num_trial == max_trials:
print "Getting obj names: No gripper pose reachable from collision free base pose found",
print "after {0} trials".format(num_trial)
if return_pose:
return (robot_pose, None, torso_angle, collisions_list)
else:
return collisions_list
