def _quadrotor_3dmodel(self, model):
# params["body"] = {"l": 0.03, "w": 0.03, "h": 0.004, "m": 0.005}
# params["payload"] = {"l": 0.035, "w": 0.02, "h": 0.008, "m": 0.01}
# params["arms"] = {"l": 0.022, "w":0.005, "h":0.005, "m":0.001}
# params["motors"] = {"h":0.02, "r":0.0035, "m":0.0015}
# params["propellers"] = {"h":0.002, "r":0.022, "m":0.00075}
# params["motor_pos"] = {"xyz": [0.065/2, 0.065/2, 0.]}
# params["arms_pos"] = {"angle": 45., "z": 0.}
# params["payload_pos"] = {"xy": [0., 0.], "z_sign": 1}
## PROPELLERS
# "X" propeller configuration, start fwd left, go clockwise
# IDs: https://wiki.bitcraze.io/projects:crazyflie2:userguide:assembly
link_colors = {
"body": (0.67843137, 1., 0.18431373),
"payload": (0., 0., 1.),
"prop_0": (1, 0, 0),
"prop_1": (0, 1, 0),
"prop_2": (0, 1, 0),
"prop_3": (1, 0, 0),
"motor_0": (0, 0, 0),
"motor_1": (0, 0, 0),
"motor_2": (0, 0, 0),
"motor_3": (0, 0, 0),
"arm_0": (0, 0, 1),
"arm_1": (0, 0, 1),
"arm_2": (0, 0, 1),
"arm_3": (0, 0, 1),
}
links = []
for i, link in enumerate(model.links):
xyz, R, color = model.poses[i].xyz, model.poses[i].R, link_colors[
link.name]
rot = np.eye(4)
rot[:3, :3] = R
# print("LINK: ", link.name, "R:", rot, end=" ")
if link.name[:4] == "prop":
prop_r = link.r
color = 0.5 * np.array(color) + 0.2
if link.type == "box":
# print("Type: Box")
link_transf = r3d.transform_and_color(
np.matmul(r3d.translate(xyz), rot), color,
r3d.box(link.l, link.w, link.h))
elif link.type == "cylinder":
# print("Type: Cylinder")
link_transf = r3d.transform_and_color(
r3d.translate(xyz), color,
r3d.cylinder(link.r, link.h, 32))
links.append(link_transf)
## ARROWS
arrow = r3d.Color((0.2, 0.3, 0.9),
r3d.arrow(0.12 * prop_r, 2.5 * prop_r, 16))
links.append(arrow)
self.have_state = False
return r3d.Transform(np.eye(4), links)
def _quadrotor_simple_3dmodel(self, diam):
r = diam / 2
prop_r = 0.3 * diam
prop_h = prop_r / 15.0
# "X" propeller configuration, start fwd left, go clockwise
rr = r * np.sqrt(2) / 2
deltas = ((rr, rr, 0), (rr, -rr, 0), (-rr, -rr, 0), (-rr, rr, 0))
colors = ((1, 0, 0), (1, 0, 0), (0, 1, 0), (0, 1, 0))
def disc(translation, color):
color = 0.5 * np.array(list(color)) + 0.2
disc = r3d.transform_and_color(r3d.translate(translation), color,
r3d.cylinder(prop_r, prop_h, 32))
return disc
props = [disc(d, c) for d, c in zip(deltas, colors)]
arm_thicc = diam / 20.0
arm_color = (0.6, 0.6, 0.6)
arms = r3d.transform_and_color(
np.matmul(r3d.translate((0, 0, -arm_thicc)), r3d.rotz(np.pi / 4)),
arm_color, [
r3d.box(diam / 10, diam, arm_thicc),
r3d.box(diam, diam / 10, arm_thicc)
])
arrow = r3d.Color((0.2, 0.3, 0.9),
r3d.arrow(0.12 * prop_r, 2.5 * prop_r, 16))
bodies = props + [arms, arrow]
self.have_state = False
return r3d.Transform(np.eye(4), bodies)
def update_goal(self, goal, goal_index):
self.goal_transforms[goal_index].set_transform(r3d.translate(
goal[0:3]))
self.goal_arrows[goal_index * 3].set_transform(
r3d.trans_and_rot(goal[0:3], self.goal_arrows_rot[0]))
self.goal_arrows[goal_index * 3 + 1].set_transform(
r3d.trans_and_rot(goal[0:3], self.goal_arrows_rot[1]))
self.goal_arrows[goal_index * 3 + 2].set_transform(
r3d.trans_and_rot(goal[0:3], self.goal_arrows_rot[2]))
def update_goal(self, goal):
self.goal_transform.set_transform(r3d.translate(goal[0:3]))
self.goal_arrows[0].set_transform(
r3d.trans_and_rot(goal[0:3], self.goal_arrows_rot[0]))
self.goal_arrows[1].set_transform(
r3d.trans_and_rot(goal[0:3], self.goal_arrows_rot[1]))
self.goal_arrows[2].set_transform(
r3d.trans_and_rot(goal[0:3], self.goal_arrows_rot[2]))
def update_state(self, dynamics, goal):
if self.scene:
self.chase_cam.step(dynamics.pos, dynamics.vel)
self.have_state = True
self.fpv_lookat = dynamics.look_at()
self.update_goal(goal=goal)
matrix = r3d.trans_and_rot(dynamics.pos, dynamics.rot)
self.quad_transform.set_transform_nocollide(matrix)
shadow_pos = 0 + dynamics.pos
shadow_pos[2] = 0.001 # avoid z-fighting
matrix = r3d.translate(shadow_pos)
self.shadow_transform.set_transform_nocollide(matrix)
def update_state(self, dynamics, goals, reached=None):
if self.scene:
self.chase_cam.step(dynamics.pos, dynamics.vel)
self.have_state = True
self.fpv_lookat = dynamics.look_at()
for i in range(self.goal_count):
self.update_goal(goal=goals[i * 3:i * 3 + 3], goal_index=i)
if reached is not None and reached[i]: # change to green
self.update_goal_color(goal=goals[i * 3:i * 3 + 3],
goal_index=i)
matrix = r3d.trans_and_rot(dynamics.pos, dynamics.rot)
self.quad_transform.set_transform_nocollide(matrix)
shadow_pos = 0 + dynamics.pos
shadow_pos[2] = 0.001 # avoid z-fighting
matrix = r3d.translate(shadow_pos)
self.shadow_transform.set_transform_nocollide(matrix)
def _random_obstacles(np_random, N, arena, our_radius):
arena = float(arena)
# all primitives should be tightly bound by unit circle in xy plane
boxside = np.sqrt(2)
box = r3d.box(boxside, boxside, boxside)
sphere = r3d.sphere(radius=1.0, facets=16)
cylinder = r3d.cylinder(radius=1.0, height=2.0, sections=32)
# TODO cone-sphere collision
#cone = r3d.cone(radius=0.5, height=1.0, sections=32)
primitives = [box, sphere, cylinder]
bodies = []
max_radius = 2.0
positions, radii, test_list = _place_obstacles(
np_random, N, arena, (0.5, max_radius), our_radius)
for i in range(N):
primitive = np_random.choice(primitives)
tex_type = r3d.random_textype()
tex_dark = 0.5 * np_random.uniform()
tex_light = 0.5 * np_random.uniform() + 0.5
color = 0.5 * np_random.uniform(size=3)
heightscl = np.random.uniform(0.5, 2.0)
height = heightscl * 2.0 * radii[i]
z = (0 if primitive is cylinder else
(height/2.0 if primitive is sphere else
(height*boxside/4.0 if primitive is box
else np.nan)))
translation = np.append(positions[i,:], z)
matrix = np.matmul(r3d.translate(translation), r3d.scale(radii[i]))
matrix = np.matmul(matrix, np.diag([1, 1, heightscl, 1]))
body = r3d.Transform(matrix,
#r3d.ProceduralTexture(tex_type, (tex_dark, tex_light), primitive))
r3d.Color(color, primitive))
bodies.append(body)
return ObstacleMap(arena, bodies, test_list)
def disc(translation, color):
color = 0.5 * np.array(list(color)) + 0.2
disc = r3d.transform_and_color(r3d.translate(translation), color,
r3d.cylinder(prop_r, prop_h, 32))
return disc
