def check_place_at_obj_pose_feasible(self, obj_region, obj_pose,
swept_volume_to_avoid):
obj = self.robot.GetGrabbed()[0]
if type(obj_region) == str:
obj_region = self.problem_env.regions[obj_region]
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)
is_feasible = self.is_collision_and_region_constraints_satisfied(
target_robot_region1, target_robot_region2, target_obj_region)
if not is_feasible:
action = {
'operator_name': 'two_arm_place',
'q_goal': None,
'object_pose': None,
'action_parameters': obj_pose
}
self.robot.SetTransform(original_trans)
obj.SetTransform(original_obj_trans)
return action, 'NoSolution'
else:
release_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(obj)
action = {
'operator_name': 'two_arm_place',
'q_goal': None,
'object_pose': None,
'action_parameters': obj_pose
}
return action, 'NoSolution'
self.robot.SetTransform(original_trans)
obj.SetTransform(original_obj_trans)
grab_obj(obj)
action = {
'operator_name': 'two_arm_place',
'q_goal': robot_xytheta,
'object_pose': obj_pose,
'action_parameters': obj_pose
}
return action, 'HasSolution'
def compute_robot_base_pose_given_object_pose(obj, robot, obj_pose, T_r_wrt_o):
original_robot_T = robot.GetTransform()
release_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(obj)
robot.SetTransform(original_robot_T)
return robot_xytheta
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()
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(obj)
return obj_pose, robot_xytheta
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'
def check_feasibility(self, operator_skeleton, place_parameters, swept_volume_to_avoid=None):
obj = self.robot.GetGrabbed()[0]
obj_original_pose = obj.GetTransform()
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['place_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
if self.action_mode == 'object_pose':
obj_pose = place_parameters
new_base_pose = self.place_object_and_robot_at_new_pose(obj, obj_pose, obj_region)
else:
utils.set_robot_config(place_parameters)
obj_pose = utils.get_body_xytheta(obj)
new_base_pose = place_parameters
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)
if len(self.robot.GetGrabbed()) == 0:
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)
if len(self.robot.GetGrabbed()) == 0:
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'
