def main():
plans, pidxs = load_plan()
plan = plans[2]
problem_env = make_environment(59)
utils.set_body_transparency('top_wall_1', 0.5)
#utils.set_robot_config(np.array([[ 3.6153236 , 0.93829982, 5.63509206]]))
utils.viewer()
[utils.set_color(obj, [0.0, 0.0, 0.7]) for obj in problem_env.objects]
utils.set_color('c_obst1', [1.0, 1.0, 0])
viewer = problem_env.env.GetViewer()
cam_transform = np.array(
[[0.58774001, -0.61021391, 0.53122562, 0.93478185],
[-0.80888257, -0.45655478, 0.37049525, -1.98781455],
[0.01645225, -0.64745402, -0.76192691, 4.26729631], [0., 0., 0., 1.]])
viewer.SetCamera(cam_transform)
robot = problem_env.robot
utils.open_gripper(robot)
manip = robot.GetManipulator('rightarm_torso')
robot.SetActiveDOFs(manip.GetArmIndices(),
DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis,
[0, 0, 1])
motion_plan_file = './test_scripts/jobtalk_figures/one_arm_domain_motion_plans_for_job_talk.pkl'
mp_plans = get_motion_plans(plan, problem_env, motion_plan_file)
import pdb
pdb.set_trace()
play_plan(plan, mp_plans, robot)
import pdb
pdb.set_trace()
problem_env.init_saver.Restore()
def get_motion_plans(plan, problem_env, fname):
if os.path.isfile(fname):
mp_plans = pickle.load(open(fname, 'r'))
return mp_plans
robot = problem_env.robot
utils.open_gripper(robot)
manip = robot.GetManipulator('rightarm_torso')
robot.SetActiveDOFs(manip.GetArmIndices(),
DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis,
[0, 0, 1])
mp = ArmBaseMotionPlanner(problem_env, 'rrt')
mp_plans = []
for action in plan:
pick_goal = action.continuous_parameters['pick']['q_goal']
mp_plan, status = mp.get_motion_plan(pick_goal,
region_name='home_region')
if status == 'HasSolution':
mp_plans.append(mp_plan)
action.execute_pick()
else:
import pdb
pdb.set_trace()
place_goal = action.continuous_parameters['place']['q_goal']
mp_plan, status = mp.get_motion_plan(place_goal,
region_name='home_region')
if status == 'HasSolution':
mp_plans.append(mp_plan)
action.execute()
else:
import pdb
pdb.set_trace()
pickle.dump(mp_plans, open(fname, 'wb'))
return mp_plans
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_and_cache_ik_solutions(self, ikcachename):
before = CustomStateSaver(self.problem_env.env)
utils.open_gripper()
# for o in self.problem_env.env.GetBodies()[2:]:
# o.Enable(False)
self.problem_env.disable_objects()
self.iksolutions = {}
o = u'c_obst0'
obj = self.problem_env.env.GetKinBody(o)
if True:
# for o, obj in self.objects.items():
# print(o)
self.iksolutions[o] = {r: [] for r in self.regions}
pick_op = Operator(operator_type='one_arm_pick', discrete_parameters={'object': obj})
pick_generator = UniformGenerator(pick_op, self.problem_env)
pick_feasibility_checker = OneArmPickFeasibilityChecker(self.problem_env)
for _ in range(10000):
pick_params = pick_generator.sample_from_uniform()
iks = []
for r, region in self.regions.items():
place_op = Operator(operator_type='one_arm_place',
discrete_parameters={
'object': obj,
'region': region})
place_generator = UniformGenerator(place_op, self.problem_env)
status = 'NoSolution'
for _ in range(10):
obj_pose = place_generator.sample_from_uniform()
set_obj_xytheta(obj_pose, obj)
set_point(obj, np.hstack([obj_pose[0:2], region.z + 0.001]))
params, status = pick_feasibility_checker.check_feasibility(pick_op, pick_params)
if status == 'HasSolution':
iks.append((obj.GetTransform(), params))
break
if status == 'NoSolution':
break
if len(iks) == len(self.regions):
for r, ik in zip(self.regions, iks):
self.iksolutions[o][r].append(ik)
if all(len(self.iksolutions[o][r]) >= 1000 for r in self.regions):
break
# print([len(self.iksolutions[o][r]) for r in self.regions])
self.iksolutions = self.iksolutions[o]
# for o in self.problem_env.env.GetBodies()[2:]:
# o.Enable(True)
self.problem_env.enable_objects()
before.Restore()
pickle.dump(self.iksolutions, open(ikcachename, 'w'))
def sample_pick_cont_parameters(self):
op_parameters = self.pick_generator.sample_from_uniform()
grasp_params, pick_base_pose = get_pick_base_pose_and_grasp_from_pick_parameters(self.target_obj, op_parameters)
if not self.is_base_feasible(pick_base_pose):
return None, 'InfeasibleBase'
utils.open_gripper()
grasps = grasp_utils.compute_one_arm_grasp(depth_portion=grasp_params[2],
height_portion=grasp_params[1],
theta=grasp_params[0],
obj=self.target_obj,
robot=self.robot)
grasp_config, grasp = grasp_utils.solveIKs(self.problem_env.env, self.robot, grasps)
param = {'q_goal': np.hstack([grasp_config, pick_base_pose]),
'grasp_params': grasp_params,
'g_config': grasp_config,
'action_parameters': op_parameters}
if grasp_config is None:
return None, 'InfeasibleIK'
else:
return param, 'HasSolution'