def find_bp(self, surface):
"""""Find a base pose for putting down
"""""
print "Getting base pose for putting down"
robot_pose, _, _= generate_reaching_poses.get_collision_free_surface_pose(self.getGoodBodies(),
self.robot,
surface,
)
return robot_pose
def find_bp(self, surface):
"""""Find a base pose for putting down
""" ""
print "Getting base pose for putting down"
robot_pose, _, _ = generate_reaching_poses.get_collision_free_surface_pose(
self.getGoodBodies(),
self.robot,
surface,
)
return robot_pose
def putdown(self, obj_name, table_name, _unused1):
print "Putting down object %s on %s" % (obj_name, table_name)
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
if table_name.startswith("dest_"):
#this is a fixed location!!!
T = getattr(tray_world, table_name, None)
if T is None:
raise ValueError(
"The location %s is unknown! check spelling?" % table_name)
T = T.copy()
#put the gripper facing down
gripper_angle = (np.pi, 0., 0) #just got this from trial and test
rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle)
T[:3, :3] = rot_mat
T[2, 3] += 0.03
try:
(pose, sol, torso_angle
) = generate_reaching_poses.get_collision_free_ik_pose(
self.getGoodBodies(),
self.env,
obj,
self.robot,
T,
only_reachable=False,
max_trials=1000)
except generate_reaching_poses.GraspingPoseError:
raise ExecutingException(
"Putting down on location has problems!")
else:
table = self.env.GetKinBody(table_name)
if table is None:
raise ValueError("Object %s does not exist" % table_name)
try:
pose, sol, torso_angle = generate_reaching_poses.get_collision_free_surface_pose(
self.getGoodBodies(),
self.robot,
table,
)
except generate_reaching_poses.GraspingPoseError, e:
raise e
def putdown(self, obj_name, table_name, _unused1):
print "Putting down object %s on %s" %(obj_name, table_name)
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
if table_name.startswith("dest_"):
#this is a fixed location!!!
T = getattr(tray_world, table_name, None)
if T is None:
raise ValueError("The location %s is unknown! check spelling?" % table_name)
T = T.copy()
#put the gripper facing down
gripper_angle = (np.pi, 0., 0) #just got this from trial and test
rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle)
T[:3,:3] = rot_mat
T[2, 3] += 0.03
try:
(pose,
sol,
torso_angle) = generate_reaching_poses.get_collision_free_ik_pose(
self.getGoodBodies(),
self.env,
obj,
self.robot,
T,
only_reachable=False,
max_trials =1000
)
except generate_reaching_poses.GraspingPoseError:
raise ExecutingException("Putting down on location has problems!")
else:
table = self.env.GetKinBody(table_name)
if table is None:
raise ValueError("Object %s does not exist" % table_name)
try:
pose, sol, torso_angle = generate_reaching_poses.get_collision_free_surface_pose(self.getGoodBodies(),
self.robot,
table,
)
except generate_reaching_poses.GraspingPoseError, e:
raise e
