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()