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 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 apply_two_arm_pick_action_stripstream(self,
action,
obj=None,
do_check_reachability=False):
leftarm_manip = self.robot.GetManipulator('leftarm')
rightarm_torso_manip = self.robot.GetManipulator('rightarm_torso')
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:
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)
set_robot_config(self.init_base_conf, self.robot)
curr_state = self.get_state()
reward = 0
pick_path = None
return curr_state, reward, g_config, pick_path
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 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