def sample_from_continuous_space(self):
assert len(self.robot.GetGrabbed()) == 0
# sample pick
pick_cont_params = None
n_ik_attempts = 0
n_base_attempts = 0
status = "NoSolution"
robot_pose = utils.get_body_xytheta(self.robot)
robot_config = self.robot.GetDOFValues()
# sample pick parameters
pick_cont_params, status = self.sample_pick_cont_parameters()
if status != 'HasSolution':
utils.set_robot_config(robot_pose)
return None, None, status
self.pick_op.continuous_parameters = pick_cont_params
self.pick_op.execute()
# sample place
place_cont_params, status = self.sample_place_cont_parameters(pick_cont_params)
# reverting back to the state before sampling
utils.one_arm_place_object(pick_cont_params)
self.robot.SetDOFValues(robot_config)
utils.set_robot_config(robot_pose)
if status != 'HasSolution':
return None, None, "InfeasibleIK"
else:
self.place_op.continuous_parameters = place_cont_params
return pick_cont_params, place_cont_params, status
def get_pick_poses(self, object, moved_obj, parent_state):
if parent_state is not None and moved_obj != object.GetName():
return parent_state.pick_used[object.GetName()]
operator_skeleton = Operator('two_arm_pick', {'object': object})
generator = UniformGenerator(operator_skeleton, self.problem_env, None)
# This is just generating pick poses. It does not check motion feasibility
self.problem_env.disable_objects_in_region('entire_region')
object.Enable(True)
self.problem_env.set_robot_to_default_dof_values()
n_iters = range(10, 500, 10)
feasible_cont_params = self.sample_feasible_picks(n_iters, generator, operator_skeleton, None)
if len(feasible_cont_params) == 0 and moved_obj == object.GetName():
given_base_pose = utils.get_robot_xytheta()
feasible_cont_params = self.sample_feasible_picks(n_iters, generator, operator_skeleton, given_base_pose)
orig_xytheta = get_body_xytheta(self.problem_env.robot)
self.problem_env.enable_objects_in_region('entire_region')
min_n_collisions = np.inf
chosen_pick_param = None
for cont_param in feasible_cont_params:
n_collisions = self.problem_env.get_n_collisions_with_objects_at_given_base_conf(cont_param['q_goal'])
if min_n_collisions > n_collisions:
chosen_pick_param = cont_param
min_n_collisions = n_collisions
utils.set_robot_config(orig_xytheta)
# assert len(motion_plan_goals) > 0 # if we can't find a pick pose then the object should be treated as unreachable
operator_skeleton.continuous_parameters = chosen_pick_param
operator_skeleton.continuous_parameters['q_goal'] = [chosen_pick_param['q_goal']]
return operator_skeleton
def compute_grasp_config(self,
obj,
pick_base_pose,
grasp_params,
from_place=False):
set_robot_config(pick_base_pose, self.robot)
if not from_place:
# checks the base feasibility
no_collision = not self.env.CheckCollision(self.robot)
if not no_collision:
return None
# TODO: for some reason this is really slow, is it actually necessary?
inside_region = self.problem_env.regions['home_region'].contains(self.robot.ComputeAABB()) or \
self.problem_env.regions['loading_region'].contains(self.robot.ComputeAABB())
if not inside_region:
return None
#stime = time.time()
open_gripper()
grasps = compute_one_arm_grasp(depth_portion=grasp_params[2],
height_portion=grasp_params[1],
theta=grasp_params[0],
obj=obj,
robot=self.robot)
#print 'grasp_computation', time.time()-stime
grasp_config, grasp = solveIKs(self.env, self.robot, grasps)
#print 'Succeed?', grasp_config is not None
#print 'IK computation', time.time()-stime
return grasp_config
def compute_n_in_way_for_object_moved(self, object_moved, abs_state,
goal_objs):
goal_objs_not_in_goal = [
goal_obj for goal_obj in goal_objs if not abs_state.binary_edges[
(goal_obj, 'home_region')][0] and goal_obj != object_moved
] # Object can't be in the path to itself
n_in_way = 0
original_config = utils.get_robot_xytheta(abs_state.problem_env.robot)
for goal_obj in goal_objs_not_in_goal:
Vpre = abs_state.cached_pick_paths[goal_obj]
objects_in_way = [
o.GetName() for o in self.problem_env.get_objs_in_collision(
Vpre, 'entire_region')
]
is_object_moved_in_the_pick_path_to_goal_obj = object_moved in objects_in_way
n_in_way += is_object_moved_in_the_pick_path_to_goal_obj
for goal_obj in goal_objs_not_in_goal:
# Don't I include the pick path collisions?
pick = abs_state.pick_used[goal_obj]
pick.execute()
Vmanip = abs_state.cached_place_paths[(goal_obj, 'home_region')]
objects_in_way = [
o.GetName() for o in self.problem_env.get_objs_in_collision(
Vmanip, 'entire_region')
]
is_object_moved_in_the_place_path_for_goal_o_to_r = object_moved in objects_in_way
n_in_way += is_object_moved_in_the_place_path_for_goal_o_to_r
utils.two_arm_place_object(pick.continuous_parameters)
utils.set_robot_config(original_config)
return n_in_way
def compute_grasp_config(self, obj, pick_base_pose, grasp_params):
set_robot_config(pick_base_pose, self.robot)
were_objects_enabled = [
o.IsEnabled() for o in self.problem_env.objects
]
self.problem_env.disable_objects_in_region('entire_region')
obj.Enable(True)
if self.env.CheckCollision(self.robot):
for enabled, o in zip(were_objects_enabled,
self.problem_env.objects):
if enabled:
o.Enable(True)
else:
o.Enable(False)
return None
grasps = compute_two_arm_grasp(depth_portion=grasp_params[2],
height_portion=grasp_params[1],
theta=grasp_params[0],
obj=obj,
robot=self.robot)
g_config = solveTwoArmIKs(self.env, self.robot, obj, grasps)
for enabled, o in zip(were_objects_enabled, self.problem_env.objects):
if enabled:
o.Enable(True)
else:
o.Enable(False)
return g_config
def update_collisions_at_prm_vertices(self, parent_collides):
# global prm_vertices
# global prm_edges
# if prm_vertices is None or prm_edges is None:
# prm_vertices, prm_edges = pickle.load(open('./prm.pkl', 'rb'))
is_robot_holding = len(self.problem_env.robot.GetGrabbed()) > 0
def in_collision(q, obj):
set_robot_config(q, self.problem_env.robot)
if is_robot_holding:
# note:
# openrave bug: when an object is held, it won't check the held_obj and given object collision unless
# collision on robot is first checked. So, we have to check it twice
col = self.problem_env.env.CheckCollision(
self.problem_env.robot)
col = self.problem_env.env.CheckCollision(
self.problem_env.robot, obj)
else:
col = self.problem_env.env.CheckCollision(
self.problem_env.robot, obj)
return col
obj_name_to_pose = {
obj.GetName(): tuple(get_body_xytheta(obj)[0].round(6))
for obj in self.problem_env.objects
}
collisions_at_all_obj_pose_pairs = {}
old_q = get_body_xytheta(self.problem_env.robot)
for obj in self.problem_env.objects:
obj_name_pose_tuple = (obj.GetName(),
obj_name_to_pose[obj.GetName()])
collisions_with_obj_did_not_change = parent_collides is not None and \
obj_name_pose_tuple in parent_collides
if collisions_with_obj_did_not_change:
collisions_at_all_obj_pose_pairs[
obj_name_pose_tuple] = parent_collides[obj_name_pose_tuple]
else:
prm_vertices_in_collision_with_obj = {
i
for i, q in enumerate(self.prm_vertices)
if in_collision(q, obj)
}
collisions_at_all_obj_pose_pairs[
obj_name_pose_tuple] = prm_vertices_in_collision_with_obj
set_robot_config(old_q, self.problem_env.robot)
# what's the diff between collides and curr collides?
# collides include entire set of (obj, obj_pose) pairs that we have seen so far
# current collides are the collisions at the current object pose
collisions_at_current_obj_pose_pairs = {
obj.GetName():
collisions_at_all_obj_pose_pairs[(obj.GetName(),
obj_name_to_pose[obj.GetName()])]
for obj in self.problem_env.objects
}
return collisions_at_all_obj_pose_pairs, collisions_at_current_obj_pose_pairs
def restore(self, problem_env=None):
if problem_env is None:
problem_env = self.problem_env
for obj_name, obj_pose in self.object_poses.items():
set_obj_xytheta(obj_pose, problem_env.env.GetKinBody(obj_name))
set_robot_config(self.robot_pose, problem_env.robot)
def __init__(self, problem_idx):
Mover.__init__(self, problem_idx)
self.operator_names = ['one_arm_pick', 'one_arm_place']
set_robot_config([4.19855789, 2.3236321, 5.2933337], self.robot)
set_obj_xytheta([3.35744004, 2.19644156, 3.52741118], self.objects[1])
self.boxes = self.objects
self.objects = self.problem_config['shelf_objects']
self.objects = [k for v in self.objects.values() for k in v]
self.objects[0], self.objects[1] = self.objects[1], self.objects[0]
self.target_box = self.env.GetKinBody('rectangular_packing_box1')
utils.randomly_place_region(self.target_box, self.regions['home_region'])
self.regions['rectangular_packing_box1_region'] = self.compute_box_region(self.target_box)
self.shelf_regions = self.problem_config['shelf_regions']
self.target_box_region = self.regions['rectangular_packing_box1_region']
self.regions.update(self.shelf_regions)
self.entity_names = [obj.GetName() for obj in self.objects] + ['rectangular_packing_box1_region',
'center_shelf_region']
self.name = 'one_arm_mover'
self.init_saver = utils.CustomStateSaver(self.env)
self.object_names = self.entity_names
# fix incorrectly named regions
self.regions = {
region.name: region
for region in self.regions.values()
}
def get_uniform_sampler_place_feasibility_rate(pick_smpls, place_smpls,
target_obj, problem_env):
feasibility_checker = two_arm_pap_feasiblity_checker.TwoArmPaPFeasibilityChecker(
problem_env)
op = Operator('two_arm_place', {
"object": target_obj,
"place_region": 'loading_region'
})
n_success = 0
orig_xytheta = utils.get_robot_xytheta(problem_env.robot)
for pick_smpl, place_smpl in zip(pick_smpls, place_smpls):
parameters = np.hstack([pick_smpl, place_smpl])
param, status = feasibility_checker.check_feasibility(
op,
parameters,
swept_volume_to_avoid=None,
parameter_mode='obj_pose')
if status == 'HasSolution':
motion, status = problem_env.motion_planner.get_motion_plan(
[param['pick']['q_goal']], cached_collisions=None)
if status == 'HasSolution':
utils.two_arm_pick_object(target_obj, param['pick'])
motion, status = problem_env.motion_planner.get_motion_plan(
[param['place']['q_goal']], cached_collisions=None)
utils.two_arm_place_object(param['pick'])
utils.set_robot_config(orig_xytheta, problem_env.robot)
n_success += status == 'HasSolution'
total_samples = len(pick_smpls)
return n_success / float(total_samples) * 100
def play_plan(objs, place_motions, environment):
d_fn = base_distance_fn(environment.robot, x_extents=3.9, y_extents=7.1)
s_fn = base_sample_fn(environment.robot, x_extents=4.225, y_extents=5, x=-3.175, y=-3)
e_fn = base_extend_fn(environment.robot)
c_fn = collision_fn(environment.env, environment.robot)
n_iterations = [20, 50, 100, 500, 1000]
pick_motions = []
pick_motions = pickle.load(open('./test_scripts/jobtalk_video_pick_motions.pkl', 'r'))
raw_input('Play plan?')
time.sleep(3)
for obj, pick_motion, place_motion in zip(objs, pick_motions, place_motions):
environment.pick_object(obj)
utils.set_robot_config(environment.init_base_conf)
if obj.GetName() == 'small_obj5':
q_goal = np.array(np.array([[-6.14148808, -5.93437004, 3.64749158]]))
place_action = {'q_goal': q_goal}
else:
place_action = {'q_goal': place_motion[-1][None, :]}
play_motion(place_motion, 0.02)
utils.two_arm_place_object(place_action)
# plan a motion to the top
play_motion(pick_motion, 0.02)
"""
q_init = environment.robot.GetActiveDOFValues()
motion, status = environment.get_motion_plan(q_init, environment.init_base_conf,
d_fn, s_fn, e_fn, c_fn, n_iterations)
if status == 'NoSolution':
obj.Enable(False)
motion, status = environment.get_motion_plan(q_init, environment.init_base_conf,
d_fn, s_fn, e_fn, c_fn, n_iterations)
obj.Enable(True)
pick_motions.append(motion)
"""
import pdb;pdb.set_trace()
def in_collision(q, obj):
#old_q = get_body_xytheta(self.problem_env.robot)
set_robot_config(q, self.problem_env.robot)
col = self.problem_env.env.CheckCollision(self.problem_env.robot,
obj)
#set_robot_config(old_q, self.problem_env.robot)
return col
def apply_two_arm_pick_action_stripstream(self,
action,
obj=None,
do_check_reachability=False):
if obj is None:
obj_to_pick = self.curr_obj
else:
obj_to_pick = obj
pick_base_pose, grasp_params = action
set_robot_config(pick_base_pose, self.robot)
grasps = compute_two_arm_grasp(depth_portion=grasp_params[2],
height_portion=grasp_params[1],
theta=grasp_params[0],
obj=obj_to_pick,
robot=self.robot)
g_config = solveTwoArmIKs(self.env, self.robot, obj_to_pick, grasps)
try:
assert g_config is not None
except:
pass
# import pdb; pdb.set_trace()
action = {'base_pose': pick_base_pose, 'g_config': g_config}
two_arm_pick_object(obj_to_pick, self.robot, action)
curr_state = self.get_state()
reward = 0
pick_path = None
return curr_state, reward, g_config, pick_path
def set_problem_type(self, problem_type):
if problem_type == 'normal':
pass
elif problem_type == 'nonmonotonic':
# from manipulation.primitives.display import set_viewer_options
# self.env.SetViewer('qtcoin')
# set_viewer_options(self.env)
set_color(self.objects[0], [1, 1, 1])
set_color(self.objects[4], [0, 0, 0])
poses = [
[2.3, -6.4, 0],
[3.9, -6.2, 0],
[1.5, -6.3, 0],
[3.9, -5.5, 0],
[0.8, -5.5, 0],
[3.2, -6.2, 0],
[1.5, -5.5, 0],
[3.2, -5.5, 0],
]
q = [2.3, -5.5, 0]
set_robot_config(q)
for obj, pose in zip(self.objects, poses):
set_obj_xytheta(pose, obj)
def get_n_collisions_with_objects_at_given_base_conf(self, conf):
set_robot_config(conf)
n_collisions = 0
for obj_name in self.object_names:
obj = self.env.GetKinBody(obj_name)
n_collisions += self.env.CheckCollision(self.robot, obj)
return n_collisions
def update_collisions_at_prm_vertices(self, parent_state):
global prm_vertices
global prm_edges
if prm_vertices is None or prm_edges is None:
prm_vertices, prm_edges = pickle.load(open('./prm.pkl', 'rb'))
holding = len(self.problem_env.robot.GetGrabbed()) > 0
if holding:
held = self.problem_env.robot.GetGrabbed()[0]
def in_collision(q, obj):
#old_q = get_body_xytheta(self.problem_env.robot)
set_robot_config(q, self.problem_env.robot)
col = self.problem_env.env.CheckCollision(self.problem_env.robot,
obj)
#set_robot_config(old_q, self.problem_env.robot)
return col
obj_name_to_pose = {
obj.GetName(): tuple(get_body_xytheta(obj)[0].round(6))
for obj in self.problem_env.objects
}
# if robot is holding an object, all collision information changes
# but we should still retain previous collision information
# because collisions at the next state will be similar to collisions at the previous state
# todo once we placed the object, instead of setting the collides to be empty,
# we could use the collision information from state-before-pick, because
# the robot shape goes back to whatever it was.
collides = copy.copy(parent_state.collides) if holding else {}
old_q = get_body_xytheta(self.problem_env.robot)
for obj in self.problem_env.objects:
obj_name_pose_tuple = (obj.GetName(),
obj_name_to_pose[obj.GetName()])
collisions_with_obj_did_not_change = parent_state is not None and \
obj_name_pose_tuple in parent_state.collides and \
not holding
if collisions_with_obj_did_not_change:
collides[obj_name_pose_tuple] = parent_state.collides[
obj_name_pose_tuple]
else:
prm_vertices_in_collision_with_obj = {
i
for i, q in enumerate(prm_vertices)
if in_collision(q, obj)
}
collides[
obj_name_pose_tuple] = prm_vertices_in_collision_with_obj
set_robot_config(old_q, self.problem_env.robot)
current_collides = {
obj.GetName():
collides[(obj.GetName(), obj_name_to_pose[obj.GetName()])]
for obj in self.problem_env.objects
}
return collides, current_collides
def is_base_feasible(self, base_pose):
utils.set_robot_config(base_pose, self.robot)
inside_region = self.problem_env.regions['home_region'].contains(self.robot.ComputeAABB()) or \
self.problem_env.regions['loading_region'].contains(self.robot.ComputeAABB())
no_collision = not self.problem_env.env.CheckCollision(self.robot)
if (not inside_region) or (not no_collision):
return False
else:
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 sample_from_continuous_space(self):
# sample pick
pick_cont_params = None
n_ik_attempts = 0
n_base_attempts = 0
status = "NoSolution"
robot_pose = utils.get_body_xytheta(self.robot)
robot_config = self.robot.GetDOFValues()
# sample pick parameters
while pick_cont_params is None:
pick_cont_params, status = self.sample_pick_cont_parameters()
if status == 'InfeasibleBase':
n_base_attempts += 1
elif status == 'InfeasibleIK':
n_ik_attempts += 1
elif status == 'HasSolution':
n_ik_attempts += 1
break
if n_ik_attempts == 1 or n_base_attempts == 4:
break
if status != 'HasSolution':
utils.set_robot_config(robot_pose)
return None, None, status
self.pick_op.continuous_parameters = pick_cont_params
self.pick_op.execute()
# sample place
n_ik_attempts = 0
n_base_attempts = 0
status = "NoSolution"
place_cont_params = None
while place_cont_params is None:
place_cont_params, status = self.sample_place_cont_parameters(pick_cont_params)
if status == 'InfeasibleBase':
n_base_attempts += 1
elif status == 'InfeasibleIK':
n_ik_attempts += 1
elif status == 'HasSolution':
n_ik_attempts += 1
break
if n_ik_attempts == 1 or n_base_attempts == 1:
break
# reverting back to the state before sampling
utils.one_arm_place_object(pick_cont_params)
self.robot.SetDOFValues(robot_config)
utils.set_robot_config(robot_pose)
if status != 'HasSolution':
return None, None, status
else:
self.place_op.continuous_parameters = place_cont_params
return pick_cont_params, place_cont_params, status
def place_object_and_robot_at_new_pose(self, obj, obj_pose, obj_region):
T_r_wrt_o = np.dot(np.linalg.inv(obj.GetTransform()), self.robot.GetTransform())
if len(self.robot.GetGrabbed()) > 0:
release_obj()
set_obj_xytheta(obj_pose, obj)
new_T_robot = np.dot(obj.GetTransform(), T_r_wrt_o)
self.robot.SetTransform(new_T_robot)
new_base_pose = get_body_xytheta(self.robot)
set_robot_config(new_base_pose, self.robot)
fold_arms()
set_point(obj, np.hstack([obj_pose[0:2], obj_region.z + 0.001]))
return new_base_pose
def is_in_region_at_base_pose(self, base_pose, obj, robot_region,
obj_region):
robot = self.robot
if obj is None:
obj_holding = self.curr_obj
else:
obj_holding = obj
with robot:
set_robot_config(base_pose, robot)
in_region = (robot_region.contains(robot.ComputeAABB())) and \
(obj_region.contains(obj_holding.ComputeAABB()))
return in_region
def is_collision_at_base_pose(self, base_pose, obj=None):
robot = self.robot
env = self.env
if obj is None:
obj_holding = self.curr_obj
else:
obj_holding = obj
with robot:
set_robot_config(base_pose, robot)
in_collision = check_collision_except(obj_holding, env)
if in_collision:
return True
return False
def check_collisions(q=None):
if q is not None:
set_robot_config(q, mover.robot)
collision = False
if mover.env.CheckCollision(mover.robot):
collision = True
for obj in mover.objects:
if mover.env.CheckCollision(obj):
collision = True
if collision:
print('collision')
if config.visualize_sim:
raw_input('Continue after collision?')
def get_place_param_with_feasible_motion_plan(self, chosen_pick_param,
candidate_pap_parameters,
cached_holding_collisions):
original_config = utils.get_body_xytheta(
self.problem_env.robot).squeeze()
utils.two_arm_pick_object(
self.operator_skeleton.discrete_parameters['object'],
chosen_pick_param)
place_op_params = [op['place'] for op in candidate_pap_parameters]
chosen_place_param = self.get_op_param_with_feasible_motion_plan(
place_op_params, cached_holding_collisions)
utils.two_arm_place_object(chosen_pick_param)
utils.set_robot_config(original_config)
return chosen_place_param
def play_action(action, t_sleep):
pick = action.continuous_parameters['pick']
place = action.continuous_parameters['place']
for c in pick['motion']:
utils.set_robot_config(c)
time.sleep(t_sleep)
action.execute_pick()
time.sleep(t_sleep)
for c in place['motion']:
utils.set_robot_config(c)
time.sleep(t_sleep)
action.execute()
def in_collision(q, obj):
set_robot_config(q, self.problem_env.robot)
if is_robot_holding:
# note:
# openrave bug: when an object is held, it won't check the held_obj and given object collision unless
# collision on robot is first checked. So, we have to check it twice
col = self.problem_env.env.CheckCollision(
self.problem_env.robot)
col = self.problem_env.env.CheckCollision(
self.problem_env.robot, obj)
else:
col = self.problem_env.env.CheckCollision(
self.problem_env.robot, obj)
return col
def sample_qg(problem_env, region_name):
q0 = utils.get_robot_xytheta()
openrave_env = problem_env.env
place_domain = utils.get_place_domain(
region=problem_env.regions[region_name])
dim_parameters = place_domain.shape[-1]
domain_min = place_domain[0]
domain_max = place_domain[1]
qgs = np.random.uniform(domain_min, domain_max,
(500, dim_parameters)).squeeze()
for qg in qgs:
utils.set_robot_config(qg)
if not openrave_env.CheckCollision(problem_env.robot):
break
utils.set_robot_config(q0)
return qg
def compute_v_manip(abs_state, goal_objs):
goal_objs_not_in_goal = [
goal_obj for goal_obj in goal_objs
if not abs_state.binary_edges[(goal_obj, 'home_region')][0]
]
v_manip = np.zeros((len(abs_state.prm_vertices), 1))
# todo optimize this code
init_end_times = 0
path_times = 0
original_config = utils.get_robot_xytheta(abs_state.problem_env.robot)
stime = time.time()
for goal_obj in goal_objs_not_in_goal:
prm_path = abs_state.cached_place_paths[(goal_obj, 'home_region')]
stime2 = time.time()
# todo possible optimization:
# I can use the prm_path[1]'s edge to compute the neighbors, instead of using all of them.
distances = [
utils.base_pose_distance(prm_path[0], prm_vtx)
for prm_vtx in abs_state.prm_vertices
]
init_end_times += time.time() - stime2
closest_prm_idx = np.argmin(distances)
prm_path.pop(0)
prm_path.insert(0, abs_state.prm_vertices[closest_prm_idx, :])
stime2 = time.time()
distances = [
utils.base_pose_distance(prm_path[-1], prm_vtx)
for prm_vtx in abs_state.prm_vertices
]
init_end_times += time.time() - stime2
closest_prm_idx = np.argmin(distances)
prm_path[-1] = abs_state.prm_vertices[closest_prm_idx, :]
stime2 = time.time()
for p in prm_path:
boolean_matching_prm_vertices = np.all(np.isclose(
abs_state.prm_vertices[:, :2], p[:2]),
axis=-1)
if np.any(boolean_matching_prm_vertices):
idx = np.argmax(boolean_matching_prm_vertices)
v_manip[idx] = 1
path_times += time.time() - stime2
print 'v_manip creation time', time.time() - stime
utils.set_robot_config(original_config, robot=abs_state.problem_env.robot)
return v_manip
def is_collision_in_single_volume(self, vol, obj):
# this is asking if the new object placement will collide with previous swept volumes
self.set_active_dofs_based_on_config_dim(vol[0])
before = self.problem_env.robot.GetActiveDOFValues() # I guess this doesn't matter?
for config in vol:
#set_robot_config(config, self.problem_env.robot)
# todo I need to set it to the right configurations
utils.set_active_config(config, self.robot)
if self.problem_env.env.CheckCollision(self.problem_env.robot):
if self.problem_env.env.CheckCollision(self.problem_env.robot, obj):
# I don't know why, but checking collision with obj directly sometimes
# does not generate proper collision check result; it has to do with whether the robot is holding the
# object when the object is enabled.
utils.set_active_config(before)
return True
utils.set_robot_config(before)
return False
def get_pick_from_initial_config(self, obj):
utils.set_robot_config(self.problem_env.initial_robot_base_pose)
pick_op = Operator(operator_type='two_arm_pick',
discrete_parameters={'object': obj})
potential_motion_plan_goals = self.generate_motion_plan_goals(
pick_op, n_configs=5)
if potential_motion_plan_goals is None:
return None, "NoSolution", None
motion, status = self.get_minimum_constraint_path_to(
potential_motion_plan_goals, obj)
if motion is None:
return None, "NoSolution", None
pick_op.low_level_motion = motion
pick_op.continuous_parameters = {
'q_goal': motion[-1],
'motion': motion
}
return motion, status, pick_op
def check_place_feasible(self, pick_parameters, place_parameters,
operator_skeleton):
pick_op = Operator('two_arm_pick',
operator_skeleton.discrete_parameters)
pick_op.continuous_parameters = pick_parameters
# todo remove the CustomStateSaver
#saver = utils.CustomStateSaver(self.problem_env.env)
original_config = utils.get_body_xytheta(
self.problem_env.robot).squeeze()
pick_op.execute()
place_op = Operator('two_arm_place',
operator_skeleton.discrete_parameters)
place_cont_params, place_status = TwoArmPlaceFeasibilityChecker.check_feasibility(
self, place_op, place_parameters)
utils.two_arm_place_object(pick_op.continuous_parameters)
utils.set_robot_config(original_config)
#saver.Restore()
return place_cont_params, place_status
