def sample_ir(Tgrasp,robot,env,oracle):
r_ee_pos = robot.GetLink('r_gripper_l_finger_tip_frame').GetTransform()[0:-1,3]
r_shoulder_pos = robot.GetLink('r_shoulder_pan_link').GetTransform()[0:-1,3]
arm_len = np.linalg.norm(r_ee_pos-r_shoulder_pos)
grasp_pos = Tgrasp[0:-1,3]
robot_pos = robot.GetTransform()[0:-1,3]
dist_to_grasp = np.linalg.norm(robot_pos-grasp_pos)
bounds = get_env_bounds(env)
target_xy = Tgrasp[0:2,3]
dxdy = sample_base_locations(arm_len,target_xy,env)
robot_config = target_xy+dxdy
angle = sample_angle_facing_given_transform(robot,Tgrasp,robot_config)
set_robot_config(np.r_[target_xy+dxdy,angle],robot)
start_time = time.time()
while checkcollision(robot,env) or (not check_within_bounds(bounds,robot_config)):
dxdy = sample_base_locations(arm_len,target_xy,env)
robot_config= target_xy+dxdy
angle = sample_angle_facing_given_transform(robot,Tgrasp,robot_config)
set_robot_config(np.r_[robot_config,angle],robot)
time_elapsed = time.time()-start_time
if time_elapsed > 30:
return None
set_robot_config(np.r_[robot_config,angle],robot)
if (inverse_kinematics(oracle,Tgrasp) is None):
return None
else:
return np.r_[dxdy+target_xy,angle]
def get_true_vals_from_base_pose_and_grasp(self,grasp,base_pose):
# set robot base location
self.robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1])
self.robot.SetActiveDOFValues(base_pose)
Tgrasp = self.gmodel.getGlobalGraspTransform(grasp,collisionfree=False)
self.env.GetKinBody('ObjToG').Enable(False)
if not self.check_IR(Tgrasp):
# print 'ir failed!!'
return 0,0,0
s_time = time.time()
goal_config = inverse_kinematics(self.oracle,Tgrasp)
ik_time = time.time() - s_time
if goal_config is None:
# print 'ik failed!!'
return 0,ik_time,0
# self.set_to_given_config(goal_config)
s_time = time.time()
traj,plantime,planstatus = self.trajOptGetMotionPlan(goal_config)
plan_time = time.time() - s_time
if planstatus == "Failed":
# print 'planning failed'
return 0,ik_time,plan_time
traj_reward,traj_len,min_dists = self.get_path_rwds_manual_min_dist(traj)
reward = (traj_reward/traj_len)
# print 'reward of tehta = ' + str(reward)
return reward,ik_time,plan_time
def getIK(robot,env,oracle,Tgrasp): config = inverse_kinematics(oracle,Tgrasp) if config is not None: with robot: set_config(oracle.robot, config, oracle.robot.GetActiveManipulator().GetArmIndices()) import pdb;pdb.set_trace() if env.CheckCollision(robot): return None else: return True else: return None
def getCollisionFreeIKSolutionForGivenGrasp(g,env,manipulator,oracle):
Tgrasp = gmodel.getGlobalGraspTransform(g,collisionfree=False)
s_time = time.time()
goal_config = inverse_kinematics(oracle,Tgrasp)
"""
env.GetCollisionChecker().SetCollisionOptions(0)
s_time = time.time()
goal_config = manipulator.FindIKSolution(Tgrasp,
IkFilterOptions.CheckEnvCollisions)
"""
if goal_config is not None:
import pdb; pdb.set_trace()
iktime = time.time()-s_time
print 'iktime = ' + str(iktime)
return goal_config,iktime
def get_goal_config_from_theta_list(theta_list):
env=Environment()
env.Reset()
simple_prob=separate(env,0)
floor = env.GetKinBody("floorwalls")
floor.Enable(False)
oracle = ManipulationOracle(simple_prob,env)
robot = env.GetRobots()[0]
target=env.GetKinBody('ObjToG')
target.Enable(False)
manipulator = robot.SetActiveManipulator('leftarm_torso')
robot.SetActiveDOFs(manipulator.GetArmIndices())
gmodel = databases.grasping.GraspingModel(robot,target)
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,\
iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
# env.SetViewer('qtcoin')
goal_config_list = []
for idx in range(np.shape(theta_list)[0]):
print idx
base_pose = theta_list[idx,0:3]
grasp = theta_list[idx,3:]
robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1])
robot.SetActiveDOFValues(base_pose)
set_default_robot_config(robot) # set arms to the pregrasp pose
Tgrasp = gmodel.getGlobalGraspTransform(grasp,collisionfree=False)
goal_config = inverse_kinematics(oracle,Tgrasp)
if goal_config is not None:
import pdb; pdb.set_trace()
goal_config_list.append(goal_config)
env.Destroy()
return goal_config_list
# for g in grasps:
while g_idx < len(grasps):
g = grasps[g_idx]
if np.all( g[gmodel.graspindices.get('igraspdir')] == np.array([0,0,1]) ): #or\
# np.all(g[gmodel.graspindices.get('igraspdir')] == np.array([0,-1,0]) ) or\
# np.all(g[gmodel.graspindices.get('igraspdir')] == np.array([0,1,0]) ) :
g_idx+=2
continue
Tgrasp = gmodel.getGlobalGraspTransform(g,collisionfree=False)
if checkIR(env,robot,Tgrasp) is False:
print "no ir"
g_idx+=2
continue
pregrasp_config = inverse_kinematics(oracle,Tgrasp)
if pregrasp_config is None:
print "no_ik"
g_idx+=2
continue
robot.SetActiveDOFValues(pregrasp_config)
if env.CheckCollision(robot):
print "collision"
g_idx+=2
continue
good_grasp_found = True
break
if good_grasp_found == False:
def objective_fn(self,theta):
#Returns the normalize cost of the theta, reward(theta)/max_rwd
self.set_robot_to_config(theta)
self.env.UpdatePublishedBodies()
# call trajOptGetMotionPlan with goal_config suggested in theta
#goal_config = theta[3:]
ObjToG = self.env.GetKinBody("ObjToG")
ObjToG.Enable(False)
collisionChecker = RaveCreateCollisionChecker(self.env,'ode')
self.env.SetCollisionChecker(collisionChecker)
with self.robot:
manipulator = self.robot.SetActiveManipulator('leftarm_torso')
self.robot.SetActiveDOFs(manipulator.GetArmIndices())
# check collision, joint limit, etc
grasps, grasp_indices = self.gmodel.computeValidGrasps(\
checkcollision=False,checkik=False)
valid_grasp_exists=False
IR_passed = False
IK_passed = False
collision_passed = False
reward = 0
for g in grasps:
Tgrasp = self.gmodel.getGlobalGraspTransform(g,collisionfree=False)
if self.check_IR(Tgrasp):
IR_passed=True
if (not self.env.CheckCollision(self.robot)) \
and (not self.robot.CheckSelfCollision()):
collision_passed = True
goal_config = inverse_kinematics(self.oracle,\
Tgrasp)
if goal_config is not None:
IK_passed = True
# there is a reachable, collisionfree,
# and ik-possible grasp
break
if IR_passed and IK_passed and collision_passed:
reward = 3.0 / self.max_rwd #normalizing
elif IR_passed and collision_passed:
reward = 2.0 / self.max_rwd
return reward
elif IR_passed:
reward = 1.0 / self.max_rwd
return reward
elif (not IR_passed) and (not IK_passed) and (not collision_passed):
reward = 0
return reward
ObjToG.Enable(True)
traj,plantime,planstatus = self.trajOptGetMotionPlan(goal_config)
if planstatus == "Failed":
print 'planning failed'
return reward
traj_reward,traj_len,min_dists = self.get_path_rwds(traj)
reward = (4+traj_reward/traj_len)/self.max_rwd #+1 for having succesful path
return reward
def main():
# create thetas
#save_dir = './trajopt_data/'
length_of_obj=0.7555
save_dir = '/media/beomjoon/New Volume/place_and_partial_path/length_'+str(length_of_obj)+'_new'+'/'
body_name_list = []
env_transform_list = []
n_train_scenes = 0
while n_train_scenes <= 500:
scene_env_file_name = "scene"+str(n_train_scenes)+"_env.dae"
env_f_path = save_dir+'env_files/'+scene_env_file_name
print env_f_path
if os.path.exists(env_f_path):
n_train_scenes+=1
print env_f_path + " already exists"
continue
env=Environment()
## Setup the environment
simple_prob = two_tables_through_door(env,length_of_obj);
robot = env.GetRobots()[0]
target_obj= env.GetKinBody("ObjToG")
#env.SetViewer('qtcoin')
oracle = ManipulationOracle(simple_prob,env)
default_l_arm_config = [0.67717021, -0.34313199, 1.2, -1.46688405, 1.24223229, -1.95442826, 2.22254125+PI/2];
robot.SetDOFValues(default_l_arm_config, robot.GetManipulator('leftarm').GetArmIndices())
floor = env.GetKinBody("floorwalls")
floor.Enable(False)
gmodel = databases.grasping.GraspingModel(robot,target_obj)
## Set robot base at a default location
robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.RotationAxis,[0,0,1])
base_pos = np.array([-0.1,-0.9,-PI/2.0])
robot.SetActiveDOFValues(base_pos)
table_height = 0.75
manipulator = robot.SetActiveManipulator('leftarm_torso')
robot.SetActiveDOFs(manipulator.GetArmIndices())
ikmodel,gmodel= load_database(env,robot,manipulator,target_obj)
taskprob = interfaces.TaskManipulation(robot)
start_config = robot.GetActiveDOFValues() # save the original config to move back to
target_obj.Enable(False)
grasps, grasp_indices = gmodel.computeValidGrasps(checkcollision=False,checkik=False)
good_grasp_found = False
good_place_pose_found = False
pregrasp_config_list = []
place_config_list = []
base_loc_list = []
g_idx_list = []
obj_target_loc_list = []
orig_obj_loc = copy.deepcopy(target_obj.GetTransform())
g_idx=0
while g_idx < len(grasps):
restore_original_scene(manipulator,start_config,base_pos,robot,target_obj,orig_obj_loc)
g = grasps[g_idx]
if np.all( g[gmodel.graspindices.get('igraspdir')] == np.array([0,0,1]) ) or\
np.all(g[gmodel.graspindices.get('igraspdir')] == np.array([0,-1,0]) ) or\
np.all(g[gmodel.graspindices.get('igraspdir')] == np.array([0,1,0]) ):
g_idx+=1
continue
Tgrasp = gmodel.getGlobalGraspTransform(g,collisionfree=False)# This only checks the collision with the target obj
if checkIR(env,robot,Tgrasp) is False:
print "no ir"+str(g_idx)
g_idx+=1
continue
# prevent collision check with the target object
target_obj.Enable(False)
pregrasp_config = inverse_kinematics(oracle,Tgrasp)
target_obj.Enable(True)
if pregrasp_config is None:
print "no_ik"+str(g_idx)
g_idx+=1
continue
robot.SetActiveDOFs(manipulator.GetArmIndices())
robot.SetActiveDOFValues(pregrasp_config)
target_obj.Enable(False)
if env.CheckCollision(robot):
print "collision"+str(g_idx)
g_idx+=1
continue
target_obj.Enable(True)
print "good grasp found"
good_grasp_found = True
robot.SetActiveDOFValues(start_config) # we've verified that there is a pregrasp pose
# check if there is a place to put the obj down and appropriate base location for it
obj_place_pose,base_loc,place_config = sample_obj_place_pose_and_base_loc(target_obj,env,g,robot,oracle,gmodel)
if obj_place_pose is None:
print "no good place pose"
g_idx+=1
continue
good_place_pose_found = True
pregrasp_config_list.append(pregrasp_config)
place_config_list.append(place_config)
base_loc_list.append(base_loc)
g_idx_list.append(g_idx)
obj_target_loc_list.append(obj_place_pose)
if len(base_loc_list) == 5:
break
g_idx = g_idx + 1
"""
orig_obj_loc = copy.deepcopy(target_obj.GetTransform())
restore_original_scene(manipulator,start_config,base_pos,robot,target_obj,orig_obj_loc)
traj,planning_time,prep_time,plan_status = getMotionPlan(robot,pregrasp_config,env)
traj_waypts = show_trajectory(robot,traj)
import pdb;pdb.set_trace()
test_theta(pregrasp_config_list[0],place_config_list[0],base_loc_list[0],manipulator,robot,target_obj,start_config)
restore_original_scene(manipulator,start_config,base_pos,robot,target_obj,orig_obj_loc)
test_theta(pregrasp_config_list[1],place_config_list[1],base_loc_list[1],manipulator,robot,target_obj,start_config)
restore_original_scene(manipulator,start_config,base_pos,robot,target_obj,orig_obj_loc)
test_theta(pregrasp_config_list[2],place_config_list[2],base_loc_list[2],manipulator,robot,target_obj,start_config)
"""
if (good_grasp_found == False) or (good_place_pose_found == False):
env.Destroy()
continue
# grab the object
"""
# do motion planning to go back to the init config
traj,planning_time,prep_time,plan_status = getMotionPlan(robot,goal_config,env)
robot.SetActiveDOFValues(goal_config)
traj_waypts = show_trajectory(robot,traj)
"""
# save env
env.Save(save_dir+'env_files/'+scene_env_file_name,Environment.SelectionOptions.Everything)
sio.savemat(save_dir+'thetas/env_' + str(n_train_scenes) + '.mat', { 'obj_target_loc_list':obj_target_loc_list,\
'pregrasp_config_list':pregrasp_config_list,\
'place_config_list':place_config_list,\
'base_loc_list':base_loc_list,\
'g_idx_list':g_idx_list, \
'grasps':grasps
})
n_train_scenes += 1
# save the reward - that is, reduced time
env.Destroy()
