def compute_grasp_action(self, obj, region, n_iter=1000):
rightarm_torso_manip = self.robot.GetManipulator('rightarm_torso')
for iter in range(n_iter):
pick_base_pose = None
print 'Sampling IR...'
while pick_base_pose is None:
with self.robot:
pick_base_pose = sample_ir(obj, self.robot, self.env, region, n_iter=1)
print 'Done!'
theta, height_portion, depth_portion = sample_one_arm_grasp_parameters()
grasp_params = np.array([theta[0], height_portion[0], depth_portion[0]])
with self.robot:
set_robot_config(pick_base_pose, self.robot)
grasps = compute_one_arm_grasp(depth_portion=grasp_params[2],
height_portion=grasp_params[1],
theta=grasp_params[0],
obj=obj,
robot=self.robot)
for g in grasps:
g_config = rightarm_torso_manip.FindIKSolution(g, 0)
if g_config is not None:
set_config(self.robot, g_config, self.robot.GetManipulator('rightarm_torso').GetArmIndices())
# Todo
# I might be able to turn to disabling obstacles quickly if the collision, not the base pose,
# is the problem
if not self.env.CheckCollision(self.robot): #check_collision_except(obj, self.env):
pick_params = {'operator_name': 'one_arm_pick', 'base_pose': pick_base_pose, 'grasp_params': grasp_params, 'g_config':g_config}
return pick_params
#one_arm_place_obj(obj, self.robot)
print "Sampling one arm pick failed"
pick_params = {'operator_name': 'one_arm_pick', 'base_pose': None, 'grasp_params': None, 'g_config': None}
return pick_params
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 check_action_feasible(self,action):
robot = self.robot
env = self.env
#curr_obj = self.objects[len(self.placements)] # fixed object order
place_obj_pose = action[0,0:3]
place_robot_pose = action[0,3:]
with robot:
status = ''
set_robot_config( place_robot_pose,robot)
inCollision = (check_collision_except(self.curr_obj,robot,env))\
or (check_collision_except(robot,self.curr_obj,env))
inRegion = (self.all_region.contains(robot.ComputeAABB())) and\
(self.loading_region.contains(self.curr_obj.ComputeAABB()))
#if inCollision:
#if not inRegion: print 'Out of collision'
if (not inCollision) and inRegion:
for node_lim in [1000,5000,np.inf]:
path,tpath,status = get_motion_plan(robot,\
place_robot_pose,env,maxiter=10,n_node_lim=node_lim)
if status=='HasSolution': break
if status == "HasSolution":
return True
else:
return False
def sample_pick_using_gen(obj, obj_shape, robot, generator, env, region):
# diable all bodies; imagine being able to pick anywhere
for body in env.GetBodies():
body.Enable(False)
# enable the target and the robot
obj.Enable(True)
robot.Enable(True)
original_trans = robot.GetTransform()
n_trials = 100 # try 20 different samples
for idx in range(n_trials):
theta, height_portion, depth_portion, base_pose \
= generate_pick_grasp_and_base_pose(generator, obj_shape, get_point(obj))
set_robot_config(base_pose, robot)
grasps = compute_two_arm_grasp(depth_portion, height_portion, theta, obj, robot) # tool trans
g_config = solveTwoArmIKs(env, robot, obj, grasps) # turns obj collision off
if g_config is None:
continue
for body in env.GetBodies():
if body.GetName().find('_pt_') != -1: continue
body.Enable(True)
return base_pose, [theta, height_portion, depth_portion], g_config
return None, None, None
def fcn(obj_name, q_init):
while True:
# todo: disable all of objects
print "Calling gengrasp"
obj = pick_unif.problem_env.env.GetKinBody(obj_name)
pick_unif.problem_env.reset_to_init_state_stripstream()
pick_unif.problem_env.disable_objects_in_region('entire_region')
obj.Enable(True)
action = pick_unif.predict(obj, pick_unif.problem_env.regions['entire_region'])
pick_base_pose = action['base_pose']
grasp = action['grasp_params']
g_config = action['g_config']
if g_config is None:
pick_unif.problem_env.enable_objects_in_region('entire_region')
yield None
else:
set_robot_config(q_init, pick_unif.problem_env.robot)
plan, status = pick_unif.problem_env.get_base_motion_plan(pick_base_pose)
pick_unif.problem_env.enable_objects_in_region('entire_region')
pick_unif.problem_env.reset_to_init_state_stripstream()
if status == 'NoPath':
yield None
else:
print grasp, pick_base_pose, g_config
yield [grasp, pick_base_pose, g_config, plan]
def sample_pick(obj, robot, env, region):
# takes the target object to be picked and returns:
# - base pose to pick the object
# - grasp parameters to pick the object
# - grasp to pick the object
# to which there is a valid path
n_trials = 5000 # try 20 different samples
for _ in range(n_trials):
base_pose = sample_ir(obj, robot, env, region)
if base_pose is None:
continue
set_robot_config(base_pose, robot)
theta, height_portion, depth_portion = sample_grasp_parameters()
grasps = compute_two_arm_grasp(depth_portion, height_portion, theta, obj, robot)
g_config = solveTwoArmIKs(env, robot, obj, grasps)
if g_config is None:
continue
for body in env.GetBodies():
body.Enable(True)
return base_pose, [theta, height_portion, depth_portion], g_config
return None, None, None
def sample_placement_using_body_gen(env, obj, robot, p_samples,
obj_region, robot_region,
GRAB_SLEEP_TIME=0.05):
# This script, with a given grasp of an object,
# - finding colfree obj placement within 100 tries. no robot at this point
# - checking colfree ik solution for robot; if not, trial is over
# - if above two passes, colfree path finding; if not, trial is over
status = "Failed"
path = None
original_trans = robot.GetTransform()
obj_orig_trans = obj.GetTransform()
tried = []
n_trials = 1 # try 5 different samples of placements with a given grasp
T_r_wrt_o = np.dot(np.linalg.inv(obj.GetTransform()), robot.GetTransform())
for _ in range(n_trials):
robot.SetTransform(original_trans)
# sample obj pose
inCollision = True
np.random.shuffle(p_samples)
for idx, xytheta in enumerate(p_samples):
if idx > 100: break
set_robot_config(xytheta, robot)
inCollision = (check_collision_except(obj, robot, env)) \
or (check_collision_except(robot, obj, env))
inRegion = (robot_region.contains(robot.ComputeAABB())) and \
(obj_region.contains(obj.ComputeAABB()))
if (not inCollision) and inRegion:
break
if inCollision or not (inRegion):
print 'obj in collision'
break # if you tried all p samples and ran out, get new pick
print 'robot in a feasible place'
# compute the resulting robot transform
robot_xytheta = xytheta
robot.SetTransform(original_trans)
stime = time.time()
print 'motion planning...'
for node_lim in [1000, 5000, np.inf]:
print node_lim
path, tpath, status = get_motion_plan(robot, \
robot_xytheta, env, maxiter=10, n_node_lim=node_lim)
if path == 'collision':
# import pdb;pdb.set_trace()
pass
print 'done', status, time.time() - stime
if status == "HasSolution":
print 'returning with solution', tpath
set_robot_config(xytheta, robot)
return None, robot_xytheta, path
else:
print 'motion planning failed', tpath
robot.SetTransform(original_trans)
print "Returnining no solution"
return None, None, None
def apply_place_action(self,action):
robot = self.robot
env = self.env
leftarm_manip = robot.GetManipulator('leftarm')
rightarm_manip = robot.GetManipulator('rightarm')
if self.check_action_feasible(action):
place_robot_pose = action[0,3:]
set_robot_config(place_robot_pose,robot)
place_obj( self.curr_obj,robot,FOLDED_LEFT_ARM,leftarm_manip,rightarm_manip)
return True
return False
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 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 sample_placement(env, obj, robot, obj_region, robot_region):
status = "Failed"
path = None
original_trans = robot.GetTransform()
tried = []
n_trials = 1 # try 5 different samples of placements
T_r_wrt_o = np.dot(np.linalg.inv(obj.GetTransform()), robot.GetTransform())
for _ in range(n_trials): # at most n_trials number of path plans
# print 'releasing obj'
while True:
sleep(GRAB_SLEEP_TIME)
robot.Release(obj)
robot.SetTransform(original_trans)
# first sample collision-free object placement
obj_xytheta = randomly_place_in_region(env, obj, obj_region) # randomly place obj
# compute the resulting robot transform
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()
set_robot_config(robot_xytheta, robot)
new_T = robot.GetTransform()
assert (np.all(np.isclose(new_T, new_T_robot)))
if not (check_collision_except(robot, obj, env)) \
and (robot_region.contains(robot.ComputeAABB())):
break
sleep(GRAB_SLEEP_TIME)
grab_obj(robot, obj)
robot.SetTransform(original_trans)
# print 'motion planning...'
stime = time.time()
for node_lim in [1000, 5000, np.inf]:
print node_lim
path, tpath, status = get_motion_plan(robot, robot_xytheta, env, maxiter=10, n_node_lim=node_lim)
print 'done', status, time.time() - stime
if status == 'HasSolution': break
print 'done', status, time.time() - stime
if status == "HasSolution":
# print 'returning with solution'
return obj_xytheta, robot_xytheta, path
# print 'grabbing obj'
sleep(GRAB_SLEEP_TIME)
robot.Grab(obj)
# print 'grabbed'
return None, None, None
def apply_two_arm_pick_action(self, action, 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,
robot=self.robot)
g_config = solveTwoArmIKs(self.env, self.robot, obj, grasps)
action = {}
action['g_config'] = g_config
action['base_pose'] = pick_base_pose
two_arm_pick_object(obj, self.robot, action)
def fcn(obstacle_name, obstacle_pose, q_init, q_goal, traj):
problem.reset_to_init_state_stripstream()
obstacle = problem.env.GetKinBody(obstacle_name)
set_obj_xytheta(obstacle_pose, obstacle)
# check collision
for p in traj:
set_robot_config(p, problem.robot)
if problem.env.CheckCollision(problem.robot):
problem.reset_to_init_state_stripstream()
return True
problem.reset_to_init_state_stripstream()
return False
def sample_ir_multiple_regions(obj, robot, env, multiple_regions):
arm_len = 0.9844 # determined by spreading out the arm and measuring the dist from shoulder to ee
# grasp_pos = Tgrasp[0:-1,3]
obj_xy = get_point(obj)[:-1]
robot_xy = get_point(robot)[:-1]
dist_to_grasp = np.linalg.norm(robot_xy - obj_xy)
n_samples = 1
for _ in range(300):
robot_xy = sample_base_locations(arm_len, obj, env)[:-1]
angle = sample_angle_facing_given_transform(obj_xy, robot_xy) # angle around z
set_robot_config(np.r_[robot_xy, angle], robot)
if (not env.CheckCollision(robot)) and np.any([r.contains(robot.ComputeAABB()) for r in multiple_regions]):
return np.array([robot_xy[0], robot_xy[1], angle])
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 sample_ir(obj, robot, env, region, n_iter=300):
arm_len = PR2_ARM_LENGTH # determined by spreading out the arm and measuring the dist from shoulder to ee
# grasp_pos = Tgrasp[0:-1,3]
obj_xy = get_point(obj)[:-1]
robot_xy = get_point(robot)[:-1]
dist_to_grasp = np.linalg.norm(robot_xy - obj_xy)
n_samples = 1
for _ in range(n_iter):
robot_xy = sample_xy_locations(obj, arm_len)[:-1]
angle = sample_angle_facing_given_transform(obj_xy, robot_xy) # angle around z
set_robot_config(np.r_[robot_xy, angle], robot)
if (not env.CheckCollision(robot)) and (region.contains(robot.ComputeAABB())):
return np.array([robot_xy[0], robot_xy[1], angle])
return None
def get_new_fetching_pick_path(self, namo_obj,
motion_planning_region_name):
# todo plan to the previous namo_obj pick configuration
motion_to_namo_pick, status1 = self.problem_env.get_base_motion_plan(
self.current_pick_conf, motion_planning_region_name)
if status1 == 'NoPath':
return None, 'NoPath'
set_robot_config(self.current_pick_conf, self.robot)
motion_from_namo_pick_to_fetch_pick, status2 = self.get_motion_plan_with_disabling(
self.fetch_pick_conf, motion_planning_region_name, False, namo_obj)
if status2 == 'NoPath':
return None, 'NoPath'
motion = motion_to_namo_pick + motion_from_namo_pick_to_fetch_pick
status = "HasSolution"
return motion, status
def process_plan(plan, namo):
namo.env.SetViewer('qtcoin')
namo.reset_to_init_state_stripstream()
for step_idx, step in enumerate(plan):
# todo finish this visualization script
import pdb;pdb.set_trace()
print "Executing operator ", step[0]
if step[0] == 'pickup':
obj_name = step[1][0]
grasp = step[1][1]
pick_base_pose = step[1][2]
g_config = step[1][3]
namo.apply_two_arm_pick_action_stripstream((pick_base_pose, grasp, g_config), namo.env.GetKinBody(obj_name))
elif step[0] == 'movebase':
q_init = step[1][0]
q_goal = step[1][1]
path = step[1][2]
visualize_path(namo.robot, path)
set_robot_config(q_goal, namo.robot)
elif step[0] == 'movebase_with_object':
q_init = step[1][4]
q_goal = step[1][5]
path = step[1][6]
set_robot_config(q_init, namo.robot)
visualize_path(namo.robot, path)
set_robot_config(q_goal, namo.robot)
else:
place_obj_name = step[1][0]
place_base_pose = step[1][4]
path = step[1][-1]
visualize_path(namo.robot, path)
action = {'base_pose': place_base_pose}
obj = namo.env.GetKinBody(place_obj_name)
two_arm_place_object(obj, namo.robot, action)
def apply_pick_action(self):
robot = self.robot
env = self.env
leftarm_manip = robot.GetManipulator('leftarm')
rightarm_torso_manip = robot.GetManipulator('rightarm_torso')
while True:
pick_base_pose,grasp_params,g_config = sample_pick(self.curr_obj,robot,env,self.all_region)
if pick_base_pose is None:
continue
set_robot_config( pick_base_pose,robot)
pick_obj(self.curr_obj,robot,g_config,leftarm_manip,rightarm_torso_manip )
set_robot_config(self.init_base_conf,robot)
pick = {}
pick['pick_base_pose'] = pick_base_pose
pick['grasp_params'] = grasp_params
pick['g_config'] = g_config
pick['obj'] = self.curr_obj.GetName()
return pick
def fcn(holding_obj_name, grasp, pick_base_conf, g_config, placed_obj_name, placed_obj_pose, q_goal, holding_obj_path):
holding_obj = problem.env.GetKinBody(holding_obj_name)
placed_obj = problem.env.GetKinBody(placed_obj_name)
problem.apply_two_arm_pick_action_stripstream((pick_base_conf, grasp, g_config), holding_obj) # how do I ensure that we are in the same state in both openrave and stripstream?
if np.all(pick_base_conf == q_goal):
return False
if holding_obj_name != placed_obj_name:
# set the obstacle in place
set_obj_xytheta(placed_obj_pose, placed_obj)
else:
return False # this is already checked
return False
# check collision
for p in holding_obj_path:
set_robot_config(p, problem.robot)
if problem.env.CheckCollision(problem.robot):
problem.reset_to_init_state_stripstream()
return True
problem.reset_to_init_state_stripstream()
return False
def fcn(holding_obj_name, grasp, pick_base_conf, placed_obj_name, placed_obj_pose, holding_obj_place_base_pose, holding_obj_place_traj):
holding_obj = problem.env.GetKinBody(holding_obj_name)
placed_obj = problem.env.GetKinBody(placed_obj_name)
problem.apply_two_arm_pick_action_stripstream((pick_base_conf, grasp), holding_obj)
if holding_obj_name != placed_obj_name:
# set the obstacle in place
set_obj_xytheta(placed_obj_pose, placed_obj)
else:
return False # this is already checked
if len(problem.robot.GetGrabbed()) == 0:
import pdb;pdb.set_trace()
# check collision
for p in holding_obj_place_traj:
set_robot_config(p, problem.robot)
if problem.env.CheckCollision(problem.robot):
problem.reset_to_init_state_stripstream()
return True
problem.reset_to_init_state_stripstream()
return False
def fcn(obj_name, grasp, pick_base_pose, g_config, region_name):
# simulate pick
while True:
problem.reset_to_init_state_stripstream()
obj = problem.env.GetKinBody(obj_name)
problem.apply_two_arm_pick_action_stripstream((pick_base_pose, grasp, g_config), obj) # how do I ensure that we are in the same state in both openrave and stripstream?
print region_name
problem.disable_objects_in_region('entire_region')
obj.Enable(True)
place_action = place_unif.predict(obj, problem.regions[region_name])
place_base_pose = place_action['base_pose']
object_pose = place_action['object_pose'].squeeze()
path, status = problem.get_base_motion_plan(place_base_pose.squeeze())
problem.enable_objects_in_region('entire_region')
print "Input", obj_name, grasp, pick_base_pose
set_robot_config(place_base_pose, problem.robot)
problem.reset_to_init_state_stripstream()
if status == 'HasSolution':
yield (place_base_pose, object_pose, path)
else:
yield None
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
def main():
env=Environment()
env.Load('env.xml')
robot = env.GetRobots()[0]
#set_default_robot_config(robot)
robot_initial_pose = get_pose(robot)
leftarm_manip = robot.GetManipulator('leftarm')
robot.SetActiveManipulator('leftarm')
ikmodel1 = databases.inversekinematics.InverseKinematicsModel(robot=robot, \
iktype=IkParameterization.Type.Transform6D, \
forceikfast=True, freeindices=None, \
freejoints=None, manip=None)
if not ikmodel1.load():
ikmodel1.autogenerate()
rightarm_manip = robot.GetManipulator('rightarm')
rightarm_torso_manip = robot.GetManipulator('rightarm_torso')
robot.SetActiveManipulator('rightarm_torso')
manip = robot.GetActiveManipulator()
ee = manip.GetEndEffector()
ikmodel2 = databases.inversekinematics.InverseKinematicsModel(robot=robot, \
iktype=IkParameterization.Type.Transform6D, \
forceikfast=True, freeindices=None, \
freejoints=None, manip=None)
if not ikmodel2.load():
ikmodel2.autogenerate()
set_config(robot,FOLDED_LEFT_ARM,robot.GetManipulator('leftarm').GetArmIndices())
set_config(robot,mirror_arm_config(FOLDED_LEFT_ARM),\
robot.GetManipulator('rightarm').GetArmIndices())
robot_initial_config = np.array([-1,1,0])
set_robot_config(robot_initial_config,robot)
env.SetViewer('qtcoin')
target = env.GetKinBody('target')
width = 0.1 #np.random.rand(1)*(max_width-min_width)+min_width
length = 0.8 #np.random.rand(1)*(max_length-min_length) + min_length
height = 1.0 #np.random.rand(1)*(max_height-min_height)+min_height
new_body = box_body(env,width,length,height,\
name='obst',\
color=(0, 5, 1))
trans = np.eye(4);
trans[2,-1] = 0.075
#env.Add(new_body);
#new_body.SetTransform(trans)
all_region = region = AARegion('all_region',((-2.51,2.51),(-2.51,2.51)),\
z=0.0001,color=np.array((1,1,0,0.25)))
activate_arms_torso_base(robot)
# Try picking different objects
pick_obj = lambda obj_name: pick(obj_name,env,robot,all_region)
import pdb;pdb.set_trace()
pbase,grasp,g_config = sample_pick(env.GetRobot('valkyrie'),robot,env,all_region)
pick_obj(env.GetRobot('valkyrie'),robot,g_config,leftarm_manip,rightarm_manip)
import pdb;pdb.set_trace()
path,tpath,status = get_motion_plan(robot,robot_xytheta,env,maxiter=10,n_node_lim=5000)
import pdb;pdb.set_trace()
sample_pick(target,robot,env,all_region)
import pdb;pdb.set_trace()
def compute_constraint_removal_motion(self, place_config,
place_region_name):
self.place_motion, status = self.problem_env.get_motion_plan(
place_config, place_region_name)
assert self.robot.GetGrabbed(
) != 0, 'CRP must be solved once the target obj is held'
self.place_region = place_region_name
self.place_config = place_config
self.pick_full_conf = self.robot.GetDOFValues()
if status == 'NoPath':
self.problem_env.disable_objects_in_region(self.pick_region_name)
held_obj = self.robot.GetGrabbed()[0]
held_obj.Enable(True)
self.place_motion, _ = self.problem_env.get_motion_plan(
place_config, 'entire_region')
self.problem_env.enable_objects_in_region(self.pick_region_name)
# this might plan a path that does not collide with any
else:
return self.place_motion, 'HasSimpleSolution'
if self.place_motion is None:
set_robot_config(self.prepick_config, self.robot)
self.reset_to_prepick()
return None, "NoPath"
objs_in_collision = self.make_constraint_removal_plan()
pick_region = self.problem_env.regions[self.pick_region_name]
task_plan = [{'region': pick_region, 'objects': objs_in_collision}]
if len(objs_in_collision) == 0:
return self.place_motion, 'HasSimpleSolution'
uct_parameter = 0
widening_parameter = 0.5
# todo set the environment to the configuration before the pick node
# set the robot to its init_base_pose
self.problem_env.is_solving_namo = True
self.problem_env.init_namo_object_names = [
o.GetName() for o in objs_in_collision
]
self.problem_env.curr_namo_object_names = [
o.GetName() for o in objs_in_collision
]
self.problem_env.namo_pick_path = self.pick_motion
self.problem_env.namo_place_path = self.place_motion
self.problem_env.namo_region = self.pick_region_name
# resetting the robot to its pre-pick configuration
try:
self.problem_env.place_object(self.pick_config)
except:
import pdb
pdb.set_trace()
set_robot_config(self.prepick_config, self.robot)
self.reset_to_prepick()
two_arm_pick_pi = PickWithBaseUnif(self.problem_env.env, self.robot)
two_arm_place_pi = PlaceUnif(self.problem_env.env, self.robot)
sampling_strategy = Uniform(self.problem_env, two_arm_pick_pi,
two_arm_place_pi)
mcts = MCTS(widening_parameter, uct_parameter, sampling_strategy,
self.problem_env, 'mover', task_plan)
#if self.namo_target_obj.GetName() == 'rectangular_packing_box2':
# import pdb;pdb.set_trace()
search_time_to_reward, plan, optimal_score_achieved = mcts.search(
n_iter=20)
#if self.namo_target_obj.GetName() == 'rectangular_packing_box2':
# import pdb;pdb.set_trace()
self.problem_env.is_solving_namo = False
self.problem_env.namo_object_names = []
self.problem_env.namo_pick_path = None
self.problem_env.namo_place_path = None
self.problem_env.namo_region = None
# in either case, set the environment to the configuration just after the pick
if plan is None:
return None, 'NoPath'
else:
try:
# todo how come it didn't add path_to_pick_target_obj and path_to_last_place?
plan = self.add_picking_target_obj_to_plan(plan)
plan = self.add_placing_target_obj_to_plan(plan)
except:
import pdb
pdb.set_trace()
return plan, 'HasSolution'
def pick_namo_target_obj(self):
set_robot_config(self.pick_config, self.robot)
self.robot.SetDOFValues(self.pick_full_conf)
grab_obj(self.robot, self.namo_target_obj)
def get_new_fetching_place_path(self, namo_obj,
motion_planning_region_name):
# pick the object
is_fetch_pick_one_arm = self.fetch_pick_op_instance[
'operator'] == 'one_arm_pick'
if is_fetch_pick_one_arm:
one_arm_pick_object(self.fetching_obj, self.problem_env.robot,
self.fetch_pick_op_instance['action'])
else:
two_arm_pick_object(self.fetching_obj, self.problem_env.robot,
self.fetch_pick_op_instance['action'])
# get pre_exit_motion -- motion prev to the exit
pre_exit_motion = []
for pidx, p in enumerate(self.fetch_place_path):
pre_exit_motion.append(p)
if np.all(p == self.fetch_pick_path_exit):
break
with self.robot:
# place it first
one_arm_place_object(self.fetching_obj, self.robot,
self.fetch_place_op_instance['action'])
place_conf_list = self.get_rotations_around_z_wrt_gripper(
self.fetching_obj, self.fetch_place_conf)
one_arm_pick_object(self.fetching_obj, self.robot,
self.fetch_place_op_instance['action'])
# put it back to where it was
one_arm_place_object(self.fetching_obj, self.robot,
self.fetch_pick_op_instance['action'])
one_arm_pick_object(self.fetching_obj, self.robot,
self.fetch_pick_op_instance['action'])
place_conf_list = [
conf for conf in place_conf_list
if self.problem_env.check_base_pose_feasible(
conf[-3:], self.fetching_obj,
self.problem_env.regions[motion_planning_region_name])
]
if is_fetch_pick_one_arm:
self.problem_env.set_arm_base_config(self.fetch_pick_path_exit)
else:
set_robot_config(self.fetch_pick_path_exit)
if len(place_conf_list) == 0:
print "Moved packin region, no feasible base pose"
return None, "NoPath"
for conf in place_conf_list:
# prevent moving the namo_obj = self.packing_region_obj in this case, because you just moved it
post_exit_motion, status = self.get_motion_plan_with_disabling(
conf,
motion_planning_region_name,
is_fetch_pick_one_arm,
exception_obj=namo_obj)
if status == 'HasSolution':
# update the fetch place config
self.fetch_place_conf = conf
self.fetch_place_op_instance['action']['base_pose'] = conf[
-3:]
break
if is_fetch_pick_one_arm:
one_arm_place_object(self.fetching_obj, self.problem_env.robot,
self.fetch_pick_op_instance['action'])
else:
two_arm_place_object(self.fetching_obj, self.problem_env.robot,
self.fetch_pick_op_instance['action'])
if status == "NoPath":
return None, "NoPath"
else:
place_motion = pre_exit_motion + post_exit_motion
return place_motion, status
def simulate_base_path(robot, path):
for p in path:
# set_config(robot, p, get_active_arm_indices(robot))
set_robot_config(p, robot)
sleep(0.001)
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
