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_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 is_grasp_config_feasible(self, obj, pick_base_pose, grasp_params,
grasp_config):
pick_action = {
'operator_name': 'two_arm_pick',
'q_goal': pick_base_pose,
'grasp_params': grasp_params,
'g_config': grasp_config
}
orig_config = get_robot_xytheta(self.robot)
two_arm_pick_object(obj, pick_action)
no_collision_at_pick_config = not self.env.CheckCollision(self.robot)
# Changing this to loading, home, and bridge regions will hurt the performance for uniform sampler,
# and I will have to re-run experiments. Our planning experience might involve base poses outside of
# the loading or kitchen regions too. I will leave it as is for now.
#inside_region = self.problem_env.regions['entire_region'].contains(self.robot.ComputeAABB())
inside_region = \
self.problem_env.regions['loading_region'].contains(self.robot.ComputeAABB()) or \
self.problem_env.regions['home_region'].contains(self.robot.ComputeAABB()) or \
self.problem_env.regions['bridge_region'].contains(self.robot.ComputeAABB())
two_arm_place_object(pick_action)
no_collision_with_arms_folded = not self.env.CheckCollision(self.robot)
set_robot_config(orig_config, self.robot)
no_collision = no_collision_at_pick_config and no_collision_with_arms_folded
return no_collision and inside_region
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_grasp_config(self, obj, pick_base_pose, grasp_params):
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)
return g_config
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 sample_grasp(target_obj):
g_config = None
i = 0
while g_config is None:
print i
pick_parameter = np.random.uniform(pick_domain[0], pick_domain[1], 6)
grasp_params, pick_base_pose = get_pick_base_pose_and_grasp_from_pick_parameters(
target_obj, pick_parameter)
set_robot_config(pick_base_pose, robot)
i += 1
if env.CheckCollision(robot):
continue
grasps = compute_two_arm_grasp(grasp_params[2], grasp_params[1],
grasp_params[0], target_obj, robot)
g_config = solveTwoArmIKs(env, robot, target_obj, grasps)
print g_config, grasp_params
return g_config, pick_base_pose, grasp_params
def main():
random_seed = 0
np.random.seed(random_seed)
random.seed(random_seed)
problem_idx = 3
problem_instantiator = ConveyorBeltInstantiator(problem_idx, 'convbelt',
1000)
environment = problem_instantiator.environment
generator = UniformGenerator('two_arm_pick', environment)
node = FakeNode(environment)
action = generator.sample_next_point(node, 100)
while action['is_feasible'] is not True:
action = generator.sample_next_point(node, 100)
init_goal_cam_transform = \
np.array([[-0.99977334, 0.00150049, 0.02123698, -1.10846174],
[-0.01359773, 0.72254398, -0.69119122, 7.63502693],
[-0.01638178, -0.69132333, -0.72235981, 10.50135326],
[0., 0., 0., 1.]])
environment.env.SetViewer('qtcoin')
viewer = environment.env.GetViewer()
viewer.SetCamera(init_goal_cam_transform)
init_poses = pickle.load(open('./aaai_visualization_init_poses.pkl', 'r'))
[
utils.set_obj_xytheta(pose, obj)
for obj, pose in zip(environment.objects, init_poses)
]
import pdb
pdb.set_trace()
goal_poses = pickle.load(open('./aaai_visualization_goal_poses.pkl', 'r'))
[
utils.set_obj_xytheta(pose, obj)
for obj, pose in zip(environment.objects, goal_poses)
]
import pdb
pdb.set_trace()
utils.two_arm_pick_object(environment.objects[-1], environment.robot,
action)
utils.set_robot_config([-1.74301404, 0.11800224, np.pi], environment.robot)
environment.env.Remove(environment.objects[0])
import pdb
pdb.set_trace()
def check_place_feasible(self, pick_parameters, place_parameters,
operator_skeleton, parameter_mode):
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, parameter_mode=parameter_mode)
utils.two_arm_place_object(pick_op.continuous_parameters)
utils.set_robot_config(original_config)
# saver.Restore()
return place_cont_params, place_status
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 compute_grasp_config(self, obj, pick_base_pose, grasp_params):
orig_config = get_robot_xytheta(self.robot)
set_robot_config(pick_base_pose, self.robot)
were_objects_enabled = [
o.IsEnabled() for o in self.problem_env.objects
] # for RSC
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)
set_robot_config(orig_config, self.robot)
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)
set_robot_config(orig_config, self.robot)
#if g_config is None:
# print "No IK solution exists"
return g_config
def compute_g_config(self, obj, pick_base_pose, grasp_params):
original_config = get_body_xytheta(self.robot)
#if True:
with self.robot:
g_config = self.compute_grasp_config(obj, pick_base_pose,
grasp_params)
if g_config is not None:
pick_action = {
'operator_name': 'two_arm_pick',
'base_pose': pick_base_pose,
'grasp_params': grasp_params,
'g_config': g_config
}
two_arm_pick_object(obj, self.robot, pick_action)
if self.problem_env.name == 'convbelt':
feasible = True
else:
inside_region = self.problem_env.regions[
'entire_region'].contains(self.robot.ComputeAABB())
pick_config_not_in_collision = not self.env.CheckCollision(
self.robot)
feasible = inside_region and pick_config_not_in_collision
not_in_collision = not self.env.CheckCollision(self.robot)
feasible = inside_region and not_in_collision
if feasible:
two_arm_place_object(obj, self.robot, pick_action)
set_robot_config(original_config, self.robot)
return g_config
else:
two_arm_place_object(obj, self.robot, pick_action)
set_robot_config(original_config, self.robot)
else:
#if obj.GetName().find("1") != -1:
# import pdb;pdb.set_trace()
set_robot_config(original_config, self.robot)
return None
def check_reachability_precondition(self, operator_instance):
if self.problem_idx == 0:
if operator_instance.type == 'two_arm_place':
held = self.robot.GetGrabbed()[0]
prev_config = get_body_xytheta(self.robot)
set_robot_config(operator_instance.continuous_parameters['base_pose'], self.robot)
if self.regions['forbidden_region'].contains(held.ComputeAABB()) \
or not(self.regions['home_region'].contains(held.ComputeAABB())):
set_robot_config(prev_config, self.robot)
return None, "NoSolution"
set_robot_config(prev_config, self.robot)
motion_planning_region_name = 'home_region'
goal_robot_xytheta = operator_instance.continuous_parameters['base_pose']
if operator_instance.low_level_motion is not None:
motion = operator_instance.low_level_motion
status = 'HasSolution'
return motion, status
motion, status = self.get_base_motion_plan(goal_robot_xytheta, motion_planning_region_name)
return motion, status
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'
grasp_params, pick_base_pose = get_pick_base_pose_and_grasp_from_pick_parameters(
target_obj, pick_parameter)
set_robot_config(pick_base_pose, robot)
i += 1
if env.CheckCollision(robot):
continue
grasps = compute_two_arm_grasp(grasp_params[2], grasp_params[1],
grasp_params[0], target_obj, robot)
g_config = solveTwoArmIKs(env, robot, target_obj, grasps)
print g_config, grasp_params
return g_config, pick_base_pose, grasp_params
target = env.GetKinBody('matrice')
target = tobj
g_config, pick_base_pose, grasp_params = sample_grasp(target)
import pdb
pdb.set_trace()
pick_action = {
'operator_name': 'two_arm_pick',
'base_pose': pick_base_pose,
'grasp_params': grasp_params,
'g_config': g_config
}
two_arm_pick_object(target, robot, pick_action)
init_base_conf = np.array([0, 1.05, 0])
set_robot_config(init_base_conf, robot)
goal_config = np.array([-2, -2, np.pi / 2])
place_path, status = problem.get_base_motion_plan(goal_config)
