Python Beaconの例

コード例 #1

ファイル: wam_mug_localize.py プロジェクト: jakebarnwell/PythonGenerator

from transforms import unscented_transform
from openravepy import *
import numpy as np
import time
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
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)

コード例 #2

ファイル: car_ekf.py プロジェクト: jakebarnwell/PythonGenerator

up_path = os.path.abspath('..')
sys.path.append(up_path)
import numpy as np
import matplotlib.pyplot as plt
from sim_env import SimEnv2D, Beacon
from robot.car import SimpleCar
from sensors import BeaconSensor
from utils import mat2tuple
import random
from math import log
from numpy.random import multivariate_normal as mvn
from kalman_filter import ekf_update

# Set up environment #args

beacons = [Beacon(np.array([-1, -1])), Beacon(np.array([1, 1]))]
beacons = beacons[1:2]
s = SimEnv2D(bounds=[-2, 2, -2, 2], beacons=beacons)

x0 = np.array([-1, 1, -np.pi / 4, 0])
car = SimpleCar(x0)
car.attach_sensor(BeaconSensor(), lambda x: x[0:2])

s.add_robot(car)

# Number of timesteps
T = 20  #arg

# Dynamics and measurement noise
num_states = car.NX
num_ctrls = car.NU

コード例 #3

ファイル: car_animate.py プロジェクト: jakebarnwell/PythonGenerator

from sim_env import SimEnv2D, Beacon, CircularObstacle
from robots import SimpleCar
from sensors import BeaconSensor
from utils import mat2tuple
import random
from math import log
from numpy.random import multivariate_normal as mvn
from kalman_filter import ekf_update
from covar import cov2vec, vec2cov
from optimize import scp_solver_beliefs
from scipy.io import loadmat
import matplotlib.animation as animation

# Set up environment #args

beacons = [Beacon(np.array([0.3, 0.7])), Beacon(np.array([1, 0.7]))]
s = SimEnv2D(bounds=[-0.1, 1.3, -0.1, 1], beacons=beacons)

x0 = np.array([0, 0, np.pi / 4, 0])
car = SimpleCar(x0)
car.attach_sensor(BeaconSensor(decay_coeff=100), lambda x: x[0:2])

s.add_robot(car)

# Number of timesteps
T = 30  #arg

# Dynamics and measurement noise
num_states = car.NX
num_ctrls = car.NU
num_measure = len(beacons) + 1  #arg/make part of robot observe

コード例 #4

from sim_env import SimEnv2D, Beacon, CircularObstacle, RectangularObstacle
from robots import SimpleCar
from sensors import BeaconSensor
from utils import mat2tuple
import random
from math import log
from numpy.random import multivariate_normal as mvn
from kalman_filter import ekf_update
from covar import cov2vec, vec2cov
from optimize import value_iteration_solver
from scipy.io import loadmat

# Set up environment #args

beacons = [
    Beacon(np.array([0.2, 0.2])),
    Beacon(np.array([1.2, 0.5])),
    Beacon(np.array([0.2, 0.8]))
]
#obstacles = [RectangularObstacle(np.array([[0.75, 0.2], [0.75, 0.4], [0.85, 0.4], [0.85,0.2]], float).T),\
#             RectangularObstacle(np.array([[0.5,0.85], [1.15,0.85], [1.15,0.6], [0.5,0.6]], float).T)]
obstacles = []

s = SimEnv2D(bounds=[-0.1, 1.5, -0.1, 1], beacons=beacons, obstacles=obstacles)

x0 = np.array([0, 0.5, 0, 0])
car = SimpleCar(x0)
car.attach_sensor(BeaconSensor(decay_coeff=25), lambda x: x[0:2])

s.add_robot(car)

コード例 #5

ファイル: car_belief_space_obstacles2.py プロジェクト: jakebarnwell/PythonGenerator

import matplotlib.pyplot as plt
from sim_env import SimEnv2D, Beacon, RectangularObstacle
from robot.car import SimpleCar
from sensors import BeaconSensor
from utils import mat2tuple
import random
from math import log
from numpy.random import multivariate_normal as mvn
from kalman_filter import ekf_update
from covar import cov2vec, vec2cov
from optimize import scp_solver_beliefs

# Set up environment #args

beacons = [
    Beacon(np.array([0.4, -0.4])),
    Beacon(np.array([-0.4, -0.8])),
    Beacon(np.array([0, -0.4]))
]
beacons = beacons[1:]
obstacles = [
    RectangularObstacle(
        np.array([[-0.5, 0.3], [-0.5, 0.5], [-0.10, 0.5], [-0.10, 0.3]],
                 float).T),
    RectangularObstacle(
        np.array([[0.5, 0.5], [0.5, 0.7], [0.10, 0.7], [0.10, 0.5]], float).T)
]
obstacles = obstacles[0:1]
s = SimEnv2D(bounds=[-0.5, 0.5, -0.1, 1.2],
             beacons=beacons,
             obstacles=obstacles)

コード例 #6

ファイル: links_bsp_test2.py プロジェクト: jakebarnwell/PythonGenerator

