def is_collision_and_region_constraints_satisfied(self, target_robot_region1, target_robot_region2,
target_obj_region):
target_region_contains = target_robot_region1.contains(self.robot.ComputeAABB()) or \
target_robot_region2.contains(self.robot.ComputeAABB())
obj = self.robot.GetGrabbed()[0]
if not target_region_contains:
return False
if not target_obj_region.contains(obj.ComputeAABB()):
return False
is_object_pose_infeasible = self.env.CheckCollision(obj)
if is_object_pose_infeasible:
return False
is_base_pose_infeasible = self.env.CheckCollision(self.robot)
if is_base_pose_infeasible:
return False
else:
dof_vals = self.robot.GetDOFValues()
utils.release_obj()
utils.fold_arms()
self.problem_env.set_robot_to_default_dof_values()
is_base_pose_infeasible = self.env.CheckCollision(self.robot)
self.robot.SetDOFValues(dof_vals)
utils.grab_obj(obj)
if is_base_pose_infeasible:
return False
return True
def apply_pick_constraint(self,
obj_name,
pick_config,
pick_base_pose=None):
# todo I think this function can be removed?
obj = self.env.GetKinBody(obj_name)
if pick_base_pose is not None:
set_robot_config(pick_base_pose, self.robot)
self.robot.SetDOFValues(pick_config)
grab_obj(self.robot, obj)
def restore(self, state_saver):
curr_obj = state_saver.curr_obj
which_operator = state_saver.which_operator
if not which_operator == 'two_arm_pick':
grab_obj(self.robot, curr_obj)
else:
if len(self.robot.GetGrabbed()) > 0:
release_obj(self.robot, self.robot.GetGrabbed()[0])
state_saver.Restore()
self.robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y
| DOFAffine.RotationAxis, [0, 0, 1])
def compute_robot_base_pose_given_object_pose(obj, robot, obj_pose,
T_r_wrt_o):
original_robot_T = robot.GetTransform()
release_obj(robot, obj)
set_obj_xytheta(obj_pose, obj)
new_T_robot = np.dot(obj.GetTransform(), T_r_wrt_o)
robot.SetTransform(new_T_robot)
robot.SetActiveDOFs([],
DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis,
[0, 0, 1])
robot_xytheta = robot.GetActiveDOFValues()
grab_obj(robot, obj)
robot.SetTransform(original_robot_T)
return robot_xytheta
def get_objects_in_collision_with_given_op_inst(self, op_inst):
saver = CustomStateSaver(self.problem_env.env)
if len(self.problem_env.robot.GetGrabbed()) > 0:
held = self.problem_env.robot.GetGrabbed()[0]
release_obj()
else:
held = None
fold_arms()
new_cols = self.problem_env.get_objs_in_collision(op_inst.low_level_motion, 'entire_region')
saver.Restore()
if held is not None:
grab_obj(held)
return new_cols
def is_swept_volume_cleared(self, obj):
saver = CustomStateSaver(self.problem_env.env)
if len(self.problem_env.robot.GetGrabbed()) > 0:
held = self.problem_env.robot.GetGrabbed()[0]
release_obj()
else:
held = None
collision_occurred = self.pick_swept_volume.is_collision_in_all_volumes(obj) \
or self.place_swept_volume.is_collision_in_all_volumes(obj)
saver.Restore()
if held is not None:
grab_obj(held)
if collision_occurred:
return False
else:
return True
def get_placement(self, obj, target_obj_region, T_r_wrt_o):
original_trans = self.robot.GetTransform()
original_config = self.robot.GetDOFValues()
self.robot.SetTransform(original_trans)
self.robot.SetDOFValues(original_config)
release_obj(self.robot, obj)
with self.robot:
# print target_obj_region
obj_pose = randomly_place_in_region(
self.env, obj, target_obj_region) # randomly place obj
obj_pose = obj_pose.squeeze()
# compute the resulting robot transform
new_T_robot = np.dot(obj.GetTransform(), T_r_wrt_o)
self.robot.SetTransform(new_T_robot)
self.robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y
| DOFAffine.RotationAxis, [0, 0, 1])
robot_xytheta = self.robot.GetActiveDOFValues()
set_robot_config(robot_xytheta, self.robot)
grab_obj(self.robot, obj)
return obj_pose, robot_xytheta
def check_feasibility(self, operator_skeleton, place_parameters, swept_volume_to_avoid=None):
obj = self.robot.GetGrabbed()[0]
obj_original_pose = obj.GetTransform()
obj_pose = place_parameters
robot_original_xytheta = get_body_xytheta(self.robot)
robot_original_config = self.robot.GetDOFValues()
grasp_params = operator_skeleton.continuous_parameters['grasp_params']
obj_region = operator_skeleton.discrete_parameters['region']
if type(obj_region) == str:
obj_region = self.problem_env.regions[obj_region]
target_robot_region = self.problem_env.regions['home_region']
target_obj_region = obj_region
new_base_pose = self.place_object_and_robot_at_new_pose(obj, obj_pose, obj_region)
is_object_pose_infeasible = self.env.CheckCollision(obj) or \
(not target_obj_region.contains(obj.ComputeAABB()))
if not is_object_pose_infeasible:
if swept_volume_to_avoid is not None:
is_object_pose_infeasible = not swept_volume_to_avoid.is_swept_volume_cleared(obj)
if is_object_pose_infeasible:
action = {'operator_name': 'one_arm_place', 'q_goal': None, 'base_pose': None, 'object_pose': None,
'action_parameters': obj_pose, 'grasp_params': grasp_params}
set_robot_config(robot_original_xytheta)
self.robot.SetDOFValues(robot_original_config)
obj.SetTransform(obj_original_pose)
grab_obj(obj)
return action, 'InfeasibleBase'
is_base_pose_infeasible = self.env.CheckCollision(self.robot) or \
(not target_robot_region.contains(self.robot.ComputeAABB()))
if is_base_pose_infeasible:
for i in range(3):
obj_pose[-1] += 90 * np.pi / 180.0
new_base_pose = self.place_object_and_robot_at_new_pose(obj, obj_pose, obj_region)
# is_object_pose_infeasible = self.env.CheckCollision(obj) or \
# (not target_obj_region.contains(obj.ComputeAABB()))
is_base_pose_infeasible = self.env.CheckCollision(self.robot) or \
(not target_robot_region.contains(self.robot.ComputeAABB()))
if not (is_base_pose_infeasible or is_object_pose_infeasible):
break
if is_base_pose_infeasible or is_object_pose_infeasible:
action = {'operator_name': 'one_arm_place', 'q_goal': None, 'base_pose': None, 'object_pose': None,
'action_parameters': obj_pose, 'grasp_params': grasp_params}
set_robot_config(robot_original_xytheta)
self.robot.SetDOFValues(robot_original_config)
obj.SetTransform(obj_original_pose)
grab_obj(obj)
return action, 'InfeasibleBase'
grasp_config = self.solve_ik_from_grasp_params(obj, grasp_params)
#self.problem_env.enable_objects_in_region('entire_region')
#[o.Enable(True) for o in self.problem_env.boxes]
if grasp_config is None:
action = {'operator_name': 'one_arm_place', 'base_pose': None, 'object_pose': None,
'q_goal': None, 'action_parameters': obj_pose, 'g_config': grasp_config,
'grasp_params': grasp_params}
set_robot_config(robot_original_xytheta)
self.robot.SetDOFValues(robot_original_config)
obj.SetTransform(obj_original_pose)
grab_obj(obj)
return action, 'InfeasibleIK'
else:
grasp_config = grasp_config.squeeze()
new_base_pose = new_base_pose.squeeze()
action = {'operator_name': 'one_arm_place', 'q_goal': np.hstack([grasp_config, new_base_pose]),
'base_pose': new_base_pose, 'object_pose': place_parameters,
'action_parameters': obj_pose, 'g_config': grasp_config, 'grasp_params': grasp_params}
set_robot_config(robot_original_xytheta)
self.robot.SetDOFValues(robot_original_config)
obj.SetTransform(obj_original_pose)
grab_obj(obj)
return action, 'HasSolution'
def check_feasibility(self,
operator_skeleton,
place_parameters,
swept_volume_to_avoid=None):
# Note:
# this function checks if the target region contains the robot when we place object at place_parameters
# and whether the robot will be in collision
obj = self.robot.GetGrabbed()[0]
obj_region = operator_skeleton.discrete_parameters['region']
if type(obj_region) == str:
obj_region = self.problem_env.regions[obj_region]
obj_pose = place_parameters
T_r_wrt_o = np.dot(np.linalg.inv(obj.GetTransform()),
self.robot.GetTransform())
original_trans = self.robot.GetTransform()
original_obj_trans = obj.GetTransform()
target_robot_region1 = self.problem_env.regions['home_region']
target_robot_region2 = self.problem_env.regions['loading_region']
target_obj_region = obj_region # for fetching, you want to move it around
robot_xytheta = self.compute_robot_base_pose_given_object_pose(
obj, self.robot, obj_pose, T_r_wrt_o)
set_robot_config(robot_xytheta, self.robot)
# why do I disable objects in region? Most likely this is for minimum constraint computation
#self.problem_env.disable_objects_in_region('entire_region')
target_region_contains = target_robot_region1.contains(self.robot.ComputeAABB()) or \
target_robot_region2.contains(self.robot.ComputeAABB())
is_base_pose_infeasible = self.env.CheckCollision(self.robot) or \
(not target_region_contains)
is_object_pose_infeasible = self.env.CheckCollision(obj) or \
(not target_obj_region.contains(obj.ComputeAABB()))
#self.problem_env.enable_objects_in_region('entire_region')
if is_base_pose_infeasible or is_object_pose_infeasible:
action = {
'operator_name': 'two_arm_place',
'base_pose': None,
'object_pose': None,
'action_parameters': place_parameters
}
self.robot.SetTransform(original_trans)
obj.SetTransform(original_obj_trans)
return action, 'NoSolution'
else:
release_obj(self.robot, obj)
if swept_volume_to_avoid is not None:
# release the object
if not swept_volume_to_avoid.is_swept_volume_cleared(obj):
self.robot.SetTransform(original_trans)
obj.SetTransform(original_obj_trans)
grab_obj(self.robot, obj)
action = {
'operator_name': 'two_arm_place',
'base_pose': None,
'object_pose': None,
'action_parameters': place_parameters
}
return action, 'NoSolution'
"""
for config in swept_volume_to_avoid:
set_robot_config(config, self.robot)
if self.env.CheckCollision(self.robot, obj):
self.robot.SetTransform(original_trans)
obj.SetTransform(original_obj_trans)
grab_obj(self.robot, obj)
action = {'operator_name': 'two_arm_place', 'base_pose': None, 'object_pose': None,
'action_parameters': place_parameters}
return action, 'NoSolution'
"""
self.robot.SetTransform(original_trans)
obj.SetTransform(original_obj_trans)
grab_obj(self.robot, obj)
action = {
'operator_name': 'two_arm_place',
'base_pose': robot_xytheta,
'object_pose': obj_pose,
'action_parameters': place_parameters
}
return action, 'HasSolution'