planar_openrave.py

Go to the documentation of this file.

#!/usr/bin/env python

import numpy as np, math
import sys
from openravepy import *

import roslib; roslib.load_manifest('hrl_motion_planners_darpa_m3')
import rospy
import hrl_lib.util as ut
import hrl_motion_planners_darpa_m3.write_xml_openrave as w_xml


def move_bodies_to_position(bodies, positions):
    for i in range(len(positions)):
        pos = positions[i]
        bod = bodies[i]
        t = bod.GetTransform()
        if np.max(np.abs(pos)) > 1.5:
            continue
        t[0,3] = pos[0]
        t[1,3] = pos[1]
        t[2,3] = pos[2]
        bod.SetTransform(t)


# We have set things up so that we need two xml files to run OpenRAVE,
# one defines the robot, and gets included in the second which defines
# the environment.
#
# We generate the robot's description from a Python file that also
# gets used by the ODE software simulation to ensure that things are
# consistent.
#
# reach_problem_dict - generated by obstacles.py (defines obstacle
#                                       dimensions and positions.)
#
# joints, links - dictionaries that define the robot.
def write_openrave_xml_files(reach_problem_dict, joints, links):
    w_xml.write_linkage_xml_file(joints, links, 'sim.robot.xml')
    w_xml.write_environment_xml_file(reach_problem_dict, 'planar.env.xml', 'sim.robot.xml')


def position_obstacles(env, movable_positions, fixed_positions, ignore_movable):
    # move the obstacles to correct places.
    bodies_list = env.GetBodies()

    if not ignore_movable:
        movable_bodies = filter(lambda x: 'movable' in x.GetName(), bodies_list)
        move_bodies_to_position(movable_bodies, movable_positions)

    fixed_bodies = filter(lambda x: 'fixed' in x.GetName(), bodies_list)
    move_bodies_to_position(fixed_bodies, fixed_positions)


# goal: [x, y, z]
# init_arm_config - list of joint angles.
# planner_name = 'BasicRRT', 'BiRRT' etc.
def plan_path_to_goal(goal, env, init_arm_config, quiet,
                      planner_name):
    robot = env.GetRobots()[0] # get the first robot
    ikmodel=databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.TranslationXY2D)
    if not ikmodel.load():
        ikmodel.autogenerate()

    robot.SetDOFValues(init_arm_config, range(len(init_arm_config)))
    manipprob = interfaces.BaseManipulation(robot, plannername=planner_name) 

    if not quiet:
        bodies_list = env.GetBodies()
        goal[2] = 0.1 # this is so that arm not in collision with goal
        t_body = filter(lambda x: 'target' in x.GetName(), bodies_list)[0]
        move_bodies_to_position([t_body], [goal])
        raw_input('Hit ENTER to continue.')

    solutions = ikmodel.manip.FindIKSolutions(IkParameterization(goal[0:2], IkParameterization.Type.TranslationXY2D), IkFilterOptions.CheckEnvCollisions)

    if solutions is None:
        solutions = []

    print 'Number of Solutions:', len(solutions)
    reached = False

    for s in solutions:
        try:
            res = manipprob.MoveManipulator(goal = s)
            robot.WaitForController(0) 
            reached = True
            break
        except planning_error, e:
            print e

    if not quiet:
        raw_input('Hit ENTER to continue.')

    q = robot.GetDOFValues().tolist()
    result_dict = {}
    result_dict['final_q'] = q
    if reached == True:
        result_dict['result'] = "Reached"
    else:
        result_dict['result'] = "Failed"

    return result_dict


def setup_openrave(reach_problem_dict, robot_defn, ignore_movable, quiet):
    write_openrave_xml_files(reach_problem_dict, robot_defn.b_jts, robot_defn.bodies)

    env = Environment()
    env.Load('planar.env.xml') # load a scene
    if not quiet:
        env.SetViewer('qtcoin') # start the viewer

    d = reach_problem_dict
    movable_positions = d['moveable_position'][0:d['num_move_used']]
    fixed_positions = d['fixed_position'][0:d['num_fixed_used']]
    position_obstacles(env, movable_positions, fixed_positions, ignore_movable)
    return env


def plan_with_stopping_dist_from_goal(dist, goal, env,
                                      init_arm_config, quiet,
                                      planner_name, n_rad, n_theta):
#    n_rad = 1
    d_dist = dist / n_rad
#    n_theta = 64
    d_theta = np.radians(360./n_theta)

    for r in range(n_rad):
        for i in range(n_theta):
            dx = (dist - d_dist * r) * math.cos(i * d_theta)
            dy = dist * math.sin(i * d_theta)
            g = [goal[0]+dx, goal[1]+dy, goal[2]]

            res_dict = plan_path_to_goal(g, env, init_arm_config, quiet,
                                         planner_name)

            if res_dict['result'] == 'Reached':
                break

    return res_dict



if __name__ == '__main__':
    import optparse
    p = optparse.OptionParser()

    p.add_option('--reach_problem_dict', '--rpd', action='store',
                 dest='rpd', type='string', default=None,
                 help='reach problem dict pkl')

    p.add_option('--linkage', action='store', dest='linkage',
                 default='', help='string that specifies the linkage to use')

    p.add_option('--quiet', '-q', action='store_true', dest='quiet',
                 help='do not bring up the gui.')

    p.add_option('--ignore_movable', '--im', action='store_true', dest='im',
                 help='do not use movable obstacles.')

    p.add_option('--planner', action='store', dest='planner',
                 help='BasicRRT, BiRRT etc.')

    p.add_option('--safety_margin', action='store',
                 dest='safety_margin', type='float', default=0.,
                 help='Safety margin added to radius of all obstacles')

    opt, args = p.parse_args()

    # load up the Python files that contain the robot definition.
    if opt.linkage == '':
        raise RuntimeError('Please specify a --linkage')

    elif opt.linkage == 'three_link_planar':
        import hrl_common_code_darpa_m3.robot_config.three_link_planar_capsule as robot_defn

    elif opt.linkage == 'six_link_planar':
        import hrl_common_code_darpa_m3.robot_config.six_link_planar as robot_defn

    else:
        print 'Unrecognized linkage: %s'%opt.linkage
        print 'Options are: three_link_planar, six_link_planar'
        print "Exiting ..."
        sys.exit()
    
    rpd = ut.load_pickle(opt.rpd)

    for dim in rpd['moveable_dimen']:
        dim[0] += opt.safety_margin
        dim[1] += opt.safety_margin

    for dim in rpd['fixed_dimen']:
        dim[0] += opt.safety_margin
        dim[1] += opt.safety_margin

    env = setup_openrave(rpd, robot_defn, opt.im, opt.quiet)
    res_dict = plan_path_to_goal(rpd['goal'], env, robot_defn.b_jt_start,
                                 opt.quiet, opt.planner)

    #res_dict = plan_with_stopping_dist_from_goal(0.025, rpd['goal'],
    #                                     env, robot_defn.b_jt_start,
    #                                     opt.quiet, opt.planner)

    env.Destroy()
        
    print 'Result:', res_dict['result']