Ejemplo n.º 1
0
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
Ejemplo n.º 3
0
        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)
Ejemplo n.º 4
0
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])
Ejemplo n.º 5
0
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
Ejemplo n.º 7
0
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]
Ejemplo n.º 8
0
        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)
Ejemplo n.º 9
0
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
Ejemplo n.º 10
0
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]
Ejemplo n.º 12
0
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]
Ejemplo n.º 15
0
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)
Ejemplo n.º 16
0
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)
Ejemplo n.º 20
0
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)
Ejemplo n.º 21
0
     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]
Ejemplo n.º 22
0
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()
Ejemplo n.º 23
0
   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