Esempi in Python per BarretWAM

Esempio n. 1

File: wam_mug_localize.py Progetto: jakebarnwell/PythonGenerator

beacons = [Beacon(np.mat(np.array([-1.3, 0.4, 2.0])).T)]
beacons.append(Beacon(np.mat(np.array([-0.0, 0.2, 1.0])).T))
#beacons = [Beacon(np.mat(np.array([0.0, 2.0, 2.0])).T)]
mug_pos = np.mat(np.array([0.0, 1.0, 0.8])).T
mug = env.GetKinBody('mug2')
mugTransform = mug.GetTransform()
mugTransform[0, 3] = mug_pos[0]
mugTransform[1, 3] = mug_pos[1]
mugTransform[2, 3] = mug_pos[2]
mug.SetTransform(mugTransform)

s = SuperRaveEnv(rave_env=env, beacons=beacons)
s.draw()

robot = env.GetRobots()[0]  # get the first robot
arm = BarretWAM(robot, env)
arm.attach_sensor(BeaconSensor3D(decay_coeff=25), lambda x: arm.traj_pos(x))

localizer = RaveLocalizerBot(arm, mug_pos)
localizer.attach_sensor(
    OpenRAVECamera(KK=np.mat(
        np.array([(640.0, 0.0, 320.0), (0.0, 480.0, 240.0), (0.0, 0.0,
                                                             1.0)]))),
    lambda x: localizer.camera_obj_state(x))
s.add_robot(localizer)

for attachedsensor in robot.GetAttachedSensors():
    if attachedsensor.GetSensor().Supports(Sensor.Type.Camera):
        attachedsensor.GetSensor().Configure(Sensor.ConfigureCommand.PowerOn)
        attachedsensor.GetSensor().Configure(
            Sensor.ConfigureCommand.RenderDataOn)

Esempio n. 2

File: wam_beacon_opt.py Progetto: jakebarnwell/PythonGenerator

        time.sleep(0.01)
"""

env = Environment()  # create the environment
env.SetViewer("qtcoin")  # start the viewer
env.Load(os.getcwd() + "/data/wam_scene.env.xml")  # load a scene
beacons = [Beacon(np.mat(np.array([-1.3, 0.4, 2.0])).T)]
beacons.append(Beacon(np.mat(np.array([-0.0, 0.2, 1.0])).T))
# beacons = [Beacon(np.mat(np.array([0.0, 2.0, 2.0])).T)]

s = SuperRaveEnv(rave_env=env, beacons=beacons)
s.draw()


robot = env.GetRobots()[0]  # get the first robot
arm = BarretWAM(robot, env)
arm.attach_sensor(BeaconSensor3D(decay_coeff=25), lambda x: arm.traj_pos(x))
s.add_robot(arm)

num_states = arm.NX
num_ctrls = arm.NU
num_measure = len(beacons)  # arg/make part of robot observe
Q = np.mat(np.diag([1e-7] * num_states))  # arg
# Q[2,2] = 1e-8 # Gets out of hand if noise in theta or phi
R = np.mat(np.diag([1e-5] * num_measure))  # arg
# R[3,3] = 5e-7

T = 20

x0 = np.array([-0.5] * arm.NX)
du = np.array([0.0, 0.1, -0.02, -0.05, 0.04, 0.02, 0.1])

Esempio n. 3

File: wam_ekf.py Progetto: jakebarnwell/PythonGenerator

        time.sleep(0.01)
"""

env = Environment() # create the environment
env.SetViewer('qtcoin') # start the viewer
env.Load(os.getcwd() + '/data/wam_scene.env.xml') # load a scene
beacons = [Beacon(np.mat(np.array([-1.3, 0.4, 2.0])).T)]
beacons.append(Beacon(np.mat(np.array([-0.0, 0.2, 1.0])).T))
#beacons = [Beacon(np.mat(np.array([0.0, 2.0, 2.0])).T)]

s = SuperRaveEnv(rave_env=env, beacons=beacons)
s.draw()


robot = env.GetRobots()[0] # get the first robot
arm = BarretWAM(robot,env)
arm.attach_sensor(BeaconSensor3D(decay_coeff=25), lambda x: arm.traj_pos(x))
s.add_robot(arm)

num_states = arm.NX
num_ctrls = arm.NU
num_measure = len(beacons) #arg/make part of robot observe
Q = np.mat(np.diag([1e-7]*num_states)) #arg
#Q[2,2] = 1e-8 # Gets out of hand if noise in theta or phi
R = np.mat(np.diag([1e-5]*num_measure)) #arg
#R[3,3] = 5e-7

T = 20

x0 = np.array([-0.5] * arm.NX)
du = np.array([0.0, 0.1, -0.02, -0.05, 0.04, 0.02, 0.1])

Esempio n. 4

    while not robot.GetController().IsDone():
        time.sleep(0.01)