up_path = os.path.abspath('..')
sys.path.append(up_path)
import numpy as np
import matplotlib.pyplot as plt
from sim_env import SimEnv2D, Beacon
from robots import Links, LocalizerBot, Dot
from optimize import scp_solver_beliefs
from utils import mat2tuple
from sensors import BeaconSensor, FOVSensor
from kalman_filter import ekf_update
from covar import cov2vec, vec2cov
from math import pi
colors = ['b', 'g', 'r', 'c', 'm', 'y']
# Set up environment

beacons = [Beacon(np.array([-0.0, 0.6]))]
ball = np.array([-0.4, 0.15])
start_pt = np.array([0.10, -0.3])
#s = SimEnv2D(bounds=[-1, 1, -1, 1], beacons=beacons)
s = SimEnv2D(bounds=[-4, 4, 0, 4], beacons=beacons)

#theta0 = np.array([-2.11, 2.65]) #x0 is broken, just set to 0
theta0 = np.array([-2.26, 2.03])  #x0 is broken, just set to 0
origin = np.array([0.0, 0.0])
links = Links(theta0, origin=origin, state_rep='angles')
x0 = np.mat(theta0).T
print links.forward_kinematics(origin, theta0)
#x0 = links.forward_kinematics(origin, theta0)
#x0 = np.mat(x0).T
# hack xN
#thetaN = np.array([-3.8, -1.9]) # can be looked up using IK

コード例 #7

import os, sys
up_path = os.path.abspath('..')
sys.path.append(up_path)
import numpy as np
import matplotlib.pyplot as plt
from sim_env import SimEnv2D, Beacon
from robots import Links
from optimize import scp_solver_beliefs
from utils import mat2tuple
from sensors import BeaconSensor
from kalman_filter import ekf_update
from covar import cov2vec, vec2cov
from math import pi
# Set up environment

beacons = [Beacon(np.array([0.8, 0.7])), Beacon(np.array([0.7, 0.7]))]

s = SimEnv2D(bounds=[-1, 1, -1, 1], beacons=beacons)
#s = SimEnv2D(bounds=[-3, 3, -3, 3], beacons=beacons)

theta0 = np.array([-0.5, -0.2])  #x0 is broken, just set to 0
origin = np.array([0.0, 0.0])
links = Links(theta0, origin=origin, state_rep='angles')
x0 = np.mat(theta0).T
#x0 = links.forward_kinematics(origin, theta0)
#x0 = np.mat(x0).T
# hack xN
thetaN = np.array([-3.8, -1.9])  # can be looked up using IK
xN = np.mat(thetaN).T
#xN = links.forward_kinematics(origin, thetaN)
#xN = np.mat(xN).T

コード例 #8

'''
Copied from car_ekf.py, really need to start sticking stuff in
function
'''

d = loadmat('../../../roboscp/car/ex1.mat')
'''
print d
stop
print d['dat_states'].shape
print d['dat_ctrls'].shape
'''

# Set up environment #args

beacons = [Beacon(np.array([0.3, 0.7])), Beacon(np.array([0.8, -0.1]))]
obstacles = [CircularObstacle(0.5, 0.63, 0.08)]
s = SimEnv2D(bounds=[-0.1, 1.3, -0.1, 1], beacons=beacons, obstacles=obstacles)

x0 = d['dat_states'][:, 0]
car = SimpleCar(x0)
car.attach_sensor(BeaconSensor(decay_coeff=25), lambda x: x[0:2])

s.add_robot(car)

# Number of timesteps
T = 30  #arg

# Dynamics and measurement noise
num_states = car.NX
num_ctrls = car.NU

コード例 #9

ファイル: car_camera_demo.py プロジェクト: jakebarnwell/PythonGenerator

from robots import SimpleCar, LocalizerBot
from sensors import BeaconSensor, FOVSensor, ExtendedCamera2D, PinholeCamera2D
from math import pi
from utils import mat2tuple
import random
from math import log
from numpy.random import multivariate_normal as mvn
from kalman_filter import ekf_update
from covar import cov2vec, vec2cov
from optimize import scp_solver_beliefs
from scipy.io import loadmat
import matplotlib.animation as animation
colors = ['b', 'g', 'r', 'c', 'm', 'y']
# Set up environment #args

beacons=[Beacon(np.array([1.2, 0.7])),
         Beacon(np.array([0.2, 0.8]))]
obstacles = [RectangularObstacle(np.array([[0.75, 0.2], [0.75, 0.4], [0.85, 0.4], [0.85,0.2]], float).T),\
             RectangularObstacle(np.array([[0.5,0.85], [1.15,0.85], [1.15,0.6], [0.5,0.6]], float).T)]
obstacles=[]

s = SimEnv2D(bounds=[-0.1, 1.5, -0.1, 1], beacons=beacons, obstacles=obstacles)

ball = np.array([1.4, 0.30])
ball = np.array([1.35, 0])
#ball = np.array([0.8, 0.35])
ball = np.array([1.3, 0.8])
x0 = np.array([0, 0.5, 0, 0])
car = SimpleCar(x0)
car.attach_sensor(BeaconSensor(decay_coeff=25), lambda x: x[0:2])