def initiate_crp_problem(self, target_obj, action, region_name):
self.pick_region_name = region_name
self.pick_config, self.pick_grasp = action
self.prepick_config = get_body_xytheta(self.robot)
self.prepick_state_saver = DynamicEnvironmentStateSaver(
self.problem_env.env)
self.namo_target_obj = target_obj
def check_feasibility(self, node, action):
robot_xytheta = get_body_xytheta(self.robot).squeeze()
new_q = robot_xytheta + action
self.problem_env.disable_objects(
) # note that this class is only for mcr purpose
set_robot_config(new_q, self.problem_env.robot)
if self.collision_fn(new_q) or \
(not self.problem_env.regions['entire_region'].contains(self.robot.ComputeAABB())):
action = {
'operator_name': 'next_base_pose',
'base_pose': None,
'action_parameters': action
}
status = "NoSolution"
else:
action = {
'operator_name': 'next_base_pose',
'base_pose': robot_xytheta,
'action_parameters': action
}
status = 'HasSolution'
self.problem_env.enable_objects()
set_robot_config(robot_xytheta, self.problem_env.robot)
return action, status
def update_target_namo_place_base_pose(self):
T_robot = self.get_world_robot_transform_given_obj_transform(
self.packing_region_obj)
with self.robot:
self.robot.SetTransform(T_robot)
fetch_place_base_pose = get_body_xytheta(self.robot)
self.fetch_place_conf[-3:] = fetch_place_base_pose
self.fetch_place_op_instance['action'][
'base_pose'] = fetch_place_base_pose
def get_rotations_around_z_wrt_gripper(self, object, conf):
# todo place the object at the designated conf
self.problem_env.set_arm_base_config(conf)
T_robot_obj = self.get_robot_transform_wrt_obj(object)
conf_list = []
for rotation in [0, np.pi / 2, np.pi, 3 * np.pi / 2]:
xytheta = get_body_xytheta(object)
xytheta[0, -1] = xytheta[0, -1] + rotation
set_obj_xytheta(xytheta, object)
T_obj_new = object.GetTransform()
T_robot_new = np.dot(T_obj_new, T_robot_obj)
self.robot.SetTransform(T_robot_new)
new_conf = copy.deepcopy(conf)
new_conf[-3:] = get_body_xytheta(self.robot).squeeze()
conf_list.append(new_conf)
return conf_list
def randomly_place_in_region(env, body, region):
if env.GetKinBody(get_name(body)) is None: env.Add(body)
for i in range(1000):
set_quat(body, quat_from_z_rot(uniform(0, 2 * PI)))
aabb = aabb_from_body(body)
cspace = region.cspace(aabb)
if cspace is None: continue
set_point(body, np.array([uniform(*cspace_range) for cspace_range in cspace] + [
region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body))
if not env.CheckCollision(body):
return get_body_xytheta(body)
return None
def fcn(q_init, q_goal):
while True:
if np.all(q_init == q_goal):
yield ([q_init],)
curr_robot_config = get_body_xytheta(namo.robot)
set_robot_config(q_init, namo.robot)
namo.disable_objects_in_region('entire_region')
plan, status = namo.get_base_motion_plan(q_goal)
namo.enable_objects_in_region('entire_region')
set_robot_config(curr_robot_config, namo.robot)
if status == 'HasSolution':
yield (plan,)
else:
import pdb;pdb.set_trace()
yield None
def namo_domain_initialize_namo_problem(self, fetch_plan, fetch_goal_node):
self.fetching_obj = self.env.GetKinBody(fetch_plan[0]['obj_name'])
self.fetch_pick_op_instance = fetch_plan[0]
#self.fetch_place_op_instance = fetch_plan[1]
self.fetch_pick_path = fetch_plan[0]['path']
#self.fetch_place_path = fetch_plan[1]['path']
self.fetch_pick_conf = self.problem_env.make_config_from_op_instance(
self.fetch_pick_op_instance)
#self.fetch_place_conf = self.problem_env.make_config_from_op_instance(self.fetch_place_op_instance)
self.problem_env.high_level_planner = self.high_level_controller
self.fetch_goal_node = fetch_goal_node
self.fetch_goal_node.state_saver = DynamicEnvironmentStateSaver(
self.env) # this assumes that we're in prefetch state
self.prefetching_robot_config = get_body_xytheta(
self.problem_env.robot).squeeze()
def gaussian_randomly_place_in_region(env, body, region, center, var):
if env.GetKinBody(get_name(body)) is None:
env.Add(body)
for i in range(1000):
xytheta = np.random.normal(center, var)
set_obj_xytheta(xytheta, body)
if not body_collision(env, body):
return xytheta
import pdb;pdb.set_trace()
for i in range(1000):
set_quat(body, quat_from_z_rot(uniform(0, 2 * PI)))
aabb = aabb_from_body(body)
cspace = region.cspace(aabb)
if cspace is None: continue
set_point(body, np.array([uniform(*cspace_range) for cspace_range in cspace] + [
region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body))
if not body_collision(env, body):
return get_body_xytheta(body)
return None
def initialize_namo_problem(self, fetch_plan, target_obj, fetch_goal_node,
fetch_pick_path_exit,
fetch_place_path_entrance,
target_packing_region_name):
self.packing_region_obj = None
self.is_movable_packing_region = target_packing_region_name.find(
'packing_box') != -1
self.fetching_obj = target_obj
self.fetch_pick_op_instance = fetch_plan[0]
self.fetch_place_op_instance = fetch_plan[1]
self.fetch_pick_path = fetch_plan[0]['path']
self.fetch_place_path = fetch_plan[1]['path']
self.fetch_pick_path_exit = fetch_pick_path_exit
self.fetch_place_path_entrance = fetch_place_path_entrance
self.fetch_pick_conf = self.problem_env.make_config_from_op_instance(
self.fetch_pick_op_instance)
self.fetch_place_conf = self.problem_env.make_config_from_op_instance(
self.fetch_place_op_instance)
self.problem_env.high_level_planner = self.high_level_controller
if self.is_movable_packing_region:
# getting relative robot base config wrt the packing region object
with self.robot:
self.set_arm_base_config(self.fetch_place_conf)
self.packing_region_obj = self.problem_env.env.GetKinBody(
target_packing_region_name)
self.T_robot_obj = self.get_robot_transform_wrt_obj(
self.packing_region_obj)
self.target_namo_place_base_pose = self.fetch_place_conf
self.fetch_goal_node = fetch_goal_node
self.prefetching_robot_config = get_body_xytheta(
self.problem_env.robot).squeeze()
def check_two_arm_place_feasibility(self, namo_obj, action,
obj_placement_region):
curr_region = self.problem_env.get_region_containing(
self.problem_env.robot)
if obj_placement_region.name.find('shelf') != -1:
motion_planning_region_name = 'home_region'
else:
motion_planning_region_name = obj_placement_region.name
print motion_planning_region_name
goal_robot_xytheta = action['base_pose']
pick_base_pose = get_body_xytheta(self.problem_env.robot)
pick_conf = self.problem_env.robot.GetDOFValues()
current_collisions = self.curr_namo_object_names
self.prev_namo_object_names = current_collisions
namo_status = 'NoPath'
namo_place_motion = None
if self.problem_env.check_base_pose_feasible(
goal_robot_xytheta, namo_obj,
self.problem_env.regions[motion_planning_region_name]):
namo_place_motion, namo_status = self.problem_env.get_base_motion_plan(
goal_robot_xytheta, motion_planning_region_name)
if namo_status == 'NoPath':
return namo_place_motion, namo_status, self.curr_namo_object_names
two_arm_place_object(namo_obj, self.problem_env.robot, action)
if self.is_c_init_pre_contact:
fetch_pick_path, fetching_pick_status = self.get_new_fetching_pick_path(
namo_obj, motion_planning_region_name)
if fetching_pick_status == "NoPath":
return None, 'NoPath', self.curr_namo_object_names
else:
fetch_pick_path = self.fetch_pick_path
packing_region_moved = self.packing_region_obj == namo_obj # note this can only happen in the pre_contact stage
if packing_region_moved:
self.update_target_namo_place_base_pose()
fetch_place_path, fetching_place_status = self.get_new_fetching_place_path(
namo_obj, motion_planning_region_name)
if fetching_place_status == "NoPath":
return None, 'NoPath', self.curr_namo_object_names
else:
fetch_place_path = self.fetch_place_path
new_collisions = self.get_obstacles_in_collision_from_fetching_path(
fetch_pick_path, fetch_place_path)
new_collisions = [
tmp for tmp in new_collisions
if self.problem_env.get_region_containing(tmp) ==
self.curr_namo_region
]
import pdb
pdb.set_trace()
# go back to pre-place
self.problem_env.robot.SetDOFValues(pick_conf)
set_robot_config(action['base_pose'], self.robot)
grab_obj(self.problem_env.robot, namo_obj)
set_robot_config(pick_base_pose, self.robot)
# if new collisions is more than or equal to the current collisions, don't bother executing it
if len(current_collisions) <= len(new_collisions):
return None, "NoPath", self.curr_namo_object_names
# otherwise, update the new namo objects
self.curr_namo_object_names = [obj.GetName() for obj in new_collisions]
# pick motion is the path to the fetching object, place motion is the path to place the namo object
motion = {
'fetch_pick_path': fetch_pick_path,
'fetch_place_path': fetch_place_path,
'place_motion': namo_place_motion
}
# update the self.fetch_place_path if the packing region has moved
self.fetch_pick_path = fetch_pick_path
self.fetch_place_path = fetch_place_path
# todo: change the place entrance configuration too
# update the high level controller task plan
namo_region = self.problem_env.get_region_containing(self.fetching_obj)
self.high_level_controller.set_task_plan([{
'region': namo_region,
'objects': new_collisions
}])
return motion, "HasSolution", self.curr_namo_object_names
def check_two_arm_place_feasibility(self, namo_obj, action,
obj_placement_region):
if action['base_pose'] is None:
return None, "NoPath", self.curr_namo_object_names
motion_planning_region_name = 'entire_region'
goal_robot_xytheta = action['base_pose']
pick_base_pose = get_body_xytheta(self.problem_env.robot)
pick_conf = self.problem_env.robot.GetDOFValues()
namo_status = 'NoPath'
namo_place_motion = None
if self.problem_env.check_base_pose_feasible(
goal_robot_xytheta, namo_obj,
self.problem_env.regions[motion_planning_region_name]):
namo_place_motion, namo_status = self.problem_env.get_base_motion_plan(
goal_robot_xytheta, motion_planning_region_name)
if namo_status == 'NoPath':
return namo_place_motion, namo_status, self.curr_namo_object_names
two_arm_place_object(namo_obj, self.problem_env.robot, action)
fetch_pick_path = self.fetch_pick_path
fetch_place_path = self.fetch_place_path
new_collisions = self.get_obstacles_in_collision_from_fetching_path(
fetch_pick_path, fetch_place_path)
# go back to pre-place
self.problem_env.robot.SetDOFValues(pick_conf)
set_robot_config(action['base_pose'], self.robot)
grab_obj(self.problem_env.robot, namo_obj)
set_robot_config(pick_base_pose, self.robot)
"""
if namo_obj in new_collisions:
print "Object moved still in collision"
return None, "NoPath", self.curr_namo_object_names
"""
# if new collisions is more than or equal to the current collisions, don't bother executing it
"""
if len(self.curr_namo_object_names) <= len(new_collisions):
print "There are more or equal number of collisions on the new path"
print len(self.curr_namo_object_names), len(new_collisions)
print namo_obj, new_collisions
return None, "NoPath", self.curr_namo_object_names
"""
# otherwise, update the new namo objects
self.prev_namo_object_names = self.curr_namo_object_names
self.curr_namo_object_names = [obj.GetName() for obj in new_collisions]
#if len(self.prev_namo_object_names) - len(self.curr_namo_object_names ) > 1:
# import pdb;pdb.set_trace()
# pick motion is the path to the fetching object, place motion is the path to place the namo object
motion = {
'fetch_pick_path': fetch_pick_path,
'fetch_place_path': fetch_place_path,
'place_motion': namo_place_motion
}
# update the self.fetch_place_path if the packing region has moved
self.fetch_pick_path = fetch_pick_path
self.fetch_place_path = fetch_place_path
# update the high level controller task plan
namo_region = self.problem_env.get_region_containing(self.fetching_obj)
self.high_level_controller.set_task_plan([{
'region': namo_region,
'objects': new_collisions
}])
return motion, "HasSolution", self.curr_namo_object_names