"""

env = Environment()  # create the environment
env.SetViewer('qtcoin')  # start the viewer
env.Load(os.getcwd() + '/data/wam_scene.env.xml')  # load a scene
beacons = [Beacon(np.mat(np.array([-1.3, 0.4, 2.0])).T)]
beacons.append(Beacon(np.mat(np.array([-0.0, 0.2, 1.0])).T))
#beacons = [Beacon(np.mat(np.array([0.0, 2.0, 2.0])).T)]

s = SuperRaveEnv(rave_env=env, beacons=beacons)
s.draw()

robot = env.GetRobots()[0]  # get the first robot
arm = BarretWAM(robot, env)
arm.attach_sensor(BeaconSensor3D(decay_coeff=25), lambda x: arm.traj_pos(x))
s.add_robot(arm)

num_states = arm.NX
num_ctrls = arm.NU
num_measure = len(beacons)  #arg/make part of robot observe
Q = np.mat(np.diag([1e-7] * num_states))  #arg
#Q[2,2] = 1e-8 # Gets out of hand if noise in theta or phi
R = np.mat(np.diag([1e-5] * num_measure))  #arg
#R[3,3] = 5e-7

T = 20

x0 = np.array([-0.5] * arm.NX)
du = np.array([0.0, 0.1, -0.02, -0.05, 0.04, 0.02, 0.1])

Esempio n. 5

File: wam_demo_quadratize.py Progetto: jakebarnwell/PythonGenerator

    #diverge_cost = 0
    return 1.0 * numpy.linalg.norm(u_t)**2 + 0.1 * diverge_cost**2


"""
def waitrobot(robot):
    while not robot.GetController().IsDone():
        time.sleep(0.01)
"""

env = Environment()  # create the environment
env.SetViewer('qtcoin')  # start the viewer
env.Load(os.getcwd() + '/data/wam_scene.env.xml')  # load a scene
robot = env.GetRobots()[0]  # get the first robot

arm = BarretWAM(robot, env)

x0 = numpy.array([0.0] * arm.NX)
du = numpy.array([0.0] * arm.NU)
du = numpy.array([0.0, 0.1, -0.02, -0.05, 0.04, 0.02, 0.1])
du = numpy.mat(du)
#U_bar = np.mat(np.random.random_sample((car.NU, T-1)))/5

T = 20
X = numpy.mat(numpy.zeros((arm.NX, T)))
U = numpy.mat(numpy.zeros((arm.NU, T - 1)))
U = numpy.mat(numpy.random.random_sample((arm.NU, T - 1)) / 10)
U[:, 10:] = -2 * U[:, 10:]

for t in range(T - 1):
    #U[:,t] = du.T

Esempio n. 6

    while not robot.GetController().IsDone():
        time.sleep(0.01)
"""

env = Environment()  # create the environment
env.SetViewer('qtcoin')  # start the viewer
env.Load(os.getcwd() + '/data/wam_scene.env.xml')  # load a scene
beacons = [Beacon(np.mat(np.array([-1.3, 0.4, 2.0])).T)]
beacons.append(Beacon(np.mat(np.array([-0.0, 0.2, 1.0])).T))
#beacons = [Beacon(np.mat(np.array([0.0, 2.0, 2.0])).T)]

s = SuperRaveEnv(rave_env=env, beacons=beacons)
s.draw()

robot = env.GetRobots()[0]  # get the first robot
arm = BarretWAM(robot, env)
arm.attach_sensor(BeaconSensor3D(decay_coeff=25), lambda x: arm.traj_pos(x))
s.add_robot(arm)

num_states = arm.NX
num_ctrls = arm.NU
num_measure = len(beacons)  #arg/make part of robot observe
Q = np.mat(np.diag([1e-7] * num_states))  #arg
#Q[2,2] = 1e-8 # Gets out of hand if noise in theta or phi
R = np.mat(np.diag([1e-5] * num_measure))  #arg
#R[3,3] = 5e-7

T = 20

x0 = np.array([-0.5] * arm.NX)
du = np.array([0.0, 0.1, -0.02, -0.05, 0.04, 0.02, 0.1])

Esempio n. 7

File: wam_demo.py Progetto: jakebarnwell/PythonGenerator

import struct
import _Getch 


"""
def waitrobot(robot):
    while not robot.GetController().IsDone():
        time.sleep(0.01)
"""

env = Environment() # create the environment
env.SetViewer('qtcoin') # start the viewer
env.Load(os.getcwd() + '/data/wam_scene.env.xml') # load a scene
robot = env.GetRobots()[0] # get the first robot

arm = BarretWAM(robot,env)

x0 = numpy.array([0.0] * arm.NX)
du = numpy.array([0.0] * arm.NU) 
du = numpy.array([0.0, 0.1, -0.02, -0.05, 0.04, 0.02, 0.1])
du = numpy.mat(du)
#U_bar = np.mat(np.random.random_sample((car.NU, T-1)))/5


T = 20
X = numpy.mat(numpy.zeros((arm.NX, T)))
U = numpy.mat(numpy.zeros((arm.NU, T-1)))
U = numpy.mat(numpy.random.random_sample((arm.NU, T-1))/5) 
U[:,10:] = -2 * U[:,10:]

for t in range(T-1):