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_small_arm(self):
# cal axis 0, 1
axis_0 = self.upper_arm_axis.norm()
axis_2 = self.upper_arm_subaxis.norm()
axis_1 = axis_0.cross(axis_2).norm()
# draw small_arm
self.small_arm = cylinder(pos=self.joint_pos,
axis=axis_0 * self.small_arm_len,
radius=self.small_arm_rad,
color=color.cyan,
opacity=0.5)
self.small_arm.rotate(axis=axis_1, angle=radians(self.small_arm_flex))
# draw xyz
self.small_arm_x = arrow(pos=self.joint_pos,
axis=axis_0 * self.note_len,
color=color.red,
radius=self.sketch_rad)
self.small_arm_x.rotate(axis=axis_1,
angle=radians(self.small_arm_flex))
self.small_arm_y = arrow(pos=self.joint_pos,
axis=axis_1 * self.note_len,
color=color.green,
radius=self.sketch_rad)
self.small_arm_z = arrow(pos=self.joint_pos,
axis=axis_2 * self.note_len,
color=color.blue,
radius=self.sketch_rad)
self.small_arm_z.rotate(axis=axis_1,
angle=radians(self.small_arm_flex))
def wrap_to_pi(angle_type, angle):
"""
Wrap the given angle (deg or rad) to [-pi pi]
:param angle_type: String of whether the angle is deg or rad
:type angle_type: `str`
:param angle: The angle to wrap
:type angle: `float`
:raises ValueError: Throws the error if the given string is not "deg" or "rad"
:return: The wrapped angle
:rtype: `float`
"""
if angle_type == "deg":
angle = angle % 360
if angle > 180:
angle -= 360
elif angle_type == "rad":
angle = angle % radians(360)
if angle > radians(180):
angle -= radians(360)
else:
raise ValueError('angle_type must be "deg" or "rad"')
return angle
def motion_no_drag(data):
"""
Create animation for projectile motion with no dragging force
"""
ball_nd = sphere(pos=vector(0, data['init_height'], 0),
radius=1,
color=color.cyan,
make_trail=True)
# Follow the movement of the ball
scene.camera.follow(ball_nd)
# Set initial velocity & position
# Animate & store
data["x1"] = []
data["y1"] = []
y0 = data["init_height"]
x0 = 0
x, y = x0, y0
ball_nd.pos.x = x
ball_nd.pos.y = y
dt = data['deltat']
t = 0
vx0 = data['init_velocity'] * cos(radians(data['theta']))
vy0 = data['init_velocity'] * sin(radians(data['theta']))
while y >= 0:
rate(1000)
data["x1"].append(x)
data["y1"].append(y)
ball_nd.pos = vector(x, y, 0)
t = t + dt
x = x0 + vx0 * t
y = y0 + vy0 * t + (data['gravity'] / 2) * (t**2)
def __init__(self, ):
self.cav = vp.canvas()
self.cav.camera.rotate(angle=vp.radians(180), axis=self.cav.up)
self.cav.camera.rotate(angle=vp.radians(180), axis=self.cav.forward)
joints_radius = 10
joints_color = vpvec([1, 1, 1])
joints_opacity = 0.8
line_color = vp.color.cyan
line_radius = 5
self.joints = [
vp.sphere(
canvas=self.cav,
pos=vpvec([0, 0, 0]),
radius=joints_radius * 0.9,
color=joints_color,
opacity=joints_opacity,
) for i in range(21)
]
self.jointline = {
c: vp.curve(
canvas=self.cav,
pos=[vpvec([0, 0, 0]), vpvec([0, 0, 0])],
color=line_color,
radius=line_radius,
)
for c in connections
}
def moving_taiqi(arm=arm):
# Taiqi
print('Taiqi starts ...')
for _ in range(30):
axis_ = arm.upper_arm_axis.cross(arm.upper_arm_subaxis).norm()
arm.upper_arm_axis = arm.upper_arm_axis.rotate(axis=axis_,
angle=radians(2))
arm.upper_arm_subaxis = arm.upper_arm_subaxis.rotate(axis=axis_,
angle=radians(2))
rate(30)
arm.update_parts()
print('Taiqi done')
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 rotate(self, layer, n=45):
# print(layer)
layerIndex = self.faceIndexDict[layer]
move = self.moves[layerIndex]
for cycle in move:
# print("cycle:",cycle)
objectPositions = [vObject.pos for vObject in cycle]
print("positions:", objectPositions)
center = vpy.vector(0, 0, 0)
for pos in objectPositions:
center += pos
center = center / len(objectPositions)
# center = sum(objectPositions)/len(objectPositions)
print("center of mass", center)
for object1, object2 in zip(cycle, cycle[1:] + [cycle[0]]):
print(object1, object2)
object1pos = object1.pos - center
object2pos = object2.pos - center
coefficientMatrix = [[object1pos.x, object1pos.y],
[object2pos.x, object2pos.y]]
solutionVector = [-object1pos.z, -object2pos.z] # z = 1
x, y = np.linalg.solve(coefficientMatrix, solutionVector)
print(f"axis: ({x},{y},1)")
# y = ( (object1pos.z - object1pos.x) / (object2pos.y * object2pos.z) ) / ((object1pos.y - object1pos.x) / object2pos.y * object2pos.y )
# x = (object1pos.z - object1pos.y * y) / object1pos.x
# z = -1
axis = vpy.vector(x, y, 1)
for _ in range(n):
object1.rotate(angle=vpy.radians(90 / n),
axis=axis,
origin=center)
time.sleep(0.02)
def moving_waizhan(arm=arm):
# Waizhan
print('Waizhan starts ...')
for _ in range(30):
arm.upper_arm_axis = arm.upper_arm_axis.rotate(
axis=arm.upper_arm_subaxis, angle=radians(-2))
rate(30)
arm.update_parts()
print('Waizhan done.')
def motion_quzhou(angle=radians(2)):
print('Quzhou ...')
for _ in range(55):
rate(30)
axis = joint_axis[0].axis.cross(joint_axis[1].axis)
origin = joint_anchor.pos
[e.rotate(origin=origin, angle=angle, axis=axis)
for e in [small_arm, hand_anchor] + joint_axis]
print('Done.')
def update(self, dt):
accel_pitch, accel_roll = self.imu.read_accelerometer_pitch_and_roll()
gyro = self.imu.read_gyroscope()
# By filtering 95% gyro (which changes quickly, but drifts) with the 5% accel, which is absolute, but is slow when filtered
# We get the best of both sensors.
self.pitch = self.filter(self.pitch + gyro.y * dt, accel_pitch)
self.roll = self.filter(self.roll + gyro.x * dt, accel_roll)
# read the magnetometer
mag = self.imu.read_magnetometer()
# Compensate for pitch and tilt
mag = mag.rotate(radians(self.pitch), vector(0, 1, 0))
mag = mag.rotate(radians(self.roll), vector(1, 0, 0))
mag_yaw = -degrees(atan2(mag.y, mag.x))
self.yaw = self.filter(self.yaw + gyro.z * dt, mag_yaw)
print(mag_yaw, self.yaw)
def motion_waizhan(angle=radians(2/1.5)):
print('Waizhan ...')
for _ in range(30):
rate(30)
axis = - joint_axis[1].axis
origin = shoulder_anchor.pos
[e.rotate(origin=origin, angle=angle, axis=axis)
for e in [upper_arm, joint_anchor, small_arm, hand_anchor] +
shoulder_axis + joint_axis]
print('Done.')
def rotate(self, layer, n=45):
toplayer, axis = self.getLayer(layer)
time.sleep(0.1)
# print(axis)
# vpy.arrow(canvas = self.canvas, axis=axis, pos=vpy.vector(5,0,0))
# vpy.arrow(canvas = self.canvas, axis=axis, pos=vpy.vector(0,5,0))
# vpy.arrow(canvas = self.canvas, axis=axis, pos=vpy.vector(0,0,5))
for i in range(n):
for box in toplayer:
box.rotate(angle=vpy.radians(90 / n), axis=axis, origin=R.zero)
time.sleep(0.02)
def rotation_C0(ta, C0=C0):
if C0.axis == ta:
return True
cv = C0.axis.norm().cross(ta.norm())
if cv.dot(cv) < 0.1:
rate(100)
C0.axis = ta.norm() * 10
return False
rate(100)
C0.rotate(angle=radians(1), axis=cv)
return False
def motion_drag(data):
"""
Create animation for projectile motion with dragging force
"""
ball_wd = sphere(pos=vector(0, data['init_height'], 0),
radius=1,
color=color.orange,
make_trail=True)
scene.camera.follow(ball_wd)
data["x2"] = []
data['vx2'] = []
data["y2"] = []
data['vy2'] = []
x = ball_wd.pos.x
y = ball_wd.pos.y
beta = data['beta']
g = data['gravity']
dt = data['deltat']
data['vx2'].append(data['init_velocity'] * cos(radians(data['theta'])))
data['vy2'].append(data['init_velocity'] * sin(radians(data['theta'])))
rate(1000)
while y >= 0:
speed = vector(x, y, 0).mag
data['vx2'].append(data['vx2'][-1] * (1.0 - beta * speed * dt))
data['vy2'].append(data['vy2'][-1] +
(g - beta * speed * data['vy2'][-1]) * dt)
data["x2"].append(data['vx2'][-1] * dt + x)
data["y2"].append(data['vy2'][-1] * dt + y)
# ball_wd.pos = vector(data["x2"][-1], data["y2"][-1], 0)
x = data["x2"][-1]
y = data["y2"][-1]
for x, y in zip(data["x2"], data["y2"]):
rate(1000)
ball_wd.pos.x = x
ball_wd.pos.y = y
def update_small_arm(self):
# cal axis 0, 1, 2
axis_0 = self.upper_arm_axis.norm()
axis_2 = self.upper_arm_subaxis.norm()
axis_1 = axis_0.cross(axis_2).norm()
# update small_arm
self.small_arm_x.pos = self.joint_pos
self.small_arm_x.axis = axis_0 * self.note_len
self.small_arm_x.rotate(axis=axis_1,
angle=radians(self.small_arm_flex))
self.small_arm_y.pos = self.joint_pos
self.small_arm_y.axis = axis_1 * self.note_len
self.small_arm_z.pos = self.joint_pos
self.small_arm_z.axis = axis_2 * self.note_len
self.small_arm_z.rotate(axis=axis_1,
angle=radians(self.small_arm_flex))
self.small_arm.pos = self.joint.pos
self.small_arm.axis = axis_0 * self.small_arm_len
self.small_arm.rotate(axis=axis_1, angle=radians(self.small_arm_flex))
def motion_taishou(angle=radians(2)):
print('Taishouing ...')
for _ in range(35):
rate(30)
if _ % 5 == 0:
axis = joint_axis[0].axis.cross(joint_axis[1].axis)
origin = joint_anchor.pos
[e.rotate(origin=origin, angle=angle, axis=axis)
for e in [small_arm, hand_anchor] + joint_axis]
axis = shoulder_axis[0].axis.cross(shoulder_axis[1].axis)
origin = shoulder_anchor.pos
[e.rotate(origin=origin, angle=angle, axis=axis)
for e in [upper_arm, joint_anchor, small_arm, hand_anchor] +
shoulder_axis + joint_axis]
print('Done.')
def motion_backing_small_arm(angle=radians(2)):
# return False, if we are fine
if all([abs(joint_axis[0].axis.diff_angle(small_arm_main_axis)) < angle,
abs(joint_axis[1].axis.diff_angle(small_arm_sub_axis)) < angle]):
return False
origin = joint_anchor.pos
# back main_axis
if abs(joint_axis[0].axis.diff_angle(small_arm_main_axis)) > angle:
axis = joint_axis[0].axis.cross(small_arm_main_axis)
[e.rotate(origin=origin, axis=axis, angle=angle)
for e in [small_arm, hand_anchor] + joint_axis]
return True
# back sub_axis
if abs(joint_axis[1].axis.diff_angle(small_arm_sub_axis)) > angle:
axis = joint_axis[1].axis.cross(small_arm_sub_axis)
[e.rotate(origin=origin, axis=axis, angle=angle)
for e in [small_arm, hand_anchor] + joint_axis]
return True
def motion_shenchu(angle=radians(2)):
print('Shenchuing ...')
for _ in range(40):
rate(30)
axis = joint_axis[0].axis.cross(joint_axis[1].axis)
origin = joint_anchor.pos
[e.rotate(origin=origin, angle=angle, axis=axis)
for e in [small_arm, hand_anchor] + joint_axis]
time.sleep(0.5)
for __ in range(2):
for _ in range(30):
rate(30)
axis = shoulder_axis[0].axis.cross(shoulder_axis[1].axis)
origin = shoulder_anchor.pos
[e.rotate(origin=origin, angle=angle, axis=axis)
for e in [upper_arm, joint_anchor, small_arm, hand_anchor] +
shoulder_axis + joint_axis]
axis = joint_axis[0].axis.cross(joint_axis[1].axis)
origin = joint_anchor.pos
[e.rotate(origin=origin, angle=-angle, axis=axis)
for e in [small_arm, hand_anchor] + joint_axis]
time.sleep(0.2)
for _ in range(30):
rate(30)
axis = shoulder_axis[0].axis.cross(shoulder_axis[1].axis)
origin = shoulder_anchor.pos
[e.rotate(origin=origin, angle=-angle, axis=axis)
for e in [upper_arm, joint_anchor, small_arm, hand_anchor] +
shoulder_axis + joint_axis]
axis = joint_axis[0].axis.cross(joint_axis[1].axis)
origin = joint_anchor.pos
[e.rotate(origin=origin, angle=angle, axis=axis)
for e in [small_arm, hand_anchor] + joint_axis]
print('Done.')
def create_bodies():
ramp = vp.box(pos=vp.vector(0, -0.25, 0),
length=5,
width=2.5,
height=0.5,
alpha=vp.radians(15))
# Minus, because function (vp.rotate) rotate counter-clockwise
ramp.rotate(angle=-ramp.alpha, axis=vp.vector(0, 0, 1))
roller = vp.cylinder(pos=vp.vector(
-1 * math.cos(ramp.alpha) * 0.5 * ramp.length,
math.sin(ramp.alpha) * 0.5 * ramp.length + 0.5, -1),
axis=vp.vector(0, 0, 2),
radius=0.5,
texture=texture(),
vel=vp.vector(0, 0, 0),
ang_vel=0,
mass=1,
friction_coeff=0.15)
slope_dir = vp.norm(vp.rotate(vp.vector(1, 0, 0), angle=-ramp.alpha))
arrow = vp.arrow(pos=vp.vector(0, 2, 1), axis=slope_dir)
return roller, ramp, arrow
def back_parts(self):
# cal axis 0, 1, 2
axis_0 = self.upper_arm_axis.norm()
axis_2 = self.upper_arm_subaxis.norm()
# return False, if we are fine
if all([(abs(axis_0.diff_angle(-self.up)) < radians(3)),
(abs(axis_2.diff_angle(self.front)) < radians(3)),
(self.small_arm_flex == 30)]):
return False
# back axis_0
if abs(axis_0.diff_angle(-self.up)) < radians(3):
axis_0 = -self.up
else:
axis_0 = axis_0.rotate(axis=axis_0.cross(-self.up),
angle=radians(2))
# back axis_2
if abs(axis_2.diff_angle(self.front)) < radians(3):
axis_2 = self.front
else:
axis_2 = axis_2.rotate(axis=axis_2.cross(self.front),
angle=radians(2))
# setting backed parameters
self.upper_arm_axis = axis_0.norm()
self.upper_arm_subaxis = axis_2.norm()
if abs(self.small_arm_flex) - 30 < 3:
self.small_arm_flex = 30
else:
if self.small_arm_flex > 30:
self.small_arm_flex -= 2
else:
self.small_arm_flex += 2
return True
def rollRight(self, angle=22.5):
self.upDirection = self.upDirection.rotate(radians(-angle),
self.heading)
self.leftDirection = self.leftDirection.rotate(radians(-angle),
self.heading)
# coding: utf-8
from vpython import compound, color, vector, box, radians, rate
box1 = box(pos=vector(1, 0, 0), length=1, width=2, height=3, color=color.red)
box2 = box(pos=vector(5, 0, 0), length=1, width=2, height=3, color=color.blue)
bb = compound([box1, box2])
print(bb)
print(box2)
while True:
rate(30)
bb.rotate(axis=vector(1, 0, 0), angle=radians(2))
# invalid for box1 and box2
from vpython import arrow, box, color, compound, radians, rate, vector
O = vector(0, 0, 0)
up = vector(1, 0, 0)
right = vector(0, 1, 0)
front = vector(0, 0, 1)
arrow(pos=O, axis=up, color=color.red)
arrow(pos=O, axis=right, color=color.green)
arrow(pos=O, axis=front, color=color.blue)
box1 = box(pos=O + vector(0, 10, 0),
width=1,
height=2,
length=3,
color=color.cyan,
opacity=0.5)
box2 = box(pos=O + vector(0, -10, 0),
width=1,
height=2,
length=3,
color=color.cyan,
opacity=0.5)
cb = compound([box1, box2])
while True:
rate(30)
cb.rotate(axis=up, angle=radians(2))
bbox.shininess = 0
# body init
body = draw_part('body')
skincolor = vector(1, 0.7, 0.2)
# right_leg init
draw_part('upper_right_leg', skincolor)
upper_right_leg = draw_part('upper_right_leg', skincolor)
small_right_leg = draw_part('small_right_leg', skincolor)
# right_leg sit motion
main_axis = (p0_right_leg-p1_right_leg).norm()
sub_axis = main_axis.cross(front).cross(main_axis).norm()
small_right_leg.rotate(origin=p1_right_leg,
axis=sub_axis.cross(main_axis), angle=radians(90))
s = sphere(pos=p1_right_leg+vector(1, 0, 0), radius=1, color=skincolor)
main_axis = (p1_right_leg-p2_right_leg).norm()
sub_axis = main_axis.cross(front).cross(main_axis).norm()
[e.rotate(origin=p2_right_leg, axis=main_axis.cross(sub_axis), angle=radians(90))
for e in [upper_right_leg, small_right_leg, s]]
# left_leg init
draw_part('upper_left_leg', skincolor)
upper_left_leg = draw_part('upper_left_leg', skincolor)
small_left_leg = draw_part('small_left_leg', skincolor)
# left_leg sit motion
main_axis = (p0_left_leg-p1_left_leg).norm()
sub_axis = main_axis.cross(front).cross(main_axis).norm()
small_left_leg.rotate(origin=p1_left_leg,
axis=sub_axis.cross(main_axis), angle=radians(90))
def get_mappings_score(self, target_aabb, source_aabb):
"""
Returns analogy scores for all mappings of two AABB objects.
Args:
target_aabb(AABB): The target AABB that we want to map to source AABB.
source_aabb(AABB): The source AABB that from source scene.
Returns:
Analogy scores for all mappings (sequence permutations) of two AABB
objects based on collided sides simmilarity, ratio of AABBs,
matching surfaces.
"""
# collision match weights
exact_collision_weight = 0.99 #0.9
partial_collision_weight = 0.5 #0.4
no_collision_match_weight = .0 #-0.1
# surface match weights
top_match_weight = .02 #0.1
bottom_match_weight = .02 #0.1
front_match_weight = .01 #0.01
back_match_weight = .01 #0.01
right_match_weight = .01 #0.01
left_match_weight = .01 #0.01
no_surf_match_weight = .0 #-0.01
# surface ratio difference penalties
xy_ratio_weight = 0.1
zy_ratio_weight = 0.1
xz_ratio_weight = 0.1
mappings_score = []
for perm_sequence in self.all_permutations.keys():
score = 1.0 # default score
for surface in self.all_permutations[perm_sequence].keys():
# scores for exact collision/partial/no-collision match
if target_aabb.collided_sides[
surface] == source_aabb.collided_sides[
self.all_permutations[perm_sequence][surface]]:
score += 1.0 * exact_collision_weight
# match partially collided sides and fully collided sides
elif target_aabb.collided_sides[
surface] != 0 and source_aabb.collided_sides[
self.all_permutations[perm_sequence]
[surface]] != 0:
score += 1.0 * partial_collision_weight
else: # there is no match
score += 1.0 * no_collision_match_weight
# score for surface match
if (surface == 'top'
and self.all_permutations[perm_sequence][surface]
== 'top'):
score += 1.0 * top_match_weight
elif (surface == 'bottom'
and self.all_permutations[perm_sequence][surface]
== 'bottom'):
score += 1.0 * bottom_match_weight
elif (surface == 'front'
and self.all_permutations[perm_sequence][surface]
== 'front'):
score += 1.0 * front_match_weight
elif (surface == 'back' and
self.all_permutations[perm_sequence][surface] == 'back'):
score += 1.0 * back_match_weight
elif (surface == 'right'
and self.all_permutations[perm_sequence][surface]
== 'right'):
score += 1.0 * right_match_weight
elif (surface == 'left' and
self.all_permutations[perm_sequence][surface] == 'left'):
score += 1.0 * left_match_weight
else:
score += 1.0 * no_surf_match_weight
# create vpython vec for xyz half size of the target object
vpython_vec_xyz = vpython.vec(target_aabb.half_size[0],
target_aabb.half_size[1],
target_aabb.half_size[2])
# rotate it based on rotation sequence
for rotation in reversed(perm_sequence):
if rotation == 'x':
vpython_vec_xyz = vpython.rotate(
vpython_vec_xyz,
angle=vpython.radians(-90),
axis=vpython.vec(1, 0, 0))
elif rotation == 'y':
vpython_vec_xyz = vpython.rotate(
vpython_vec_xyz,
angle=vpython.radians(-90),
axis=vpython.vec(0, 1, 0))
# compare xy, zy, xz ratios of target and source aabbs after rotation
# and penalise for difference
xy_ratio_diff = abs(
(abs(vpython_vec_xyz.x) / abs(vpython_vec_xyz.y)) -
(source_aabb.half_size[0] / source_aabb.half_size[1]))
xy_ratio_diff *= xy_ratio_weight
zy_ratio_diff = abs(
(abs(vpython_vec_xyz.z) / abs(vpython_vec_xyz.y)) -
(source_aabb.half_size[2] / source_aabb.half_size[1]))
zy_ratio_diff *= zy_ratio_weight
xz_ratio_diff = abs(
(abs(vpython_vec_xyz.x) / abs(vpython_vec_xyz.z)) -
(source_aabb.half_size[0] / source_aabb.half_size[2]))
xz_ratio_diff *= xz_ratio_weight
sum_ratio_diff = xy_ratio_diff + zy_ratio_diff + xz_ratio_diff
# penalise for ratio difference
score -= sum_ratio_diff
mappings_score.append((score, perm_sequence))
return mappings_score
def __create_object(self):
"""
Create the physical graphical object
:returns: The graphical entity
:rtype: class:`vpython.baseobj`
"""
if self.shape == '':
# 2D coords of the circle boundary
shape_path = shapes.circle(radius=self.__marker_size / 2)
elif self.shape == '+':
# 2D coords of the cross boundary
shape_path = shapes.cross(width=self.__marker_size, thickness=self.__marker_size / 5)
elif self.shape == 'o':
# 2D coords of the circle boundary
shape_path = shapes.circle(radius=self.__marker_size / 2)
elif self.shape == '*':
# 2D coords of the star boundary
shape_path = shapes.star(radius=self.__marker_size / 2, n=6)
elif self.shape == '.':
# 2D coords of the square boundary
shape_path = shapes.rectangle(width=self.__marker_size, height=self.__marker_size)
elif self.shape == 'x':
# 2D coords of the cross boundary
shape_path = shapes.cross(width=self.__marker_size, thickness=self.__marker_size / 5, rotate=radians(45))
elif self.shape == 's':
# 2D coords of the square boundary
shape_path = shapes.rectangle(width=self.__marker_size, height=self.__marker_size,
thickness=self.__marker_size)
elif self.shape == 'd':
# 2D coords of the diamond boundary
shape_path = shapes.rectangle(width=self.__marker_size, height=self.__marker_size,
thickness=self.__marker_size, rotate=radians(45))
elif self.shape == '^':
# 2D coords of the triangle boundary
shape_path = shapes.triangle(length=self.__marker_size)
elif self.shape == 'v':
# 2D coords of the triangle boundary
shape_path = shapes.triangle(length=self.__marker_size, rotate=radians(180))
elif self.shape == '<':
# 2D coords of the triangle boundary
shape_path = shapes.triangle(length=self.__marker_size, rotate=radians(90))
elif self.shape == '>':
# 2D coords of the triangle boundary
shape_path = shapes.triangle(length=self.__marker_size, rotate=radians(-90))
elif self.shape == 'p':
# 2D coords of the pentagon boundary
shape_path = shapes.pentagon(length=self.__marker_size)
elif self.shape == 'h':
# 2D coords of the hexagon boundary
shape_path = shapes.hexagon(length=self.__marker_size)
# CURRENTLY UNUSED
# elif self.__shape == 'o':
# # 2D coords of the octagon boundary
# shape_path = shapes.octagon(length=1)
# elif self.__shape == 'r':
# # 2D coords of the ring boundary (with thickness = 10%)
# shape_path = shapes.circle(radius=0.5, thickness=0.1)
else:
raise ValueError("Invalid shape given")
# Create the shape
x = self.se2.t[0]
y = self.se2.t[1]
obj = extrusion(scene=self.scene,
path=[vector(x, y, 0.001), vector(x, y, -0.001)],
shape=shape_path,
color=self.colourVec,
shininess=0)
self.size = obj.size
if self.shape == '':
obj.visible = False
return obj
def _create_hab(self, entity: Entity, texture: str) -> \
vpython.compound:
def vertex(x: float, y: float, z: float) -> vpython.vertex:
return vpython.vertex(pos=vpython.vector(x, y, z))
# See the docstring of ThreeDeeObj._create_obj for why the dimensions
# that define the shape of the habitat will not actually directly
# translate to world-space.
body = vpython.cylinder(pos=vpython.vec(0, 0, 0),
axis=vpython.vec(-5, 0, 0),
radius=10)
head = vpython.cone(pos=vpython.vec(0, 0, 0),
axis=vpython.vec(2, 0, 0),
radius=10)
wing = vpython.triangle(v0=vertex(0, 0, 0),
v1=vertex(-5, 30, 0),
v2=vertex(-5, -30, 0))
wing2 = vpython.triangle(v0=vertex(0, 0, 0),
v1=vertex(-5, 0, 30),
v2=vertex(-5, 0, -30))
hab = vpython.compound([body, head, wing, wing2],
make_trail=True,
texture=texture)
hab.axis = calc.angle_to_vpy(entity.heading)
hab.radius = entity.r / 2
hab.shininess = 0.1
hab.length = entity.r * 2
boosters: List[vpython.cylinder] = []
body_radius = entity.r / 8
for quadrant in range(0, 4):
# Generate four SRB bodies.
normal = vpython.rotate(vpython.vector(0, 1, 1).hat,
angle=quadrant * vpython.radians(90),
axis=vpython.vector(1, 0, 0))
boosters.append(
vpython.cylinder(radius=self.BOOSTER_RADIUS,
pos=(self.BOOSTER_RADIUS + body_radius) *
normal))
boosters.append(
vpython.cone(
radius=self.BOOSTER_RADIUS,
length=0.2,
pos=((self.BOOSTER_RADIUS + body_radius) * normal +
vpython.vec(1, 0, 0))))
# Append an invisible point to shift the boosters down the fuselage.
# For an explanation of why that matters, read the
# ThreeDeeObj._create_obj docstring (and if that doesn't make sense,
# get in touch with Patrick M please hello hi I'm always free!)
boosters.append(vpython.sphere(opacity=0, pos=vpython.vec(1.2, 0, 0)))
booster_texture = texture.replace(f'{entity.name}.jpg', 'SRB.jpg')
hab.boosters = vpython.compound(boosters, texture=booster_texture)
hab.boosters.length = entity.r * 2
hab.boosters.axis = hab.axis
parachute: List[vpython.standardAttributes] = []
string_length = entity.r * 0.5
parachute_texture = texture.replace(f'{entity.name}.jpg',
'Parachute.jpg')
# Build the parachute fabric.
parachute.append(
vpython.extrusion(
path=vpython.paths.circle(radius=0.5, up=vpython.vec(-1, 0,
0)),
shape=vpython.shapes.arc(angle1=vpython.radians(5),
angle2=vpython.radians(95),
radius=1),
pos=vpython.vec(string_length + self.PARACHUTE_RADIUS / 2, 0,
0)))
parachute[0].height = self.PARACHUTE_RADIUS * 2
parachute[0].width = self.PARACHUTE_RADIUS * 2
parachute[0].length = self.PARACHUTE_RADIUS
for quadrant in range(0, 4):
# Generate parachute attachment lines.
string = vpython.cylinder(axis=vpython.vec(string_length,
self.PARACHUTE_RADIUS,
0),
radius=0.2)
string.rotate(angle=(quadrant * vpython.radians(90) -
vpython.radians(45)),
axis=vpython.vector(1, 0, 0))
parachute.append(string)
parachute.append(
vpython.sphere(opacity=0,
pos=vpython.vec(
-(string_length + self.PARACHUTE_RADIUS), 0,
0)))
hab.parachute = vpython.compound(parachute, texture=parachute_texture)
hab.parachute.visible = False
return hab
import virtual_robot
imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets)
pitch = 0
roll = 0
yaw = 0
model = virtual_robot.make_robot()
virtual_robot.robot_view()
latest = time.time()
while True:
vp.rate(1000)
current = time.time()
dt = current - latest
latest = current
gyro = imu.read_gyroscope()
roll += gyro.x * dt
pitch += gyro.y * dt
yaw += gyro.z * dt
# reset the model
model.up = vp.vector(0, 1, 0)
model.axis = vp.vector(1, 0, 0)
# Reposition it
model.rotate(angle=vp.radians(roll), axis=vp.vector(1, 0, 0))
model.rotate(angle=vp.radians(pitch), axis=vp.vector(0, 1, 0))
model.rotate(angle=vp.radians(yaw), axis=vp.vector(0, 0, 1))
model = virtual_robot.make_robot()
virtual_robot.robot_view()
compass = vp.canvas(width=400, height=400)
vp.cylinder(radius=1, axis=vp.vector(0, 0, 1), pos=vp.vector(0, 0, -1))
needle = vp.arrow(axis=vp.vector(1, 0, 0), color=vp.color.red)
vp.graph(xmin=0, xmax=60, scroll=True)
graph_roll = vp.gcurve(color=vp.color.red)
graph_pitch = vp.gcurve(color=vp.color.green)
graph_yaw = vp.gcurve(color=vp.color.blue)
timer = DeltaTimer()
while True:
vp.rate(100)
dt, elapsed = timer.update()
fusion.update(dt)
# reset the model
model.up = vp.vector(0, 1, 0)
model.axis = vp.vector(1, 0, 0)
# Reposition it
model.rotate(angle=vp.radians(fusion.roll), axis=vp.vector(1, 0, 0))
model.rotate(angle=vp.radians(fusion.pitch), axis=vp.vector(0, 1, 0))
model.rotate(angle=vp.radians(fusion.yaw), axis=vp.vector(0, 0, 1))
needle.axis = vp.vector(vp.sin(vp.radians(fusion.yaw)),
vp.cos(vp.radians(fusion.yaw)), 0)
graph_roll.plot(elapsed, fusion.roll)
graph_pitch.plot(elapsed, fusion.pitch)
graph_yaw.plot(elapsed, fusion.yaw)
