def _init_anim(self):
import vpython as vp
l_plate = self.domain_param['l_plate']
m_ball = self.domain_param['m_ball']
r_ball = self.domain_param['r_ball']
d_plate = 0.01 # only for animation
# Init render objects on first call
self._anim['canvas'] = vp.canvas(width=800,
height=800,
title="Quanser Ball-Balancer")
self._anim['ball'] = vp.sphere(pos=vp.vec(self.state[2], self.state[3],
r_ball + d_plate / 2.),
radius=r_ball,
mass=m_ball,
color=vp.color.red,
canvas=self._anim['canvas'])
self._anim['plate'] = vp.box(pos=vp.vec(0, 0, 0),
size=vp.vec(l_plate, l_plate, d_plate),
color=vp.color.green,
canvas=self._anim['canvas'])
self._anim['null_plate'] = vp.box(
pos=vp.vec(0, 0, 0),
size=vp.vec(l_plate * 1.1, l_plate * 1.1, d_plate / 10),
color=vp.color.cyan,
opacity=0.5, # 0 is fully transparent
canvas=self._anim['canvas'])
def create_bodies():
width = 3
height = 2
body1 = vp.box(pos=vp.vector(-5, 0, 0), axis=vp.vector(0, 0, 1), width=width, height=height,
arrow=vp.arrow(pos=vp.vector(0, 0, 0), shaftwidth=0.5, color=vp.color.red, visible=False),
radius=1/2 * math.sqrt(width**2 + height**2),
mass=10, # kg
moment_inertia=100, # [kg*m^2]
area=10, # [m^2]
vel=vp.vector(0, 0, 0), # [m/s]
ang_vel=vp.vector(0, 0, 0), # [rad/s^2]
theta=vp.radians(20)) # [rad]
width = 2
height = 3
body2 = vp.box(pos=vp.vector(3, 0.5, 0), axis=vp.vector(0, 0, 1), width=width, height=height,
arrow=vp.arrow(pos=vp.vector(0, 0, 0), shaftwidth=0.5, color=vp.color.blue, visible=False),
radius=1/2 * math.sqrt(width**2 + height**2),
mass=10, # [kg]
moment_inertia=100, # [kg*m^2]
area=10, # [m^2]
vel=vp.vector(0, 0, 0), # [m/s]
ang_vel=vp.vector(0, 0, 0), # [rad/s^2]
theta=vp.radians(0)) # [rad]
bodies = [body1, body2]
for body in bodies:
body.vertices = vertices(body)
body.rotate(angle=body.theta)
return bodies
def __init__(self, radius=10.0, bodies=[]):
self.radius = radius
self.bodies = bodies
self.base = vis.cylinder(pos=vec(0, 0, -2),
axis=vec(0, 0, 1),
radius=self.radius,
color=vis.color.gray(0.6))
self.top_plate = vis.box(pos=vec(0, .5, -1),
length=20,
height=.25,
width=1,
color=vis.color.red)
self.bottom_plate = vis.box(pos=vec(0, -.5, -1),
length=20,
height=.25,
width=1,
color=vis.color.blue)
self.polarity = "up"
self.e_field = vec(0, E_mag, 0)
self.e_indicator = vis.arrow(pos=vec(-11, 0, 0),
axis=vec(0, 2, 0),
color=vis.color.yellow)
self.b_field = vec(0, 0, -B_mag) # 1 mT in -z direction
self.time_label = vis.label(pixel_pos=vec(0, 0, 0),
xoffset=100,
yoffset=-1 * (scene.height - 30),
align="left",
box=True,
line=False)
def vpython_visualization(relcat_object, x, ball_size, comparison_ball_size):
"""
Requires VPython to run
"""
relcat_object.run_test_trial(x, ball_size, comparison_ball_size)
import vpython
vpython.scene.title = "Trial Simulation"
vpython.scene.center = vpython.vector(relcat_object.world_right / 2,
relcat_object.world_bottom / 2, 0)
vpython.scene.background = vpython.color.black
vpython.scene.range = 175
vpython.box(pos=vpython.vector(relcat_object.world_right / 2,
relcat_object.world_bottom / 2, 0),
length=relcat_object.world_right,
height=relcat_object.world_bottom,
width=1,
color=vpython.color.white)
ball = vpython.sphere(
pos=vpython.vector(relcat_object.object_records['ball']['x'][0],
relcat_object.object_records['ball']['y'][0], 0),
radius=relcat_object.object_records['ball']['radius'][0],
color=vpython.color.blue)
agent = vpython.sphere(
pos=vpython.vector(relcat_object.object_records['agent']['x'][0],
relcat_object.object_records['agent']['y'][0], 0),
radius=relcat_object.object_records['agent']['radius'][0],
color=vpython.color.orange)
rays = []
for i in range(relcat_object.num_rays):
rays.append(
arrow(pos=vpython.vector(relcat_object.object_records[i]['x1'][0],
relcat_object.object_records[i]['y1'][0],
0),
axis=vpython.vector(
relcat_object.object_records[i]['x2'][0] -
relcat_object.object_records[i]['x1'][0],
relcat_object.object_records[i]['y2'][0] -
relcat_object.object_records[i]['y1'][0], 0),
color=vpython.color.cyan,
shaftwidth=1))
while True:
for i in range(len(relcat_object.time_records)):
rate(150)
ball.pos.x = relcat_object.object_records['ball']['x'][i]
ball.pos.y = relcat_object.object_records['ball']['y'][i]
ball.radius = relcat_object.object_records['ball']['radius'][i]
agent.pos.x = relcat_object.object_records['agent']['x'][i]
agent.pos.y = relcat_object.object_records['agent']['y'][i]
agent.radius = relcat_object.object_records['agent']['radius'][i]
for j, ray in enumerate(rays):
ray.pos.x = relcat_object.object_records[j]['x1'][i]
ray.pos.y = relcat_object.object_records[j]['y1'][i]
ray.axis.x = relcat_object.object_records[j]['x2'][i] \
- relcat_object.object_records[j]['x1'][i]
ray.axis.y = relcat_object.object_records[j]['y2'][i] \
- relcat_object.object_records[j]['y1'][i]
def main(grid, init, goal, steering_noise, distance_noise, measurement_noise,weight_data, weight_smooth, p_gain, d_gain):
landmarkss = []
for i in range(len(grid)):
for j in range(len(grid[0])):
if grid[i][j]==1:
landmarkss.append((i,j))
z = (len(grid)*10)+5
x = (len(grid[0])*10)+5
escena = vpython.canvas(width=1200,height=1000)
escena.title = "Simulador de Autonomía Vehicular"
escena.background = vpython.color.cyan
escena.center = vpython.vector(x/2,-1,z/2)
escena.forward= vpython.vector(0,-2,-1)
#escena.range= 80
#ejex= vpython.curve(color=vpython.color.blue)
#ejey=vpython.curve(color=vpython.color.red)
#ejez=vpython.curve(color=vpython.color.green)
#vpython.userzoom = True
#for i in range(x):
# pos_x = vpython.vector(i,0,0)
# ejex.append(pos_x)
# pos_y = vpython.vector(0,i,0)
# ejey.append(pos_y)
# pos_z=vpython.vector(0,0,i)
# ejez.append(pos_z)
suelo = vpython.box(pos=vpython.vector(x/2,-0.5,z/2),color=vpython.color.white,size=vpython.vector(x,1.0,z),texture={'file':vpython.textures.metal, 'place':'all'})
high_wall = 30
for i in range(len(landmarkss)):
x_wall = (landmarkss[i][1]*10)+5
z_wall = (landmarkss[i][0]*10)+5
if i%2==0:
wall = vpython.box(pos=vpython.vector(x_wall,15,z_wall),color=vpython.vector(1,0.7,0.2),size=vpython.vector(10,high_wall,10),texture={'file':vpython.textures.metal, 'place':'all'})
else:
wall = vpython.box(pos=vpython.vector(x_wall,25,z_wall),color=vpython.vector(1,0.7,0.2),size=vpython.vector(10,high_wall+20,10),texture={'file':vpython.textures.metal, 'place':'all'})
#IMPLEMENTING PLANING: A* ALGORITHM
path = plan(grid, init, goal)
path.astar()
path_way = path.path
#IMPLEMENTING SMOOTHNESS PATH
path.smooth(weight_data, weight_smooth)
path_smoothed = path.spath
inicio =vpython.text(text="Inicio",align='center',color=vpython.color.green,pos=vpython.vector((path_smoothed[0][1]*10)+5,22,(path_smoothed[0][0]*10)+5),height=5,width=12)
meta = vpython.text(text="Meta",align='center',color=vpython.color.green,pos=vpython.vector((path_smoothed[-1][1]*10)+5,22,(path_smoothed[-1][0]*10)+5),height=5,width=12)
#IMPLEMENTING PARTICLE FILTERS AND PID
return run(grid, goal, path.spath, [p_gain, d_gain],escena)
def plot_boxes(u, v):
vp.canvas()
for uu, vv in zip(u, v):
vp.box(pos=vp.vec(0, 0, 0),
axis=vp.vec(uu[0], uu[1], uu[2]),
length=1,
width=1,
height=1,
up=vp.vec(vv[0], vv[1], vv[2]),
color=vp.vec(
np.random.rand(1)[0],
np.random.rand(1)[0],
np.random.rand(1)[0]))
def arrow_between_cylinders(pos1, pos2, color, r):
def distance(pos1, pos2):
x = pos2.x - pos1.x
y = pos2.y - pos1.y
lenR = np.sqrt(x * x + y * y)
return (lenR)
lenArrow = distance(pos1, pos2)
box(pos=(pos1 + pos2) / 2,
axis=pos2 - pos1,
length=lenArrow - 10,
width=.1,
height=.25,
color=color)
def representer(xs, ys, name, clock, orienter):
escena = vpython.canvas(width=1200, height=600)
escena.title = name
escena.background = vpython.color.cyan
pos = vpython.vector(xs[0], 2, -ys[0])
escena.range = 100
escena.center = vpython.vector(0, 0, 0)
escena.forward = vpython.vector(1, -2, 1)
#suelo = vpython.box(pos = vpython.vector(0,-1,0),size=vpython.vector(10,0.01,200),color=vpython.color.white)
suelo = vpython.box(pos=vpython.vector(250, -1, 0),
size=vpython.vector(500, 0.01, 200),
color=vpython.color.white,
texture=vpython.textures.wood)
vehicle = vpython.box(pos=pos,
color=vpython.color.red,
size=vpython.vector(8, 4, 6))
trayectoria = vpython.curve(color=vpython.color.yellow)
road = vpython.curve(color=vpython.color.black)
muro = vpython.box(pos=vpython.vector(0, 20, 10),
color=vpython.color.blue,
size=vpython.vector(20, 40.99, 2),
texture=vpython.textures.stucco)
piedra1 = vpython.sphere(pos=vpython.vector(150, -1, -ys[150]),
color=vpython.color.black,
radius=1)
piedra2 = vpython.sphere(pos=vpython.vector(xs[400], -1, -ys[400]),
color=vpython.color.black,
radius=1)
time.sleep(clock)
#escena.range = 45
escena.range = 20
for i in range(len(xs)):
if i > 0:
vehicle.rotate(axis=vpython.vector(0, 1, 0),
angle=-orienter[i - 1])
pos = vpython.vector(xs[i], 2, -ys[i])
#suelo.pos=vpython.vector(i/2.0,-1,0)
#suelo.size = vpython.vector(i,0.01,200)
muro.pos = vpython.vector(i / 2, 20, 10)
muro.size = vpython.vector(i, 40.99, 2)
vehicle.pos = pos
escena.center = vpython.vector(xs[i], 5, -ys[i])
trayectoria.append(pos)
vehicle.rotate(axis=vpython.vector(0, -1, 0), angle=-orienter[i])
road.append(vpython.vector(i, 2, 0))
vpython.rate(40)
def create_walls(self):
brown = vec(0.55, 0.27, 0.07)
wall_right = box(pos=vec(0, 0, (WIDTH + WALL_WIDTH) / 2),
size=vec(LENGTH, WALL_WIDTH, WALL_WIDTH),
color=brown)
wall_left = box(pos=vec(0, 0, -(WIDTH + WALL_WIDTH) / 2),
size=vec(LENGTH, WALL_WIDTH, WALL_WIDTH),
color=brown)
wall_up = box(pos=vec((LENGTH + WALL_WIDTH) / 2, 0, 0),
size=vec(WALL_WIDTH, WALL_WIDTH, WIDTH + 2 * WALL_WIDTH),
color=brown)
wall_down = box(pos=vec(-(LENGTH + WALL_WIDTH) / 2, 0, 0),
size=vec(WALL_WIDTH, WALL_WIDTH,
WIDTH + 2 * WALL_WIDTH),
color=brown)
def test_graphics_canvas_init(self):
# Create a canvas with all options being used (different to defaults)
scene = canvas.GraphicsCanvas3D(
height=360,
width=480,
title="Test Graphics Canvas Creation",
caption="Caption text here",
grid=False
)
try:
# Put a box in the created scene
box(canvas=scene.scene)
except:
# Something went wrong
self.assertEqual(False, True)
def __draw_grid__(self):
reward_height = self.grid_height * 0.2
for col in range(self.cols):
for row in range(self.rows):
loc = (row, col)
if self.grid_dict['rewards'][loc] is None:
color = vp.color.black
elif self.grid_dict['start'] == loc:
color = vp.color.green
elif self.grid_dict['actions'][loc] == 'win':
color = vp.color.white
win_loc = loc
elif self.grid_dict['actions'][loc] == 'lose':
color = vp.color.white
lose_loc = loc
else:
color = vp.color.white
x = col * self.grid_width - self.ctr_x
y = -row * self.grid_height + self.ctr_y
self.squares[col][row] = vp.box(
pos=vp.vector(x, y, 0),
axis=vp.vector(0, 0, self.grid_thick),
width=self.grid_width - self.grid_thick,
height=self.grid_height - self.grid_thick,
color=color)
self.squares[win_loc[1]][win_loc[0]].texture = 'DATAcated.jpg'
self.squares[lose_loc[1]][lose_loc[0]].texture = 'NotReg.jpg'
def __draw_grid__(self):
reward_height = self.grid_height * 0.2
for col in range(self.cols):
for row in range(self.rows):
loc = (row, col)
if self.grid_dict['rewards'][loc] is None:
color = vp.color.black
elif self.grid_dict['start'] == loc:
color = vp.color.green
elif self.grid_dict['actions'][loc] == 'win':
color = vp.color.yellow
elif self.grid_dict['actions'][loc] == 'lose':
color = vp.color.red
else:
color = vp.color.white
x = col * self.grid_width - self.ctr_x
y = -row * self.grid_height + self.ctr_y
self.squares[col][row] = vp.box(
pos=vp.vector(x, y, 0),
axis=vp.vector(0, 0, self.grid_thick),
width=self.grid_width - self.grid_thick,
height=self.grid_height - self.grid_thick,
color=color)
def init_visual(self):
color = vp.color.cyan
self.visual = vp.box(pos=self.get_pos(),
length=longeur_wagon,
height=largeur_wagon,
width=hauteur_wagon,
color=color)
def test_vpython_to_se3(self):
# Create a scene
scene = canvas.GraphicsCanvas3D(title="TEST VPYTHON TO SE3")
# Create a basic entity
# pos = 1, 2, 3
# X = 0, 0, -1
# Y = -1, 0, 0
# Z = 0, 1, 0
entity = box(
pos=vector(1, 2, 3),
axis=vector(0, 0, -1),
up=vector(-1, 0, 0)
)
scene.scene.waitfor("draw_complete")
# Check resulting SE3
arr = array([
[0, -1, 0, 1],
[0, 0, 1, 2],
[-1, 0, 0, 3],
[0, 0, 0, 1]
])
expected = SE3(arr)
self.assertEqual(common.vpython_to_se3(entity), expected)
def create_line(pos1, pos2, scene):
"""
Create a line from position 1 to position 2.
:param scene: The scene in which to draw the object
:type scene: class:`vpython.canvas`
:param pos1: 3D position of one end of the line.
:type pos1: class:`vpython.vector`
:param pos2: 3D position of the other end of the line.
:type pos2: class:`vpython.vector`
"""
# Length of the line using the magnitude
line_len = mag(pos2-pos1)
# Position of the line is the midpoint (centre) between the ends
position = (pos1 + pos2) / 2
# Axis direction of the line (to align the box (line) to intersect the two points)
axis_dir = pos2 - pos1
# Return a box of thin width and height to resemble a line
thickness = 0.01
return box(canvas=scene,
pos=position,
axis=axis_dir,
length=line_len,
width=thickness,
height=thickness,
color=color.black)
def make_robot():
chassis_width = 155 # left side to right
chassis_thickness = 3 # plastic thickness
chassis_length = 200 # front to back
wheel_thickness = 26
wheel_diameter = 70
axle_x = 30 # wheel axle from
axle_z = -20
rear_castor_position = vp.vector(-80, -6, -30)
rear_castor_radius = 14
rear_caster_thickness = 12
base = vp.box(length=chassis_length,
height=chassis_thickness,
width=chassis_width)
# rotate to match body - so Z is height and Y is width
base.rotate(angle=vp.radians(90), axis=vp.vector(1, 0, 0))
wheel_dist = chassis_width / 2
wheel_l = vp.cylinder(radius=wheel_diameter / 2,
length=wheel_thickness,
pos=vp.vector(axle_x, -wheel_dist, axle_z),
axis=vp.vector(0, -1, 0))
wheel_r = vp.cylinder(radius=wheel_diameter / 2,
length=wheel_thickness,
pos=vp.vector(axle_x, wheel_dist, axle_z),
axis=vp.vector(0, 1, 0))
rear_castor = vp.cylinder(radius=rear_castor_radius,
length=rear_caster_thickness,
pos=rear_castor_position,
axis=vp.vector(0, 1, 0))
return vp.compound([base, wheel_l, wheel_r, rear_castor])
def __init__(self):
self.guiItem = vp.box(pos=vp.vector(0, 0, 0), size=vp.vector(20, 15, 1), color=color.magenta)
# vp.rate(100)
self.xOrient = 0
self.yOrient = 0
self.zOrient = 0
self.comm = comms()
def create_scene(self, width, height, vmax):
self.__scene = vp.scene
self.__scene.width = width
self.__scene.height = height
self.__scene.range = 15
self.__scene.camera.pos = vp.vector(-30, 0, 10)
self.__scene.camera.axis = vp.vector(30, 0, -10)
self.__scene.camera.up = vp.vector(0, 0, 1)
axis = [vp.vector(1, 0, 0), vp.vector(0, 1, 0), vp.vector(0, 0, 1)]
self.__floor = []
w = 100
l = w * 1.7
N = 3
for i in range(N):
for j in range(N):
b = vp.box(pos=vp.vector(-w + w * i, -l + l * j, -0.5),
height=1,
width=l,
length=w,
up=vp.vector(0, 0, 1))
b.texture = 'Resources\\floor.jpg'
self.__floor.append(b)
for ax in axis:
vp.arrow(pos=vp.vector(0, 0, 0),
axis=ax,
shaftwidth=0.05,
color=ax)
self.__dt = 0.1
self.__frame = 0
self.__vmax = vmax
def __set_graphic(self, structure):
"""
Set the graphic object depending on if one was given. If no object was given, create a box and return it
:param structure: `float` or `str` representing the joint length or STL path to load from
:type structure: `float`, `str`
:raises ValueError: Joint length must be greater than 0
:return: Graphical object for the joint
:rtype: class:`vpython.compound`
"""
if isinstance(structure, float):
length = structure
if length <= 0.0:
raise ValueError("Joint length must be greater than 0")
box_midpoint = vector(length / 2, 0, 0)
# Create a box along the +x axis, with the origin (point of rotation) at (0, 0, 0)
graphic_obj = box(canvas=self.__scene,
pos=vector(box_midpoint.x, box_midpoint.y,
box_midpoint.z),
axis=x_axis_vector,
size=vector(length, 0.1, 0.1),
up=y_axis_vector)
# Set the boxes new origin
graphic_obj = compound([graphic_obj],
origin=vector(0, 0, 0),
axis=x_axis_vector,
up=y_axis_vector)
return graphic_obj
else:
return import_object_from_numpy_stl(structure, self.__scene)
def makeBoxMat(Rows=Rows,Cols=Cols):
boxMat = dict()
for row in range(Rows):
for col in range(Cols):
r=random()
boxMat[row,col] = box(pos = vec(row,col,0), color=vec(r,1-r,0))
return boxMat
def draw_snooker_table(pt1, pt2, z):
# pt1 = left top corner
# pt2 = right bottom corner
# z = ball radius
print('Drawing table', pt1, pt2, z)
length = pt2[0] - pt1[0]
width = pt2[1] - pt1[1]
height = 10
# Create table, note that the position is determined by the center of the box
table = vp.box(
pos=vp.vec(pt2[0] - length / 2, z - height / 2, pt2[1] - width / 2),
length=int(length),
height=10,
width=int(width),
color=vp.vec(0, 103 / 255, 1 / 255) # Green
,
texture={
'file': './snooker_texture.jpg',
'place': 'all'
})
table.shininess = 100000
corner_pocket_br = vp.extrusion(
path=vp.paths.arc(radius=height, angle1=-pi / 2, angle2=+pi / 2),
color=vp.vector(0.8, 0.8, 0.8),
shape=[[vp.shapes.rectangle(pos=(1, 0), width=height, height=height)]])
#
corner_pocket_br.rotate(angle=3 * pi / 4, axis=vp.vec(0, 1, 0))
corner_pocket_br.pos = vp.vec(pt1[0], 0, pt1[1])
corner_pocket_tr = corner_pocket_br.clone(pos=vp.vector(pt1[0], 0, pt2[1]))
corner_pocket_tr.rotate(angle=pi / 2, axis=vp.vec(0, 1, 0))
corner_pocket_tl = corner_pocket_tr.clone(pos=vp.vector(pt2[0], 0, pt2[1]))
corner_pocket_tl.rotate(angle=pi / 2, axis=vp.vec(0, 1, 0))
corner_pocket_bl = corner_pocket_tl.clone(pos=vp.vector(pt2[0], 0, pt1[1]))
corner_pocket_bl.rotate(angle=pi / 2, axis=vp.vec(0, 1, 0))
def __init__(self,
x,
y,
shape='_',
colour=color.white,
player='first',
opacity=1):
self.x_pos = x
self.y_pos = y
self.shape = shape
self.color = colour
self.blocks = []
radius = 0.23 if player == 'first' else 0.22
for _ in range(3):
block = compound([
box(pos=vector(x, y, 0),
height=.8,
width=.8,
length=.8,
opacity=opacity),
sphere(pos=vector(x, y, 0.5), radius=radius, opacity=opacity),
sphere(pos=vector(x, y, -0.5), radius=radius, opacity=opacity)
])
self.blocks.append(block)
self.shape_dictionary = {
'_': (0, 0, 1, 0, 2, 0),
'I': (0, 0, 0, 1, 0, 2),
'b': (0, 0, 1, 0, 0, 1),
'd': (0, 0, 1, 0, 1, 1),
'p': (0, 0, 0, 1, 1, 1),
'q': (0, 1, 1, 0, 1, 1),
}
self.updater(self.x_pos, self.y_pos)
def _init_anim(self):
import vpython as vp
r_ball = self.domain_param['r_ball']
l_beam = self.domain_param['l_beam']
d_beam = self.domain_param['d_beam']
x = float(self.state[0]) # ball position along the beam axis [m]
a = float(self.state[1]) # angle [rad]
self._anim['canvas'] = vp.canvas(width=800, height=600, title="Ball on Beam")
self._anim['ball'] = vp.sphere(
pos=vp.vec(x, d_beam/2. + r_ball, 0),
radius=r_ball,
color=vp.color.red,
canvas=self._anim['canvas']
)
self._anim['beam'] = vp.box(
pos=vp.vec(0, 0, 0),
axis=vp.vec(vp.cos(a), vp.sin(a), 0),
length=l_beam,
height=d_beam,
width=2*d_beam,
color=vp.color.green,
canvas=self._anim['canvas']
)
def draw_aabb(mesh_list, opacity=0.2):
"""
Draws the AABB of a mesh.
Args:
mesh_list(list): A list of Mesh objects which AABB that should be
drawn on the scene.
opacity(float): Optional. Opacity of the AABB.
Returns:
A dict of vpython.box objects that represents the AABBs on the scene.
key is the mesh name and the value is the vpython.box.
"""
aabbs = {}
for mesh in mesh_list:
aabbs[mesh.name] = vpython.box(pos=vpython.vec(
mesh.aabb.pos[0],
mesh.aabb.pos[1],
mesh.aabb.pos[2],
),
size=vpython.vec(
mesh.aabb.half_size[0] * 2,
mesh.aabb.half_size[1] * 2,
mesh.aabb.half_size[2] * 2,
))
aabbs[mesh.name].opacity = opacity
aabbs[mesh.name].color = vpython.vec(mesh.aabb.color[0],
mesh.aabb.color[1],
mesh.aabb.color[2])
aabbs[mesh.name].name = mesh.name + '_aabb'
return aabbs
def __init__(self, theta, alpha, frequency):
self.frequency = frequency
vp.scene.width, vp.scene.height = 1000, 600
vp.scene.range = 0.25
vp.scene.title = "QubeServo2-USB rotary pendulum"
# Dimensions of the rotary pendulum parts
base_w, base_h, base_d = 0.102, 0.101, 0.102 # width, height, & len of base
rotor_d, rotor_h = 0.0202, 0.01 # height, diameter of the rotor platform
rotary_top_l, rotary_top_d = 0.032, 0.012 # height, diameter of the rotary top
self.arm_l, arm_d = 0.085, 0.00325 # height, diameter of the arm
self.pen_l, self.pen_d = 0.129, 0.00475 # height, diameter of the pendulum
# Origin of parts
arm_origin = vp.vec(0, 0, 0)
self.rotary_top_origin = vp.vec(0, 0, -rotary_top_l / 2)
rotor_origin = arm_origin - vp.vec(0, rotor_h + rotary_top_d / 2 - 0.0035, 0)
base_origin = rotor_origin - vp.vec(0, base_h / 2, 0)
# Create the part objects
base = vp.box(
pos=base_origin,
size=vp.vec(base_w, base_h, base_d),
color=vp.vec(0.45, 0.45, 0.45),
)
rotor = vp.cylinder(
pos=rotor_origin,
axis=vp.vec(0, 1, 0),
size=vp.vec(rotor_h, rotor_d, rotor_d),
color=vp.color.yellow,
)
self._rotary_top = vp.cylinder(
pos=self.rotary_top_origin,
axis=vp.vec(0, 0, 1),
size=vp.vec(rotary_top_l, rotary_top_d, rotary_top_d),
color=vp.color.red,
)
self._arm = vp.cylinder(
pos=arm_origin,
axis=vp.vec(0, 0, 1),
size=vp.vec(self.arm_l, arm_d, arm_d),
color=vp.vec(0.7, 0.7, 0.7),
)
self._pendulum = vp.cylinder(
pos=self.pendulum_origin(theta),
axis=vp.vec(0, 1, 0),
size=vp.vec(self.pen_l, self.pen_d, self.pen_d),
color=vp.color.red,
)
# Rotate parts to their init orientations
self._rotary_top.rotate(angle=theta, axis=vp.vec(0, 1, 0), origin=arm_origin)
self._arm.rotate(angle=theta, axis=vp.vec(0, 1, 0), origin=arm_origin)
self._pendulum.rotate(
angle=alpha,
axis=self.pendulum_axis(theta),
origin=self.pendulum_origin(theta),
)
self.theta, self.alpha = theta, alpha
def _init_anim(self):
import vpython as vp
# Convert to float for VPython
Lr = float(self.domain_param['Lr'])
Lp = float(self.domain_param['Lp'])
# Init render objects on first call
self._anim['canvas'] = vp.canvas(width=800,
height=600,
title="Quanser Qube")
scene_range = 0.2
arm_radius = 0.003
pole_radius = 0.0045
self._anim['canvas'].background = vp.color.white
self._anim['canvas'].lights = []
vp.distant_light(direction=vp.vec(0.2, 0.2, 0.5), color=vp.color.white)
self._anim['canvas'].up = vp.vec(0, 0, 1)
self._anim['canvas'].range = scene_range
self._anim['canvas'].center = vp.vec(0.04, 0, 0)
self._anim['canvas'].forward = vp.vec(-2, 1.2, -1)
vp.box(pos=vp.vec(0, 0, -0.07),
length=0.09,
width=0.1,
height=0.09,
color=vp.color.gray(0.5))
vp.cylinder(axis=vp.vec(0, 0, -1),
radius=0.005,
length=0.03,
color=vp.color.gray(0.5))
# Joints
self._anim['joint1'] = vp.sphere(radius=0.005, color=vp.color.white)
self._anim['joint2'] = vp.sphere(radius=pole_radius,
color=vp.color.white)
# Arm
self._anim['arm'] = vp.cylinder(radius=arm_radius,
length=Lr,
color=vp.color.blue)
# Pole
self._anim['pole'] = vp.cylinder(radius=pole_radius,
length=Lp,
color=vp.color.red)
# Curve
self._anim['curve'] = vp.curve(color=vp.color.white,
radius=0.0005,
retain=2000)
def generate_stage(self):
wall_r = box(pos=vector(self.side, 0, 0),
size=vector(self.thk, self.s2, self.s3),
color=color.blue)
wall_l = box(pos=vector(-self.side, 0, 0),
size=vector(self.thk, self.s2, self.s3),
color=color.blue)
wall_g = box(pos=vector(0, -self.side, 0),
size=vector(self.s3, self.thk, self.s3),
color=color.blue)
wall_t = box(pos=vector(0, self.side, 0),
size=vector(self.s3, self.thk, self.s3),
color=color.blue)
wall_bk = box(pos=vector(0, 0, -self.side),
size=vector(self.s2, self.s2, self.thk),
color=color.blue)
def create_line(pos1,
pos2,
scene,
colour=None,
thickness=0.01,
opacity=1): # pragma nocover
"""
Create a line from position 1 to position 2.
:param scene: The scene in which to draw the object
:type scene: class:`vpython.canvas`
:param pos1: 3D position of one end of the line.
:type pos1: class:`vpython.vector`
:param pos2: 3D position of the other end of the line.
:type pos2: class:`vpython.vector`
:param colour: RGB list to colour the line to
:type colour: `list`
:param thickness: Thickness of the line
:type thickness: `float`
:param opacity: Opacity of the line
:type opacity: `float`
:raises ValueError: RGB colour must be normalised between 0->1
:raises ValueError: Thickness must be greater than 0
:return: A box resembling a line
:rtype: class:`vpython.box`
"""
# Set default colour
# Stops a warning about mutable parameter
if colour is None:
colour = [0, 0, 0]
if colour[0] > 1.0 or colour[1] > 1.0 or colour[2] > 1.0 or \
colour[0] < 0.0 or colour[1] < 0.0 or colour[2] < 0.0:
raise ValueError("RGB values must be normalised between 0 and 1")
if thickness < 0.0:
raise ValueError("Thickness must be greater than 0")
# Length of the line using the magnitude
line_len = mag(pos2 - pos1)
# Position of the line is the midpoint (centre) between the ends
position = (pos1 + pos2) / 2
# Axis direction of the line (to align the box (line) to intersect the
# two points)
axis_dir = pos2 - pos1
# Return a box of thin width and height to resemble a line
# thickness = 0.01
return box(canvas=scene,
pos=position,
axis=axis_dir,
length=line_len,
width=thickness,
height=thickness,
color=vector(colour[0], colour[1], colour[2]),
opacity=opacity)
def representer(grid,path_way,clock,landmarks):
z =len(grid)*10
x=len(grid[0])*10
escena = vpython.canvas(width=1200,height=600)
escena.title = "Path Finder usando el algoritmo A*"
escena.background = vpython.color.cyan
escena.center = vpython.vector(x/2,-1,z/2)
#escena.forward= vpython.vector(1,-1,0.2)
escena.forward= vpython.vector(0,-2,-1)
#ejex= vpython.curve(color=vpython.color.blue)
#ejey=vpython.curve(color=vpython.color.red)
#ejez=vpython.curve(color=vpython.color.green)
#vpython.userzoom = True
#for i in range(x):
# pos_x = vpython.vector(i,0,0)
# ejex.append(pos_x)
# pos_y = vpython.vector(0,i,0)
# ejey.append(pos_y)
# pos_z=vpython.vector(0,0,i)
# ejez.append(pos_z)
suelo = vpython.box(pos=vpython.vector(x/2,-1,z/2),color=vpython.color.yellow,size=vpython.vector(x,0.1,z),texture=vpython.textures.rough)
high_wall = 20
for i in range(len(landmarks)):
z_wall=(landmarks[i][0]*10)+5
x_wall=(landmarks[i][1]*10)+5
muro = vpython.box(pos=vpython.vector(x_wall,10,z_wall),color=vpython.color.white,size=vpython.vector(10,high_wall,10),texture=vpython.textures.wood)
robot = vpython.sphere(pos=vpython.vector(path_way[0][1]+5,3,path_way[0][0]+5),color=vpython.color.black,radius=3)
trayectoria = vpython.curve(color=vpython.color.white)
inicio =vpython.text(text="Inicio",align='center',color=vpython.color.green,pos=vpython.vector(5,22,5),height=5,width=12)
meta = vpython.text(text="Meta",align='center',color=vpython.color.green,pos=vpython.vector((path_way[-1][1]*10)+5,22,(path_way[-1][0]*10)+5),height=5,width=12)
time.sleep(clock)
escena.range=150
for i in range(len(path_way)):
#escena.center = vpython.vector((path_way[i][1]*10)+5,-100,(path_way[i][0]*10)+5-20)
escena.center = vpython.vector((path_way[i][1]*10)+5,4,(path_way[i][0]*10)+5)
#time.sleep(0.3)
time.sleep(0.1)
pos = vpython.vector((path_way[i][1]*10)+5,3,(path_way[i][0]*10)+5)
robot.pos = pos
trayectoria.append(pos)
#vpython.rate(100)
vpython.rate(300)
def __init__(self, geom, ident=None, onCanvas=v.scene):
self.src = v.box(display=onCanvas)
(xsize, ysize, zsize) = geom.getLengths()
self.src.length = xsize
self.src.height = ysize
self.src.width = zsize
Vpy_Object.__init__(self, geom, ident)
def init_vpython(self):
scene = canvas(title="Fans", width=self.win, height=self.win, x=0, y=0,
center=vec(0, 0, 0), forward=vec(1,0,-1),
up=self.up)
scene.autoscale = False
scene.range = 25
h = 0.1
mybox = box(pos=vec(0, 0, -h/2), length=self.L, height=h, width=L, up=self.up,
color=color.white)
# create dust particles
for pos in self.positions:
p = sphere(pos=vec(*pos), radius=self.radius, color=color.red)
p.update = True # to determine if needs position update
self.particles.append(p)
gray = vec(0.7,0.7,0.7) # color of edges of container
up = vec(0, 0, 1)
# In[ ]:
scene = canvas(title="Fans", width=win, height=win, x=0, y=0,
center=vec(0, 0, 0), forward=vec(1,0,-0.5),
up=up)
scene.autoscale = False
scene.range = 10
h = 0.1
mybox = box(pos=vec(0, 0, -h/2), length=L, height=h, width=L, up=up, color=gray)
m = 1 # kg
radius = 0.5 # arbitrary for now
dt = 1e-3
start = vec(0, 0, radius)
v0 = vec(0, 0, 10)
g = vec(0, 0, -9.81)
Fg = m*g
particles =[]
for position, c in zip([start, start+vec(0, 3*radius, 0)],
[color.red, color.blue]):
p = sphere(pos=position, radius=radius, color=c)
p.v = v0
p.update = True # to determine if needs position update
def __init__(self, x, y, z, h, width=0.05, thick = 0.05,
barcolor=vec(1,1,1), axis=vec(1,0,0)):
self.bars=[vp.box(length=width, width=thick, color=barcolor,
axis=axis) for i in range(len(x))]
self.move(x, y, z, h)
'''
Created on 8 juil. 2012
@author: Laurent
'''
from vpython import box, sphere, vector, color, rate
floor = box(pos=vector(0, 0, 0), length=4, height=0.5, width=4, color=color.blue)
ball = sphere(pos=vector(0, 4, 0), radius=1, color=color.red)
ball.velocity = vector(0, -1, 0)
dt = 0.01
while 1:
rate(100)
ball.pos = ball.pos + ball.velocity * dt
if ball.pos.y < ball.radius:
ball.velocity.y = abs(ball.velocity.y)
else:
ball.velocity.y = ball.velocity.y - 9.8 * dt