Ejemplo n.º 1
0
def load_scene(env, scenename=None, stop=False, physics=True):
    """ Load a robot model into the given environment, configuring a
    trajectorycontroller and a reference robot to show desired movements vs. actual
    pose. The returned tuple contains:
        robot: handle to the created robot
        controller: either trajectorycontroller or idealcontroller depending on physics
        name-to-joint-index converter
        ref_robot: handle to visiualization "ghost" robot
        recorder: video recorder python class for quick video dumps
    """

    # Set the robot controller and start the simulation
    recorder = viewerrecorder(env)
    #Default to "sim-timed video" i.e. plays back much faster
    recorder.videoparams[0:2] = [1024, 768]
    recorder.realtime = False

    with env:
        if stop:
            env.StopSimulation()

        if type(scenename) is list:
            for n in scenename:
                loaded = env.Load(n)
        elif type(scenename) is str:
            loaded = env.Load(scenename)

    time.sleep(1)
    #Explicitly disable physics if option is selected
    with env:
        if not physics:
            env.SetPhysicsEngine(
                rave.RaveCreatePhysicsEngine(env, 'GenericPhysicsEngine'))
        robot = env.GetRobots()[0]
        pose = ones(robot.GetDOF()) * .6
        robot.SetDOFValues(pose)
        collisionChecker = rave.RaveCreateCollisionChecker(env, 'pqp')
        if collisionChecker == None:
            collisionChecker = rave.RaveCreateCollisionChecker(env, 'ode')
            print 'Note: Using ODE collision checker since PQP is not available'
        env.SetCollisionChecker(collisionChecker)

        if env.GetPhysicsEngine().GetXMLId(
        ) != 'GenericPhysicsEngine' and physics:
            controller = rave.RaveCreateController(env, 'servocontroller')
            robot.SetController(controller)
            controller.SendCommand('set gains 100 0 0')
            controller.SetDesired(pose / pose * pi / 4 * 1.1)

    time.sleep(.5)
    ind = makeNameToIndexConverter(robot)

    return (robot, controller, ind, recorder)
Ejemplo n.º 2
0
def load_scene(env,scenename=None,stop=False,physics=True):
    """ Load a robot model into the given environment, configuring a
    trajectorycontroller and a reference robot to show desired movements vs. actual
    pose. The returned tuple contains:
        robot: handle to the created robot
        controller: either trajectorycontroller or idealcontroller depending on physics
        name-to-joint-index converter
        ref_robot: handle to visiualization "ghost" robot
        recorder: video recorder python class for quick video dumps
    """

    # Set the robot controller and start the simulation
    recorder=viewerrecorder(env)
    #Default to "sim-timed video" i.e. plays back much faster
    recorder.videoparams[0:2]=[1024,768]
    recorder.realtime=False

    with env:
        if stop:
            env.StopSimulation()

        if type(scenename) is list:
            for n in scenename:
                loaded=env.Load(n)
        elif type(scenename) is str:
            loaded=env.Load(scenename)

    time.sleep(1)
    #Explicitly disable physics if option is selected
    with env:
        if not physics:
            env.SetPhysicsEngine(rave.RaveCreatePhysicsEngine(env,'GenericPhysicsEngine'))
        robot = env.GetRobots()[0]
        pose=ones(robot.GetDOF())*.6
        robot.SetDOFValues(pose)
        collisionChecker = rave.RaveCreateCollisionChecker(env,'pqp')
        if collisionChecker==None:
            collisionChecker = rave.RaveCreateCollisionChecker(env,'ode')
            print 'Note: Using ODE collision checker since PQP is not available'
        env.SetCollisionChecker(collisionChecker)

        if env.GetPhysicsEngine().GetXMLId()!='GenericPhysicsEngine' and physics:
            controller=rave.RaveCreateController(env,'servocontroller')
            robot.SetController(controller)
            controller.SendCommand('set gains 100 0 0')
            controller.SetDesired(pose/pose*pi/4*1.1)
    
    time.sleep(.5)
    ind=makeNameToIndexConverter(robot)

    return (robot,controller,ind,recorder)
Ejemplo n.º 3
0
    waypt0 = list(pose0)
    waypt1 = list(pose1)

    waypt0.extend(zeros(7))
    waypt1.extend(zeros(7))

    waypt0.append(t0)
    waypt1.append(t1)
    waypt2 = copy(waypt0)
    waypt2[-1] = t1

    traj.Insert(0, waypt0)
    traj.Insert(1, waypt1)
    traj.Insert(2, waypt2)

    planningutils.RetimeActiveDOFTrajectory(traj, robot, True)

    # Prove that the retiming actually works
    for k in range(40):
        data = traj.Sample(float(k) / 10)
        print data[ind("LKP")]

    vidrec = viewerrecorder(env)
    controller.SetPath(traj)
    vidrec.start()
    controller.SendCommand("start")
    while not (controller.IsDone()):
        time.sleep(0.1)
    vidrec.stop()
Ejemplo n.º 4
0
    waypt0 = list(pose0)
    waypt1 = list(pose1)

    waypt0.extend(zeros(7))
    waypt1.extend(zeros(7))

    waypt0.append(t0)
    waypt1.append(t1)
    waypt2 = copy(waypt0)
    waypt2[-1] = t1

    traj.Insert(0, waypt0)
    traj.Insert(1, waypt1)
    traj.Insert(2, waypt2)

    planningutils.RetimeActiveDOFTrajectory(traj, robot, True)

    #Prove that the retiming actually works
    for k in range(40):
        data = traj.Sample(float(k) / 10)
        print data[ind('LKP')]

    vidrec = viewerrecorder(env)
    controller.SetPath(traj)
    vidrec.start()
    controller.SendCommand('run')
    while not (controller.IsDone()):
        time.sleep(.1)
    vidrec.stop()