def main(): """Loads an environment and generates random positions until the robot can reach an oject from a collision-free pose. """ env = openravepy.Environment() env.Load('data/pr2test1.env.xml') robot=env.GetRobots()[0] mug = env.GetKinBody('mug1') manip = robot.SetActiveManipulator('leftarm_torso') ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot,iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() num_contacts = 1 #just cheating isreachable = False while (num_contacts > 0) or (not isreachable): T = generate_random_pos(robot, mug) robot.SetTransform(T) env.GetCollisionChecker().SetCollisionOptions(openravepy.CollisionOptions.Contacts) # get first collision report = openravepy.CollisionReport() collision=env.CheckCollision(robot,report=report) num_contacts = len(report.contacts) openravepy.raveLogInfo("Number of contacts: " + str(num_contacts)) sol = check_reachable(robot, manip, mug) isreachable = sol is not None #time.sleep(1) robot.SetDOFValues(sol, manip.GetArmIndices()) openravepy.raveLogInfo("Done!!") env.SetViewer('qtcoin')
def main(): """Loads an environment and generates random positions until the robot can reach an oject from a collision-free pose. """ env = openravepy.Environment() env.Load('data/pr2test1.env.xml') env.SetViewer('qtcoin') robot=env.GetRobots()[0] manip = robot.SetActiveManipulator('rightarm') pose = generate_random_pos(robot) robot.SetTransform(pose) obj = env.GetKinBody('mug1') #env.GetKinBody('obstacle').SetVisible(False) try: pose, sol, torso= get_collision_free_grasping_pose(robot, obj, 100) robot.SetTransform(pose) robot.SetDOFValues(sol, manip.GetArmIndices()) openravepy.raveLogInfo("Done!!") except ValueError, e: openravepy.raveLogError("Error while trying to find a pose: %s" % e) return
def test_anchors(self): raveLogInfo('Loaded {}'.format(filename)) pose = openhubo.Pose(self.robot) errors_home = check_all_constraints(pose) pose['RSR'] = 15. * pi / 180 pose['LSR'] = -15. * pi / 180 errors_bent_SR = check_all_constraints(pose) pose['REP'] = 10. * pi / 180 pose['LEP'] = 10. * pi / 180 errors_bent_EP = check_all_constraints(pose) #raveLogDebug( "Error sums for {}:".format(self.robot.GetName())) self.assertLess( min([ errmax(errors_home), errmax(errors_bent_SR), errmax(errors_bent_EP) ]), 1e-10)
def main(env,options): "Main example code." if options.iktype is not None: # cannot use .names due to python 2.5 (or is it boost version?) for value,type in IkParameterization.Type.values.iteritems(): if type.name.lower() == options.iktype.lower(): iktype = type break else: iktype = IkParameterizationType.Transform6D env.Load(options.scene) with env: # load the Transform6D IK models ikmodels = [] for robot in env.GetRobots(): for manip in robot.GetManipulators(): print manip try: robot.SetActiveManipulator(manip) ikmodel = databases.inversekinematics.InverseKinematicsModel(robot, iktype=iktype) if not ikmodel.load(): ikmodel.autogenerate() if not ikmodel.has(): continue ikmodels.append(ikmodel) except Exception, e: print 'failed manip %s'%manip, e if len(ikmodels) == 0: raveLogWarn('no manipulators found that can be loaded with iktype %s'%str(iktype)) # create the pickers pickers = [] for ikmodel in ikmodels: raveLogInfo('creating picker for %s'%str(ikmodel.manip)) picker = RaveCreateKinBody(env,'') picker.InitFromBoxes(array([ [0.05,0,0,0.05,0.01,0.01], [0,0.05,0,0.01,0.05,0.01], [0,0,0.05,0.01,0.01,0.05] ]),True) for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()): color = zeros(3) color[igeom] = 1 geom.SetDiffuseColor(color) picker.SetName(ikmodel.manip.GetName()) env.Add(picker,True) # have to disable since not part of collision picker.Enable(False) picker.SetTransform(ikmodel.manip.GetTransform()) pickers.append([picker,ikmodel.manip])
def create_trajectory(robot,waypts=None): """ Create a trajectory based on a robot's config spec. Optionally added a list of waypoints """ traj=_rave.RaveCreateTrajectory(robot.GetEnv(),'') config=robot.GetConfigurationSpecification() config.AddDeltaTimeGroup() traj.Init(config) if waypts is not None: _rave.raveLogInfo("Appending waypoint(s)") try: for w in waypts: traj_append(traj,w) except TypeError: #fallthrough if single waypoint traj_append(traj,waypts) return [traj,config]
def get_occluding_objects_names(robot, obj, body_filter, num_trials = 300, just_one_attempt = False, return_pose = False): """ Returns the names of all the objects as calculated by get_occluding_objects with additional filtering. Paramters: body_filter: a function that takes a KinBody and returns True or False. just_one_attempt: If True then it will return only the result of one successfull grasping attempt. Example usage: get_occluding_objects_names(robot, obj, lambda b:b.GetName().startswith("random"), 500) """ if return_pose: (pose, sol, torso_angle, obstacles_bodies) = get_occluding_objects(robot, obj, num_trials, just_one_attempt, return_pose) else: obstacles_bodies = get_occluding_objects(robot, obj, num_trials, just_one_attempt, return_pose) openravepy.raveLogInfo("Bodies: %s" % obstacles_bodies) nonempty = lambda l:len(l)>0 obstacles = set( filter(nonempty, (tuple(str(b.GetName()) for b in l if body_filter(b) and b.GetName() != obj.GetName() ) for l in obstacles_bodies ) ) ) if return_pose: return pose, sol, torso_angle, obstacles else: return obstacles
def create_trajectory(robot,waypts=None,filename=None): """ Create a trajectory based on a robot's config spec. Optionally added a list of waypoints """ traj=_rave.RaveCreateTrajectory(robot.GetEnv(),'') config=robot.GetConfigurationSpecification() config.AddDeltaTimeGroup() traj.Init(config) if waypts is not None: _rave.raveLogInfo("Appending waypoint(s)") try: for w in waypts: traj_append(traj,w) except TypeError: #fallthrough if single waypoint traj_append(traj,waypts) if filename is not None: read_trajectory_from_file(traj,filename) return [traj,config]
def test_anchors(self): raveLogInfo('Loaded {}'.format(filename)) pose=openhubo.Pose(self.robot) errors_home=check_all_constraints(pose) pose['RSR']=15.*pi/180 pose['LSR']=-15.*pi/180 errors_bent_SR=check_all_constraints(pose) pose['REP']=10.*pi/180 pose['LEP']=10.*pi/180 errors_bent_EP=check_all_constraints(pose) #raveLogDebug( "Error sums for {}:".format(self.robot.GetName())) self.assertLess(min([errmax(errors_home),errmax(errors_bent_SR),errmax(errors_bent_EP)]),1e-10)
def get_occluding_objects_names(good_bodies, robot, obj, body_filter, num_trials = 0, just_one_attempt = False, return_pose = False): """ Returns the names of all the objects as calculated by get_occluding_objects with additional filtering. Paramters: body_filter: a function that takes a KinBody and returns True or False. just_one_attempt: If True then it will return only the result of one successfull grasping attempt. Example usage: get_occluding_objects_names(good_bodies, robot, obj, lambda b:b.GetName().startswith("random"), 500) """ if return_pose: (pose, sol, torso_angle, obstacles_bodies) = get_occluding_objects(good_bodies, robot, obj, num_trials, just_one_attempt, return_pose, body_filter) else: obstacles_bodies = get_occluding_objects(good_bodies, robot, obj, num_trials, just_one_attempt, return_pose, body_filter) openravepy.raveLogInfo("Bodies: %s" % obstacles_bodies) obstacles = set() for l in obstacles_bodies: t = [] for b in l: if body_filter(b) and (b.GetName() != obj.GetName()): t.append(str(b.GetName())) t = tuple(t) if len(t) > 0: obstacles.add(t) if return_pose: return pose, sol, torso_angle, obstacles else: return obstacles
def main(): """Loads an environment and generates random positions until the robot can reach an oject from a collision-free pose. """ env = openravepy.Environment() env.Load('data/pr2test1.env.xml') robot = env.GetRobots()[0] mug = env.GetKinBody('mug1') manip = robot.SetActiveManipulator('leftarm_torso') ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot, iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() num_contacts = 1 #just cheating isreachable = False while (num_contacts > 0) or (not isreachable): T = generate_random_pos(robot, mug) robot.SetTransform(T) env.GetCollisionChecker().SetCollisionOptions( openravepy.CollisionOptions.Contacts) # get first collision report = openravepy.CollisionReport() collision = env.CheckCollision(robot, report=report) num_contacts = len(report.contacts) openravepy.raveLogInfo("Number of contacts: " + str(num_contacts)) sol = check_reachable(robot, manip, mug) isreachable = sol is not None #time.sleep(1) robot.SetDOFValues(sol, manip.GetArmIndices()) openravepy.raveLogInfo("Done!!") env.SetViewer('qtcoin')
def generate_grasping_pose(robot, obj_to_grasp, use_general_grasps=True, checkik = True): """Returns a list with all the valid grasps for object obj_to_grasp. Parameters: robot: an OpenRave robot obj_to_grasp: a KinBody that the robot should grasp use_general_grasps: if True, don't calculate actual grasp points, but use a pre-generated list. It is much faster if a grasping model has not been generated. checkik: passed to GraspingModel.computeValidGrasps Returns: a list of Transformation matrices, one for each valid grasping pose """ class __Options(object): def __init__(self): self.delta=0.1 self.normalanglerange=0.5 self.standoffs = [0] #self.rolls = np.arange(0,2*np.pi,0.5*np.pi) self.rolls = np.arange(0,np.pi,0.5*np.pi) self.directiondelta = 0.4 pass gmodel = openravepy.databases.grasping.GraspingModel(robot, obj_to_grasp) if use_general_grasps: gmodel.grasps = utils.pre_grasps else: if not gmodel.load(): openravepy.raveLogInfo("Generating grasping model...") gmodel.autogenerate(__Options()) openravepy.raveLogInfo("Generating grasps") validgrasps, _ = gmodel.computeValidGrasps(checkcollision=False, checkik = checkik, checkgrasper = False) openravepy.raveLogInfo("Number of valid grasps: %d" % len(validgrasps)) return [gmodel.getGlobalGraspTransform(grasp) for grasp in validgrasps]
def plan_env_matching(or_robot, contact_points, contact_regions, torso_pose_grid, start, goal, torso_path, structures, motion_mode, motion_plan_library, motion_plan_clusters, motion_plan_explore_order, planning_time_limit, elasticstrips=None, general_ik_interface=None): ###################################################################################### # Initialization rave.raveLogInfo('plan_env_matching') DOFNameActiveIndexDict = or_robot.DOFNameActiveIndexDict lower_limits = or_robot.lower_limits higher_limits = or_robot.higher_limits IKInitDOFValues = or_robot.IKInitDOFValues StandingPostureDOFValues = or_robot.StandingPostureDOFValues or_robot.robot.SetDOFValues(StandingPostureDOFValues) goal_x = goal[0] goal_y = goal[1] goal_z = torso_pose_grid.get_cell_height( torso_pose_grid.position_to_grid_xy((goal_x, goal_y))) if goal_z < -0.05: goal_z = 0 goal_theta = goal[2] start_x = start[0] start_y = start[1] start_z = torso_pose_grid.get_cell_height( torso_pose_grid.position_to_grid_xy((start_x, start_y))) if start_z < -0.05: start_z = 0 start_theta = start[2] goal_dist = math.hypot(goal_x - start_x, goal_y - start_y) collision_link_list = [ 'l_shin', 'l_foot', 'r_shin', 'r_foot', 'l_forearm', 'l_palm', 'r_forearm', 'r_palm' ] selfcollision_link_pair_list = [('l_foot', 'r_foot'), ('l_shin', 'r_shin'), ('l_palm', 'l_thigh'), ('l_index_prox', 'l_thigh'), ('l_index_med', 'l_thigh'), ('l_index_dist', 'l_thigh'), ('r_palm', 'r_thigh'), ('r_index_prox', 'r_thigh'), ('r_index_med', 'r_thigh'), ('r_index_dist', 'r_thigh')] i_xpa = DOFNameActiveIndexDict['x_prismatic_joint'] i_ypa = DOFNameActiveIndexDict['y_prismatic_joint'] i_zpa = DOFNameActiveIndexDict['z_prismatic_joint'] i_rra = DOFNameActiveIndexDict['roll_revolute_joint'] i_pra = DOFNameActiveIndexDict['pitch_revolute_joint'] i_yra = DOFNameActiveIndexDict['yaw_revolute_joint'] env = or_robot.env start_bound_radius = 0.1 goal_bound_radius = 0.1 # DrawRegion(or_robot.env,xyzrpy_to_SE3([0,0,init_z+0.01,0,0,0]),start_bound_radius) # DrawRegion(or_robot.env,xyzrpy_to_SE3([goal_x,goal_y,goal_z+0.01,0,0,goal_theta]),goal_bound_radius) ###################################################################################### start_time = time.time() solution_found = False subsegment_goal = copy.deepcopy(start) largest_covered_cell_index = 0 subsegment_list = [] # check if every part of the path is covered while largest_covered_cell_index != len(torso_path) - 1: if time.time() - start_time > planning_time_limit: break subsegment_start = copy.deepcopy(subsegment_goal) subsegment_start_to_goal_dist = math.hypot( goal[0] - subsegment_start[0], goal[1] - subsegment_start[1]) found_matching_motion_plan = False for motion_plan_index in motion_plan_explore_order: or_robot.robot.SetDOFValues(StandingPostureDOFValues) plan = motion_plan_library[motion_plan_index] plan.contact_plan[0].parent = None plan_shorter_than_torso_path = plan.plan_travel_dist < subsegment_start_to_goal_dist + goal_bound_radius # if the plan travel distance is shorter than the subsegment length, use all the motion plan if plan_shorter_than_torso_path: # the intersection of the motion plan to the closest torso path node current_travel_dist = 0 cell_position = subsegment_start subsegment_goal = None subsegment_torso_path_range = None for i, cell in enumerate( torso_path[largest_covered_cell_index:]): last_cell_position = cell_position cell_position = cell.get_position() last_travel_dist = current_travel_dist current_travel_dist = math.hypot( cell_position[0] - subsegment_start[0], cell_position[1] - subsegment_start[1]) if plan.plan_travel_dist < current_travel_dist: # decide subsegment_goal if abs(current_travel_dist - plan.plan_travel_dist) < abs( last_travel_dist - plan.plan_travel_dist): subsegment_goal = cell_position else: subsegment_goal = last_cell_position # decide subsegment_torso_path_range subsegment_torso_path_range = ( largest_covered_cell_index, i + largest_covered_cell_index) break # means the motion plan cover the whole remaining torso path if subsegment_torso_path_range is None: subsegment_goal = cell_position subsegment_torso_path_range = (largest_covered_cell_index, len(torso_path) - 1) involved_contact_sequence = copy.deepcopy( plan.contact_sequence) # if the plan travel distance is longer than the subsegment length, use part of the motion plan else: # find the contacts that will be involved in the the feature matching current_left_foot_contact = None current_right_foot_contact = None involved_contact_sequence = [] orientation_offset = None final_left_foot_contact = None final_right_foot_contact = None for contact in plan.contact_sequence: if contact.manip == 'l_foot': current_left_foot_contact = contact elif contact.manip == 'r_foot': current_right_foot_contact = contact if current_left_foot_contact is None or current_right_foot_contact is None: involved_contact_sequence.append( copy.deepcopy(contact)) continue current_mean_foot_position = ( current_left_foot_contact.transform[0:3, 3] + current_right_foot_contact.transform[0:3, 3]) / 2.0 current_travel_dist = math.hypot( current_mean_foot_position[0], current_mean_foot_position[1]) if current_travel_dist <= subsegment_start_to_goal_dist + goal_bound_radius: involved_contact_sequence.append( copy.deepcopy(contact)) orientation_offset = math.atan2( current_mean_foot_position[1], current_mean_foot_position[0]) final_left_foot_contact = current_left_foot_contact final_right_foot_contact = current_right_foot_contact else: break # decide subsegment_goal subsegment_goal = copy.deepcopy(goal) # decide subsegment indices range subsegment_torso_path_range = (largest_covered_cell_index, len(torso_path) - 1) subsegment_goal_x = subsegment_goal[0] subsegment_goal_y = subsegment_goal[1] subsegment_goal_z = torso_pose_grid.get_cell_height( torso_pose_grid.position_to_grid_xy( (subsegment_goal_x, subsegment_goal_y))) if subsegment_goal_z < -0.05: subsegment_goal_z = 0 subsegment_goal_theta = subsegment_goal[2] subsegment_start_x = subsegment_start[0] subsegment_start_y = subsegment_start[1] subsegment_start_z = torso_pose_grid.get_cell_height( torso_pose_grid.position_to_grid_xy( (subsegment_start_x, subsegment_start_y))) if subsegment_start_z < -0.05: subsegment_start_z = 0 subsegment_start_theta = subsegment_start[2] # initialize the rigid plan pose if plan_shorter_than_torso_path: init_theta = math.atan2(subsegment_goal_y - subsegment_start_y, subsegment_goal_x - subsegment_start_x) else: init_theta = math.atan2( subsegment_goal_y - subsegment_start_y, subsegment_goal_x - subsegment_start_x) - orientation_offset init_x = subsegment_start_x init_y = subsegment_start_y init_z = subsegment_start_z # the plan are all aligned to the mean position of two feet at the beginning. q_seq = np.array([[init_x], [init_y], [init_z], [init_theta] ]) # (x,y,z,theta) of the whole sequence # jacobian alignment and get the matching score plan_cost = fitting_env_and_get_plan_cost( or_robot, involved_contact_sequence, contact_regions, q_seq, (subsegment_start_x, subsegment_start_y, subsegment_start_z, subsegment_start_theta), (subsegment_goal_x, subsegment_goal_y, subsegment_goal_z, subsegment_goal_theta), start_bound_radius, goal_bound_radius, dynamic_final_feet_contacts=True) print('Plan %i, Cost: %5.5f' % (motion_plan_index, plan_cost)) if plan_cost == 9999 or plan_cost == 4999: # length not match rave.raveLogWarn( 'Contact pose distance too high, reject the motion plan.') continue plan_yaw = q_seq[3, 0] * rad_to_deg plan_transform = xyzrpy_to_SE3( [q_seq[0, 0], q_seq[1, 0], q_seq[2, 0], 0, 0, plan_yaw]) plan_contact_sequence = copy.deepcopy(involved_contact_sequence) for i in range(len(involved_contact_sequence)): plan_contact_sequence[i].transform = np.dot( plan_transform, involved_contact_sequence[i].transform) ################################################## # initialize the objects needed for elastic strips aligned_rave_traj = rave.RaveCreateTrajectory(or_robot.env, '') traj_serial = open(plan.traj_path, 'r').read() aligned_rave_traj.deserialize(traj_serial) if plan_shorter_than_torso_path: waypoint_contact_manips = copy.deepcopy( plan.waypoint_contact_manips) plan_path = copy.deepcopy(plan.contact_plan) plan_aligned_traj = [None ] * aligned_rave_traj.GetNumWaypoints() for i in range(aligned_rave_traj.GetNumWaypoints()): plan_aligned_traj[i] = copy.deepcopy( aligned_rave_traj.GetWaypoint(i)) else: final_left_foot_contact_position = final_left_foot_contact.transform[ 0:3, 3] final_right_foot_contact_position = final_right_foot_contact.transform[ 0:3, 3] final_left_foot_contact_reached = False final_right_foot_contact_reached = False contact_state_bound_indices = None for i, node in enumerate(plan.contact_plan): if np.linalg.norm( (np.array(node.left_leg[0:3]) - final_left_foot_contact_position)) < 0.002: final_left_foot_contact_reached = True else: if final_left_foot_contact_reached and final_right_foot_contact_reached: contact_state_bound_indices = (0, i - 1) break if np.linalg.norm( (np.array(node.right_leg[0:3]) - final_right_foot_contact_position)) < 0.002: final_right_foot_contact_reached = True else: if final_left_foot_contact_reached and final_right_foot_contact_reached: contact_state_bound_indices = (0, i - 1) break if final_left_foot_contact_reached and final_right_foot_contact_reached and i == len( plan.contact_plan) - 1: contact_state_bound_indices = (0, i) if contact_state_bound_indices is None: print('empty contact state bound indices') IPython.embed() (plan_path, plan_aligned_traj, waypoint_contact_manips) = extract_partial_motion_plan( plan.contact_plan, aligned_rave_traj, plan.waypoint_contact_manips, contact_state_bound_indices) if len(waypoint_contact_manips) == 0: rave.raveLogWarn( 'Extracted motion plan does not contain any waypoint, reject the motion plan.' ) continue # check if the motion plan orientation matches the start and goal orientation if (abs( angle_diff(subsegment_start_theta, plan_path[0].get_virtual_body_yaw() + plan_yaw)) > 30 or abs( angle_diff( subsegment_goal_theta, plan_path[-1].get_virtual_body_yaw() + plan_yaw)) > 30): rave.raveLogWarn( 'Path orientation difference too high, reject the motion plan.' ) continue ################################################## # delete_contact_visualization(or_robot) if draw_planning_progress: show_contact(or_robot, plan_contact_sequence) # DrawLocation(or_robot.env,np.array([[1,0,0,0],[0,1,0,0],[0,0,1,get_z(0,0,structures)+0.01],[0,0,0,1]]),[255,0,0],start_bound_radius) # DrawLocation(or_robot.env,np.array([[1,0,0,goal_x],[0,1,0,goal_y],[0,0,1,goal_z+0.01],[0,0,0,1]]),[0,0,255],goal_bound_radius) # initialize robot configuration or_robot.robot.SetDOFValues(IKInitDOFValues) num_waypoints = len(plan_aligned_traj) plan_traj = [None] * num_waypoints for i in range(num_waypoints): q = copy.deepcopy(plan_aligned_traj[i]) affine_x = q[i_xpa] affine_y = q[i_ypa] affine_z = q[i_zpa] affine_roll = q[i_rra] * rad_to_deg affine_pitch = q[i_pra] * rad_to_deg affine_yaw = q[i_yra] * rad_to_deg affine_transform = xyzrpy_to_SE3([ affine_x, affine_y, affine_z, affine_roll, affine_pitch, affine_yaw ]) new_affine_transform = np.dot(plan_transform, affine_transform) new_affine_xyzrpy = SE3_to_xyzrpy(new_affine_transform) q[i_xpa] = min(max(new_affine_xyzrpy[0], lower_limits[i_xpa]), higher_limits[i_xpa]) q[i_ypa] = min(max(new_affine_xyzrpy[1], lower_limits[i_ypa]), higher_limits[i_ypa]) q[i_zpa] = min(max(new_affine_xyzrpy[2], lower_limits[i_zpa]), higher_limits[i_zpa]) q[i_rra] = min( max(new_affine_xyzrpy[3] * deg_to_rad, lower_limits[i_rra]), higher_limits[i_rra]) q[i_pra] = min( max(new_affine_xyzrpy[4] * deg_to_rad, lower_limits[i_pra]), higher_limits[i_pra]) q[i_yra] = min( max(new_affine_xyzrpy[5] * deg_to_rad, lower_limits[i_yra]), higher_limits[i_yra]) plan_traj[i] = q # Elastic Strips # trajectory list is going to be a list of lists, each list stores the activeDOF value of the waypoint trajectory_list = plan_traj manips_list = set(['l_leg', 'r_leg']) for contact_manips in waypoint_contact_manips: for contact_manip in contact_manips: if contact_manip[0] == 'l_arm': manips_list.add('l_arm') elif contact_manip[0] == 'r_arm': manips_list.add('r_arm') manips_list = list(manips_list) support_list = [('l_leg', mu), ('r_leg', mu), ('l_arm', mu), ('r_arm', mu)] print('before elastic strips.') or_robot.robot.SetDOFValues(StandingPostureDOFValues) elasticstrips = ElasticStrips(env, or_robot.robot.GetName()) or_robot.robot.SetDOFValues(StandingPostureDOFValues) print('reachability elastic strips') if len(trajectory_list) != len(waypoint_contact_manips): rave.raveLogError( 'Trajectory waypoint number mismatch. Cannot do Elastic Strips.' ) IPython.embed() transition_trajectory_list = [] transition_waypoint_contact_manips = [] waypoint_manip_num = len(waypoint_contact_manips[0]) downsampled_trajectory_list = [] downsampled_waypoint_contact_manips = [] for i in range(len(trajectory_list) + 1): if i == len(trajectory_list) or waypoint_manip_num != len( waypoint_contact_manips[i]): if len(transition_trajectory_list) > 2: downsampled_transition_trajectory_list = transition_trajectory_list[ 0:len(transition_trajectory_list):5] downsampled_transition_waypoint_contact_manips = transition_waypoint_contact_manips[ 0:len(transition_trajectory_list):5] if (len(transition_trajectory_list) - 1) % 5 != 0: downsampled_transition_trajectory_list.append( transition_trajectory_list[-1]) downsampled_transition_waypoint_contact_manips.append( transition_waypoint_contact_manips[-1]) else: downsampled_transition_trajectory_list = transition_trajectory_list downsampled_transition_waypoint_contact_manips = transition_waypoint_contact_manips downsampled_trajectory_list.extend( downsampled_transition_trajectory_list) downsampled_waypoint_contact_manips.extend( downsampled_transition_waypoint_contact_manips) if i < len(trajectory_list): waypoint_manip_num = len(waypoint_contact_manips[i]) transition_trajectory_list = [trajectory_list[i]] transition_waypoint_contact_manips = [ waypoint_contact_manips[i] ] else: transition_trajectory_list.append(trajectory_list[i]) transition_waypoint_contact_manips.append( waypoint_contact_manips[i]) num_waypoints = len(downsampled_trajectory_list) trajectory_list = downsampled_trajectory_list waypoint_contact_manips = downsampled_waypoint_contact_manips with env: with or_robot.robot: output_waypoints_list = elasticstrips.RunElasticStrips( manips=manips_list, trajectory=trajectory_list, contact_manips=waypoint_contact_manips, printcommand=False) or_robot.robot.SetDOFValues(StandingPostureDOFValues) if output_waypoints_list: end_time = time.time() modified_rave_traj = rave.RaveCreateTrajectory( or_robot.env, '') modified_rave_traj.Init( aligned_rave_traj.GetConfigurationSpecification()) q_modified = [None] * or_robot.robot.GetActiveDOF() for w in range(num_waypoints): for i in range(or_robot.robot.GetActiveDOF()): q_modified[i] = output_waypoints_list[ w * (or_robot.robot.GetActiveDOF() + 1) + i + 1] modified_rave_traj.Insert( modified_rave_traj.GetNumWaypoints(), q_modified) # extract contact plan path from the trajectory prev_contact_manips = waypoint_contact_manips[0] or_robot.robot.SetActiveDOFValues( modified_rave_traj.GetWaypoint(0)) plan_path[0].left_leg = SE3_to_xyzrpy( or_robot.robot.GetManipulator('l_leg').GetTransform()) plan_path[0].right_leg = SE3_to_xyzrpy( or_robot.robot.GetManipulator('r_leg').GetTransform()) if plan_path[0].left_arm[0] != -99.0: plan_path[0].left_arm = SE3_to_xyzrpy( or_robot.robot.GetManipulator('l_arm').GetTransform()) if plan_path[0].right_arm[0] != -99.0: plan_path[0].right_arm = SE3_to_xyzrpy( or_robot.robot.GetManipulator('r_arm').GetTransform()) plan_path[0].prev_move_manip = None current_path_index = 1 for w in range(1, modified_rave_traj.GetNumWaypoints()): or_robot.robot.SetActiveDOFValues( modified_rave_traj.GetWaypoint(w)) if len(prev_contact_manips) < len( waypoint_contact_manips[w]): plan_path[current_path_index].left_leg = copy.copy( plan_path[current_path_index - 1].left_leg) plan_path[current_path_index].right_leg = copy.copy( plan_path[current_path_index - 1].right_leg) plan_path[current_path_index].left_arm = copy.copy( plan_path[current_path_index - 1].left_arm) plan_path[current_path_index].right_arm = copy.copy( plan_path[current_path_index - 1].right_arm) if plan_path[current_path_index].prev_move_manip == 0: plan_path[ current_path_index].left_leg = SE3_to_xyzrpy( or_robot.robot.GetManipulator( 'l_leg').GetTransform()) elif plan_path[ current_path_index].prev_move_manip == 1: plan_path[ current_path_index].right_leg = SE3_to_xyzrpy( or_robot.robot.GetManipulator( 'r_leg').GetTransform()) elif plan_path[ current_path_index].prev_move_manip == 2: plan_path[ current_path_index].left_arm = SE3_to_xyzrpy( or_robot.robot.GetManipulator( 'l_arm').GetTransform()) elif plan_path[ current_path_index].prev_move_manip == 3: plan_path[ current_path_index].right_arm = SE3_to_xyzrpy( or_robot.robot.GetManipulator( 'r_arm').GetTransform()) while current_path_index < len(plan_path) - 1: current_path_index = current_path_index + 1 if plan_path[ current_path_index].prev_move_manip == 0 or plan_path[ current_path_index].prev_move_manip == 1: break elif plan_path[ current_path_index].prev_move_manip == 2 and plan_path[ current_path_index].left_arm[ 0] != -99.0: break elif plan_path[ current_path_index].prev_move_manip == 3 and plan_path[ current_path_index].right_arm[ 0] != -99.0: break prev_contact_manips = waypoint_contact_manips[w] for i in range(len(plan_path)): pose_list = [ plan_path[i].left_leg, plan_path[i].right_leg, plan_path[i].left_arm, plan_path[i].right_arm ] for j in range(len(pose_list)): pose_list[j][0:3] = [ round(v, 3) for v in pose_list[j][0:3] ] pose_list[j][3:6] = [ round(v, 1) for v in pose_list[j][3:6] ] # check if it satisfies the end point balance criterion transition_feasible = True with env: with or_robot.robot: for plan_node in plan_path: if not transition_feasibility( or_robot, general_ik_interface, plan_node): transition_feasible = False break # check if the iniitial node is feasible if transition_feasible: all_support_list = [('l_leg', mu), ('r_leg', mu)] if plan_path[0].left_arm[0] != -99.0: all_support_list.append(('l_arm', mu)) if plan_path[0].right_arm[0] != -99.0: all_support_list.append(('r_arm', mu)) for support_list in itertools.combinations( all_support_list, len(all_support_list) - 1): if node_IK(or_robot, general_ik_interface, plan_path[0], support_list) is None: transition_feasible = False break # check if the goal node is feasible if transition_feasible: all_support_list = [('l_leg', mu), ('r_leg', mu)] if plan_path[-1].left_arm[0] != -99.0: all_support_list.append(('l_arm', mu)) if plan_path[-1].right_arm[0] != -99.0: all_support_list.append(('r_arm', mu)) for support_list in itertools.combinations( all_support_list, len(all_support_list) - 1): if node_IK(or_robot, general_ik_interface, plan_path[-1], support_list) is None: transition_feasible = False break if not transition_feasible: rave.raveLogWarn( 'Transition is not feasible, reject the motion plan.') continue found_matching_motion_plan = True # final_rave_traj = rave.RaveCreateTrajectory(or_robot.env,'') # final_rave_traj.Init(aligned_rave_traj.GetConfigurationSpecification()) final_rave_traj = None final_waypoint_contact_manips = None # with env: # (feasible_traj,final_waypoint_contact_manips) = step_interpolation(final_rave_traj,plan_path,or_robot) # break rave.raveLogInfo('Matching motion plan found.') delete_contact_visualization(or_robot) subsegment_list.append( (plan_path, final_rave_traj, final_waypoint_contact_manips, subsegment_torso_path_range)) largest_covered_cell_index = subsegment_torso_path_range[1] break else: rave.raveLogWarn('Elastic Strips fail. Not feasible.') delete_contact_visualization(or_robot) if not found_matching_motion_plan: if subsegment_list: subsegment_list.append( (None, None, None, subsegment_torso_path_range)) break delete_contact_visualization(or_robot) if not subsegment_list or time.time() - start_time > planning_time_limit: return None else: return subsegment_list
def get_torso_grasping_pose(good_bodies, robot, object_to_grasp, max_trials = 100, use_general_grasps = True, ): """Returns the torso height from where the robot can grasp an object. Parameters: robot: an OpenRave robot instance object_to_grasp: a KinBody that the robot should grasp max_trials: how many attempts before giving up use_general_grasps: f True, don't calculate actual grasp points, but use a pre-generated list. It is much faster if a grasping model has not been generated. Returns: (sol, torso_angle): , the active manipulator angles and the torso joint angle from where the robot can grasp an object. Raises GraspingPoseError if no valid solution is found. """ env = robot.GetEnv() robot_pose = robot.GetTransform() torso_angle = robot.GetJoint("torso_lift_joint").GetValues()[0] manip = robot.GetActiveManipulator() ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot,iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() grasping_poses = generate_grasping_pose(robot, object_to_grasp, use_general_grasps) openravepy.raveLogInfo("I've got %d grasping poses" % len(grasping_poses)) env.GetCollisionChecker().SetCollisionOptions(openravepy.CollisionOptions.Contacts) collision = env.CheckCollision(robot) sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, grasping_poses) isreachable = sol is not None min_torso, max_torso = utils.get_pr2_torso_limit(robot) num_trial = 0 with robot: while ((collision) or (not isreachable)) and (num_trial < max_trials): num_trial +=1 torso_angle = move_random_torso(robot, min_torso, max_torso) report = openravepy.CollisionReport() collision = env.CheckCollision(robot, report=report) if not collision: grasping_poses = generate_grasping_pose(robot, object_to_grasp, use_general_grasps) sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, grasping_poses) isreachable = sol is not None else: continue if (sol is None) or collision: e = GraspingPoseError("No collision free grasping pose found within %d steps" % max_trials) raise e else: return (sol, torso_angle)
def generate_grasping_pose(robot, obj_to_grasp, use_general_grasps=True, checkik = True): """Returns a list with all the valid grasps for object obj_to_grasp. Parameters: robot: an OpenRave robot obj_to_grasp: a KinBody that the robot should grasp use_general_grasps: if True, don't calculate actual grasp points, but use a pre-generated list. It is much faster if a grasping model has not been generated. checkik: passed to GraspingModel.computeValidGrasps Returns: a list of Transformation matrices, one for each valid grasping pose """ class __Options(object): def __init__(self): self.delta=0.0 self.normalanglerange=0.0 self.standoffs = [0] #self.rolls = np.arange(0,2*np.pi,0.5*np.pi) self.rolls = np.arange(0,np.pi,0.25*np.pi) self.directiondelta = 0.0 pass gmodel = openravepy.databases.grasping.GraspingModel(robot, obj_to_grasp) #use_general_grasps = False if use_general_grasps: """ not_vertical = lambda grasp: openravepy.axisAngleFromRotationMatrix( gmodel.getGlobalGraspTransform(grasp))[2] > 0.1 x_c = lambda grasp: gmodel.getGlobalGraspTransform(grasp)[:3,0][2] < 0.1 gmodel.grasps = filter(x_c, utils.cylinder_pre_grasps) """ gmodel.grasps = utils.side_cylinder_pre_grasps """ for i, grasp in enumerate(utils.cylinder_pre_grasps): rot = gmodel.getGlobalGraspTransform(grasp) a = openravepy.axisAngleFromRotationMatrix(rot) print i print a gmodel.showgrasp(grasp) """ np.random.shuffle(gmodel.grasps) else: if not gmodel.load(): openravepy.raveLogInfo("Generating grasping model...") gmodel.autogenerate(__Options()) """ text = '[' for grasp in gmodel.grasps: text += '[' for joint in grasp: text += str(joint) text += ',' text += '],\n' text += ']' with open("grasps.txt", 'w') as grasp_file: grasp_file.write(text) """ openravepy.raveLogInfo("Generating grasps") validgrasps, _ = gmodel.computeValidGrasps(checkcollision=False, checkik = checkik, checkgrasper = False) """ for grasp in validgrasps: gmodel.showgrasp(grasp) """ openravepy.raveLogInfo("Number of valid grasps: %d" % len(validgrasps)) import pdb #pdb.set_trace() return [gmodel.getGlobalGraspTransform(grasp) for grasp in validgrasps]
def load_scene_from_options(env, options): """Load a scene for openhubo based on the options structure (see setup function for detailed options).""" if hasattr(options, 'physics') and options.physics is False: #Kludge since we won't be not using ODE for a while... physics = False elif hasattr(options, 'physics') and options.physics: physics = True elif options._physics == 'ode': physics = True else: physics = False vidrecorder = _recorder(env, filename=options.recordfile) vidrecorder.videoparams[0:2] = [1024, 768] vidrecorder.realtime = False with env: if options.stop or physics: #KLUDGE: need to do this for stability reasons, not sure why, probably can be fixed another way print "Stopping OpenRAVE simulation to load models and configure..." env.StopSimulation() if type(options.scenefile) is list: for n in options.scenefile: env.Load(n) elif type(options.scenefile) is str: env.Load(options.scenefile) #This method ensures that the URI is preserved in the robot if options.robotfile is not None: robot = env.ReadRobotURI(options.robotfile) env.Add(robot) else: robot = env.GetRobots()[0] _rave.raveLogWarn("Assuming robot is {} (id=0)...".format( robot.GetName())) robot.SetDOFValues(zeros(robot.GetDOF())) #Legacy command mode if options.physicsfile == True: options.physicsfile = 'physics.xml' with env: if physics and not check_physics(env): _rave.raveLogInfo('Loading physics parameters from "{}"'.format( options.physicsfile)) env.Load(options.physicsfile) elif not physics: env.SetPhysicsEngine( _rave.RaveCreatePhysicsEngine(env, 'GenericPhysicsEngine')) else: #TODO: find a more efficient way to avoid double creation? _rave.raveLogInfo( "Physics engine already configured, overwriting...") env.Load(options.physicsfile) if check_physics(env): _rave.raveLogInfo('Creating controller for physics simulation') controller = _rave.RaveCreateController(env, 'trajectorycontroller') robot.SetController(controller) controller.SendCommand('set gains 150 0 .9 ') controller.SendCommand('set radians ') else: #Just load ideal controller if physics engine is not present _rave.raveLogInfo( 'Physics engine not loaded, using idealcontroller...') controller = _rave.RaveCreateController(env, 'idealcontroller') robot.SetController(controller) if options.ghost and options.robotfile: ghost_robot = load_ghost(env, options.robotfile, prefix="ghost_") if check_physics(env): controller.SendCommand("set visrobot " + ghost_robot.GetName()) else: ghost_robot = None collisionChecker = _rave.RaveCreateCollisionChecker(env, 'pqp') if collisionChecker == None: collisionChecker = _rave.RaveCreateCollisionChecker(env, 'ode') _rave.raveLogWarn( 'Using ODE collision checker since PQP is not available...') env.SetCollisionChecker(collisionChecker) ind = make_name_to_index_converter(robot) if options.atheight is not None: align_robot(robot, options.atheight) if ghost_robot: align_robot(ghost_robot, options.atheight) #FIXME: better way to do this? global TIMESTEP TIMESTEP = get_timestep(env) return (robot, controller, ind, ghost_robot, vidrecorder)
def get_occluding_objects(good_bodies, robot, object_to_grasp, max_trials = 0, just_one_attempt = False, return_pose = False, body_filter = None ): """Generates a list of all the objects that prevent the robot from reaching a target object. Several (up to max_trials) attempts are performed to grasp Parameters: robot: a openravepy.Robot instance object_to_grasp: a openravepy.KinBody instance representing the object to grasp Returns: a list of sets of objects """ if just_one_attempt != return_pose: raise ValueError("If asking for a return poes then set just_one_attempt to True") env = robot.GetEnv() robot_pose = robot.GetTransform() manip = robot.GetActiveManipulator() ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot,iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() min_torso, max_torso = utils.get_pr2_torso_limit(robot) num_trial = 0 collisions_list = [] with robot: while num_trial < max_trials: num_trial +=1 # sample a base pose torso_angle = generate_reaching_poses.move_random_torso(robot, min_torso, max_torso) robot_pose = generate_reaching_poses.generate_random_pos(robot, object_to_grasp) robot.SetTransform(robot_pose) report = openravepy.CollisionReport() collision = env.CheckCollision(robot, report=report) if not collision: openravepy.raveLogInfo("Got a position not in collision") #sample a gripper pose grasping_poses = generate_reaching_poses.generate_grasping_pose(robot, object_to_grasp, use_general_grasps = True, checkik=False) openravepy.raveLogInfo("Got %d grasping poses" % len(grasping_poses)) #check if gripper pose is reachable from base pose #use robot's base pose to transform precomputed #gripper poses into the robot's frame of reference sol, collisions = generate_reaching_poses.check_reachable(good_bodies, env, object_to_grasp, manip, grasping_poses, only_reachable = True) if sol is None: print "Finding collisions: trial {0} No sol from base pose to gripper pose".\ format(num_trial) goodCollisions = False if body_filter is not None: goodCollisions = filter(body_filter, collisions) print "good collisions: " + repr(goodCollisions) print "all collisions: " + repr(collisions) if sol is not None and goodCollisions: print "Sol from base pose to gripper pose found in trial {0}".\ format(num_trial) openravepy.raveLogInfo("Getting the list of collisions") with robot: #robot.SetDOFValues(sol, robot.GetActiveManipulator().GetArmIndices()); #collisions_list.append(utils.get_all_collisions(robot, env)) collisions_list.append(collisions) if just_one_attempt: if return_pose: return (robot_pose, sol, torso_angle, collisions_list) else: return collisions_list if num_trial == max_trials: print "Getting obj names: No gripper pose reachable from collision free base pose found", print "after {0} trials".format(num_trial) if return_pose: return (robot_pose, None, torso_angle, collisions_list) else: return collisions_list
def generate_poses(self, obj, use_general_grasps=True, approach_dist=0.15): """ Parameters: Returns: Returns a list of tuples (grasp_pose, pre_grasp_pose) Where grasp_pose and pre_grasp_pose are both 4x4 transformation matrices """ class _GraspOptions(object): def __init__(self): self.normalanglerange = 0.0 self.standoffs = [0] self.rolls = np.arange(0.5*np.pi, 2*np.pi, np.pi) self.boxdelta = 0.01 self.directiondelta = approach_dist gmodel = openravepy.databases.grasping.GraspingModel(self.robot, obj) if use_general_grasps: gmodel.grasps = self.pregenerated_grasps else: if not gmodel.load(): openravepy.raveLogInfo("Generating grasping model...") gmodel.autogenerate(_GraspOptions()) # only use horizontal grasps horiz_grasp_filter = lambda g: \ abs(gmodel.getGlobalApproachDir(g)[2]) < 0.01 gmodel.grasps = filter(horiz_grasp_filter, gmodel.grasps) # only use grasps in the upper half (+y is up) upper_grasp_filter = lambda g: \ gmodel.GetLocalGraspTransform(g, collisionfree=True)[1][3] > 0 gmodel.grasps = filter(upper_grasp_filter, gmodel.grasps) data = json.dumps([g.tolist() for g in gmodel.grasps]) with open(GRASPS_FILE_NAME, 'w') as outfile: outfile.write(data) openravepy.raveLogInfo("Generating grasps") validgrasps, _ = gmodel.computeValidGrasps(checkcollision=False, checkik=False, checkgrasper=False) np.random.shuffle(validgrasps) openravepy.raveLogInfo("Number of valid grasps: %d" % len(validgrasps)) grasp_pose_list = [] for grasp in validgrasps: gmodel.setPreshape(grasp) grasp_pose = gmodel.getGlobalGraspTransform(grasp, collisionfree=True) pre_grasp_pose = gmodel.getGlobalGraspTransform(grasp, collisionfree=True) approach = gmodel.getGlobalApproachDir(grasp) * approach_dist pre_grasp_pose[0][3] -= approach[0] pre_grasp_pose[1][3] -= approach[1] pre_grasp_pose[2][3] -= approach[2] grasp_pose_list.append((grasp_pose, pre_grasp_pose)) return grasp_pose_list
def main(batch_id=0, mode='all_manipulators', sample_env_num=1000, surface_source='flat_ground_environment'): ## initialization of new random environment env_handler = environment_handler() env = env_handler.env ## initialize the robot rave.raveLogInfo('Load and Initialize the Robot.') or_robot = load_escher.escher(env) robot = or_robot.robot DOFNameIndexDict = or_robot.DOFNameIndexDict DOFNameActiveIndexDict = or_robot.DOFNameActiveIndexDict lower_limits = or_robot.lower_limits higher_limits = or_robot.higher_limits IKInitDOFValues = or_robot.IKInitDOFValues step_transition_model = or_robot.step_transition_model hand_transition_model = or_robot.hand_transition_model resolution = map_grid_resolution max_x = 2.0 min_x = -2.0 max_y = 2.0 min_y = -2.0 # construct possible next contact list (possible_next_transition_given_torso, possible_next_step ) = get_possible_transitions_and_steps(step_transition_model) # initialize the IK solver interface general_ik_interface = CBiRRT(env, or_robot.robot.GetName()) # Initialize Escher C++ interface traversability_cpp = traversability_cpp_utility_wrapper(env) try: print('Load footstep_window...') out_file = open(planning_data_path + 'footstep_window', 'r') footstep_windows = pickle.load(out_file) out_file.close() print('Finished loading.') # IPython.embed() except Exception: raw_input('ERROR: Footstep window file not found.') return # setup collision checking kinbodies collision_box_list_dict = { 'l_leg': [or_robot.foot_collision_box_1, or_robot.foot_collision_box_2], 'r_leg': [or_robot.foot_collision_box_1, or_robot.foot_collision_box_2], 'l_arm': [or_robot.hand_collision_box_1, or_robot.hand_collision_box_2], 'r_arm': [or_robot.hand_collision_box_1, or_robot.hand_collision_box_3] } sample_env_counter = 0 torso_pose_list = [(0, 0, 0), (0, 0, 30), (0, 0, 60)] query_torso_transition = set() goal_index_dict = {} recording_data = [] while sample_env_counter < sample_env_num: print('Environment Number: %d.' % (sample_env_counter)) # Initialize the new sampled environment env_handler.update_environment(or_robot, surface_source=surface_source) structures = env_handler.structures structures_dict = {} for struct in structures: structures_dict[struct.id] = struct # Compute the map grid torso_pose_grid = map_grid(resolution, structures, or_robot, [min_x, max_x, min_y, max_y]) goal_num = len(torso_pose_grid.chest_neighbor_window[0]) + len( torso_pose_grid.chest_neighbor_window[30]) + len( torso_pose_grid.chest_neighbor_window[60]) env_transition_feature_vector = [0] * goal_num before_getting_feature_time = time.time() feasible_stance = get_feasible_step_combinations( or_robot, general_ik_interface, structures, structures_dict, torso_pose_list, torso_pose_grid, step_transition_model, collision_box_list_dict) #################################################################################################### # get the mapping from torso pose transition to environment feature: env_transition_feature_vector # #################################################################################################### if not goal_index_dict: index = 0 for torso_pose in torso_pose_list: theta1 = torso_pose[2] (ix1, iy1, itheta1) = torso_pose_grid.position_to_grid(torso_pose) for neighbor in torso_pose_grid.chest_neighbor_window[theta1]: from_cell = torso_pose_grid.cell_3D_list[ix1][iy1][itheta1] to_cell = torso_pose_grid.cell_3D_list[ix1 + neighbor[0]][ iy1 + neighbor[1]][itheta1] query_torso_transition.add( (ix1, iy1, itheta1, ix1 + neighbor[0], iy1 + neighbor[1])) goal_index_dict[(neighbor[0] * torso_pose_grid.resolution, neighbor[1] * torso_pose_grid.resolution, theta1)] = index index = index + 1 query_torso_transition = list(query_torso_transition) # store goal_index_dict print('Storing goal_index_dict...') out_file = open('goal_index_dict', 'w') pickle.dump(goal_index_dict, out_file) out_file.close() print('Storing finished.') (footstep_transition_traversability_legs_only,footstep_transition_traverasbility,hand_transition_traversability) = \ traversability_cpp.SendStartCalculatingTraversabilityCommand(structures, footstep_windows, footstep_windows, query_torso_transition, foot_contact_point_resolution, torso_pose_grid, hand_transition_model) index = 0 for torso_pose in torso_pose_list: theta1 = torso_pose[2] (ix1, iy1, itheta1) = torso_pose_grid.position_to_grid(torso_pose) for neighbor in torso_pose_grid.chest_neighbor_window[theta1]: from_cell = torso_pose_grid.cell_3D_list[ix1][iy1][itheta1] to_cell = torso_pose_grid.cell_3D_list[ix1 + neighbor[0]][ iy1 + neighbor[1]][itheta1] if from_cell.g != 9999.0 and from_cell.g != 4999.0 and to_cell.g != 9999.0 and to_cell.g != 4999.0: hand_traversability = hand_transition_traversability[( ix1, iy1, itheta1)] footstep_traversability = footstep_transition_traverasbility[ (ix1, iy1, itheta1, ix1 + neighbor[0], iy1 + neighbor[1])][0] env_transition_feature_vector[index] = [ footstep_traversability, hand_traversability[0], hand_traversability[1], hand_traversability[2], hand_traversability[3] ] else: env_transition_feature_vector[index] = [0, 0, 0, 0, 0] index = index + 1 #################################################################################################### after_getting_feature_time = time.time() print('Total time for checking ground mapping: %5.5f' % (after_getting_feature_time - before_getting_feature_time)) successful_goal_count = [0] * goal_num # planning_result = [[False]*goal_num] * len(feasible_stance) initial_state_projection_numerical_problem = False for j, stance in enumerate(feasible_stance): print( 'New Stance No: %d: (%5.3f,%5.3f,%5.1f) (%5.3f,%5.3f,%5.1f)' % (j, stance[2][0], stance[2][1], stance[2][5], stance[3][0], stance[3][1], stance[3][5])) # sample the initial stance init_left_leg = stance[0] init_right_leg = stance[1] pre_proj_left_leg = stance[2] pre_proj_right_leg = stance[3] torso_yaw = stance[4] goal_list = torso_pose_grid.chest_neighbor_window[torso_yaw] # initialize the node init_left_arm = [-99.0, -99.0, -99.0, -99.0, -99.0, -99.0] init_right_arm = [-99.0, -99.0, -99.0, -99.0, -99.0, -99.0] init_manipulation_objects_configuration = None init_manipulating_objects = None initial_node = node(init_left_leg, init_right_leg, init_left_arm, init_right_arm, 0, 0, None, None) if not ground_mapping(or_robot, initial_node, structures_dict, torso_pose_grid): initial_state_projection_numerical_problem = True break ## plan footstep to different goal cell to see the result for k, goal in enumerate(goal_list): goal_x = torso_pose_grid.resolution * goal[0] goal_y = torso_pose_grid.resolution * goal[1] goal_theta = torso_yaw goal_index = goal_index_dict[(goal_x, goal_y, torso_yaw)] feature = env_transition_feature_vector[goal_index] print( 'New Goal: %5.3f,%5.3f,%5.3f. Feature: (%5.3f,%5.3f,%5.3f,%5.3f,%5.3f).' % (goal_x, goal_y, goal_theta, feature[0], feature[1], feature[2], feature[3], feature[4])) if feature[0] == 0 or (mode != 'legs_only' and feature[1] == 0 and feature[2] == 0 and feature[3] == 0 and feature[4] == 0): continue initial_node_copy = copy.deepcopy(initial_node) initial_node_copy.h = h_estimation( initial_node_copy.left_leg, initial_node_copy.right_leg, initial_node_copy.left_arm, initial_node_copy.right_arm, or_robot, torso_pose_grid, motion_mode=traversability_regressor_mode_name_index_dict[ mode], goal=(goal_x, goal_y, goal_theta)) initial_node_copy.parent = None # need to specify motion mode path = ANA_Star( general_ik_interface, or_robot, structures, torso_pose_grid, goal=(goal_x, goal_y, goal_theta), initial_node=initial_node_copy, step_transition_model=step_transition_model, hand_transition_model=hand_transition_model, motion_mode=traversability_regressor_mode_name_index_dict[ mode], planning_time_limit=-1.0, collect_training_data=True) if len(path) != 0: successful_goal_count[ goal_index] = successful_goal_count[goal_index] + 1 # planning_result[j][goal_index] = True if len(feasible_stance ) != 0 and not initial_state_projection_numerical_problem: # store the data recording_data.append( (env_transition_feature_vector, successful_goal_count)) sample_env_counter = sample_env_counter + 1 if sample_env_counter % 10 == 0 or sample_env_counter == sample_env_num: print('Dumping traversability_training_data...') out_file = open( 'traversability_training_data_' + str(batch_id), 'w') pickle.dump(recording_data, out_file) out_file.close() print('Dumping finished.') IPython.embed()
if __name__ == "__main__": batch_id = 0 mode = 'all_manipulators' sample_env_num = 1000 i = 1 while i < len(sys.argv): command = sys.argv[i] i += 1 if command == 'batch_id': batch_id = sys.argv[i] elif command == 'mode': mode = sys.argv[i] if mode not in traversability_regressor_mode_list: rave.raveLogInfo('Unknown motion mode: %s. Abort.' % (mode)) sys.exit() elif command == 'sample_env_num': sample_env_num = int(sys.argv[i]) elif command == 'surface_source': surface_source = sys.argv[i] else: rave.raveLogInfo('Invalid command: %s. Abort.' % (command)) sys.exit() i += 1 main(batch_id, mode, sample_env_num, surface_source)
def main(env,options): "Main example code." if options.iktype is not None: # cannot use .names due to python 2.5 (or is it boost version?) for value,type in IkParameterization.Type.values.items(): if type.name.lower() == options.iktype.lower(): iktype = type break else: iktype = IkParameterizationType.Transform6D env.Load(options.scene) with env: # load the Transform6D IK models ikmodels = [] for robot in env.GetRobots(): for manip in robot.GetManipulators(): print(manip) try: robot.SetActiveManipulator(manip) ikmodel = databases.inversekinematics.InverseKinematicsModel(robot, iktype=iktype) if not ikmodel.load(): ikmodel.autogenerate() if not ikmodel.has(): continue ikmodels.append(ikmodel) except Exception as e: print('failed manip %s %r'%(manip, e)) if len(ikmodels) == 0: raveLogWarn('no manipulators found that can be loaded with iktype %s'%str(iktype)) # create the pickers pickers = [] for ikmodel in ikmodels: raveLogInfo('creating picker for %s'%str(ikmodel.manip)) picker = RaveCreateKinBody(env,'') picker.InitFromBoxes(array([ [0.05,0,0,0.05,0.01,0.01], [0,0.05,0,0.01,0.05,0.01], [0,0,0.05,0.01,0.01,0.05] ]),True) for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()): color = zeros(3) color[igeom] = 1 geom.SetDiffuseColor(color) picker.SetName(ikmodel.manip.GetName()) env.Add(picker,True) # have to disable since not part of collision picker.Enable(False) picker.SetTransform(ikmodel.manip.GetTransform()) pickers.append([picker,ikmodel.manip]) raveLogInfo('pickers loaded, try moving them') def PickerThread(env,pickers,iktype): """this function runs in a separate thread monitoring each of the pickers """ Tpickers = [None]*len(pickers) while True: if env.GetViewer() is None: break with env: for ipicker,(picker,manip) in enumerate(pickers): T=picker.GetTransform() if Tpickers[ipicker] is not None and sum(abs(Tpickers[ipicker]-T).flatten()) <= 1e-10: continue data = None if iktype == IkParameterizationType.Direction3D: data = T[0:3,2] elif iktype == IkParameterizationType.Lookat3D: data = T[0:3,3] elif iktype == IkParameterizationType.Ray4D: data = Ray(T[0:3,3],T[0:3,2]) elif iktype == IkParameterizationType.Rotation3D: data = T[0:3,0:3] elif iktype == IkParameterizationType.Transform6D: data = T elif iktype == IkParameterizationType.Translation3D: data = T[0:3,3] elif iktype == IkParameterizationType.TranslationDirection5D: ikparam = Ray(T[0:3,3],T[0:3,2]) elif iktype == IkParameterizationType.TranslationLocalGlobal6D: ikparam = [T[0:3,3],zeros(3)] else: raveLogWarn('iktype %s unsupported'%str(iktype)) continue sol = manip.FindIKSolution(IkParameterization(data,iktype),IkFilterOptions.CheckEnvCollisions) if sol is not None: robot.SetDOFValues(sol,manip.GetArmIndices()) # have to update all other pickers since two manipulators can be connected to the same joints (torso) for ipicker2, (picker2,manip2) in enumerate(pickers): Tpickers[ipicker2] = manip2.GetTransform() picker2.SetTransform(manip2.GetTransform()) # changing color produces bugs with qtcoin # for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()): # color = zeros(3) # color[igeom] = 0.4 if sol is None else 1.0 # geom.SetDiffuseColor(color) Tpickers[ipicker] = T # update rate of 0.05s time.sleep(0.05) # create the thread t = threading.Thread(target=PickerThread,args=[env,pickers,iktype]) t.start() while True: t.join(1)
for ikmodel in ikmodels: raveLogInfo('creating picker for %s'%str(ikmodel.manip)) picker = RaveCreateKinBody(env,'') picker.InitFromBoxes(array([ [0.05,0,0,0.05,0.01,0.01], [0,0.05,0,0.01,0.05,0.01], [0,0,0.05,0.01,0.01,0.05] ]),True) for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()): color = zeros(3) color[igeom] = 1 geom.SetDiffuseColor(color) picker.SetName(ikmodel.manip.GetName()) env.Add(picker,True) # have to disable since not part of collision picker.Enable(False) picker.SetTransform(ikmodel.manip.GetTransform()) pickers.append([picker,ikmodel.manip]) raveLogInfo('pickers loaded, try moving them') def PickerThread(env,pickers,iktype): """this function runs in a separate thread monitoring each of the pickers """ Tpickers = [None]*len(pickers) while True: if env.GetViewer() is None: break with env: for ipicker,(picker,manip) in enumerate(pickers): T=picker.GetTransform() if Tpickers[ipicker] is not None and sum(abs(Tpickers[ipicker]-T).flatten()) <= 1e-10: continue data = None if iktype == IkParameterizationType.Direction3D: data = T[0:3,2]
def main(): ### Initialize the environment handler rave.raveLogInfo('Load the Environment Handler.') env_handler = environment_handler() env = env_handler.env # load and initialize the robot rave.raveLogInfo('Load and Initrialize the Robot.') escher = load_athena.athena(env) # escher = load_escher.escher(env) escher.body_collision_box.SetTransform(np.eye(4, dtype=float)) escher.move_body_to_collision_box_transform(np.eye(4, dtype=float)) env_handler.DrawOrientation(np.eye(4, dtype=float), size=0.5) escher.body_collision_box.SetTransform([[1, 0, 0, 100], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) # escher.robot.SetDOFValues(np.zeros(escher.robot.GetDOF())) # initialize the IK solver for each manipulator escher.robot.SetActiveManipulator('l_leg') ikmodel = rave.databases.inversekinematics.InverseKinematicsModel( escher.robot, iktype=rave.IkParameterizationType.Transform6D) # ikmodel = rave.databases.inversekinematics.InverseKinematicsModel(escher.robot, iktype = rave.IkParameterizationType.Translation3D) if not ikmodel.load(): ikmodel.autogenerate() manip = escher.robot.GetManipulator('l_leg') counter = 0 total_time = 0 with env: while (counter < 10000): sampled_config = sample_configuration(escher.robot, 'l_leg') dof_values = escher.robot.GetDOFValues() for i in range(manip.GetArmDOF()): dof_values[manip.GetArmIndices()[i]] = sampled_config[i] escher.robot.SetDOFValues(dof_values) # ikparam = IkParameterization(manip.GetTransform()[0:3,3],ikmodel.iktype) start = time.time() config = manip.FindIKSolution( manip.GetTransform(), rave.IkFilterOptions.IgnoreSelfCollisions) end = time.time() total_time += (end - start) if config is not None: print(config) print('oh~~~~~') IPython.embed() counter += 1 print(total_time * 1000 / 10000) IPython.embed() # for ix in range(-100,100): # for iy in range(-100,100): # for iz in range(-100,100): # target_pose = np.array([[1,0,0,ix*0.01],[0,1,0,iy*0.01],[0,0,1,iz*0.01],[0,0,0,1]]) # config = manip.FindIKSolution(target_pose, rave.IkFilterOptions.IgnoreSelfCollisions) # if config is not None: # IPython.embed() IPython.embed()
def Generate(self, robot, num_batches, **kw_args): params = self.defaults._replace(max_batches=num_batches, **kw_args) # lock env, save robot state # and create family with robot, type(self).AddedFamilyModule(self.env, robot) as family: setcache_path = self.get_setcache_path(family, params.roadmap, read_only=False) # disable any link that moves due to non-active dofs openravepy.raveLogInfo('[Generate] Computing self-checked set cache for these links:') for linkindex,link in enumerate(robot.GetLinks()): inactive_affected = False for dofindex in range(robot.GetDOF()): if dofindex in robot.GetActiveDOFIndices(): continue joint = robot.GetJointFromDOFIndex(dofindex) jointindex = robot.GetJoints().index(joint) if robot.DoesAffect(jointindex, linkindex): inactive_affected = True if inactive_affected: link.Enable(False) else: openravepy.raveLogInfo('[Generate] [{}] {} ({} geoms)'.format( linkindex, link.GetName(), len(link.GetGeometries()))) # save self-check family.SendCommand('Let Self = $live') openravepy.raveLogInfo('[Generate] Current family:') family.SendCommand('PrintCurrentFamily') self_header = family.SendCommand('GetHeaderFromSet Self') openravepy.raveLogInfo('[Generate] Self header:') for line in self_header.rstrip('\n').split('\n'): openravepy.raveLogInfo('[Generate] {}'.format(line)) # create a planner planner = openravepy.RaveCreatePlanner(self.env, 'FamilyPlanner') if planner is None: raise prpy.planning.base.UnsupportedPlanningError('Unable to create FamilyPlanner planner.') # create params orparams = openravepy.Planner.PlannerParameters() orparams.SetRobotActiveJoints(robot) orparams.SetInitialConfig([]) orparams.SetGoalConfig([]) xml = self.xml_from_params(params) xml.append('<solve_all>true</solve_all>') xml.append('<family_module>{}</family_module>'.format(family.SendCommand('GetInstanceId'))) xml.append('<family_setcaches><setcache><name>Self</name><filename>{}</filename></setcache></family_setcaches>'.format(setcache_path)) orparams.SetExtraParameters('\n'.join(xml)) success = planner.InitPlan(robot, orparams) if not success: raise RuntimeError('InitPlan failed!') result = planner.PlanPath(None) if result != openravepy.PlannerStatus.HasSolution: raise RuntimeError('Planning failed!') # ensure directories exist # from http://stackoverflow.com/a/5032238/5228520 try: os.makedirs(os.path.dirname(setcache_path)) except OSError as exception: if exception.errno != errno.EEXIST: raise openravepy.raveLogInfo('[Generate] Saving set cache ...') planner.SendCommand('SaveSetCaches') # return dummy trajectory traj = openravepy.RaveCreateTrajectory(self.env, '') traj.Init(robot.GetActiveConfigurationSpecification()) return traj