Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
load_vel = np.array([state[13], state[14], state[15]]).flatten()
current_quat = Quaternion(att)
x_axis = current_quat.rotation_matrix.dot(np.array([1.0, 0.0, 0.0]))
y_axis = current_quat.rotation_matrix.dot(np.array([0.0, 1.0, 0.0]))
z_axis = current_quat.rotation_matrix.dot(np.array([0.0, 0.0, 1.0]))
tether_vec = load_pos - pos
if self.viewer is None:
self.viewer = canvas(title='Quadrotor 3D Slungload', width=640, height=480, center=vector(0, 0, 0), forward=vector(1, 1, -1), up=vector(0, 0, 1), background=color.white)
self.render_quad1 = box(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(x_axis[0],x_axis[1],x_axis[2]), length=0.2, height=0.05, width=0.05)
self.render_quad2 = box(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(y_axis[0],y_axis[1],y_axis[2]), length=0.2, height=0.05, width=0.05)
self.render_rotor1 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor2 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor3 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor4 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_velocity = pointer = arrow(pos=vector(pos[0],pos[1],0), axis=vector(vel[0],vel[1],vel[2]), shaftwidth=0.05, color=color.green)
self.render_ref = sphere(canvas = self.viewer, pos=vector(ref_pos[0], ref_pos[1], ref_pos[2]), radius=0.02, color=color.blue, make_trail = True)
self.render_tether = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(tether_vec[0],tether_vec[1],tether_vec[2]), radius=0.01, color=color.black)
self.render_load = sphere(canvas = self.viewer, pos=vector(load_pos[0], load_pos[1], load_pos[2]), radius=0.1, color=color.red, make_trail = True)
if self.state is None: return None
self.render_quad1.pos.x = pos[0]
self.render_quad1.pos.y = pos[1]
self.render_quad1.pos.z = pos[2]
self.render_quad2.pos.x = pos[0]
self.render_quad2.pos.y = pos[1]
self.render_quad2.pos.z = pos[2]
rotor_pos = 0.5*x_axis
self.render_rotor1.pos.x = pos[0] + rotor_pos[0]
self.render_rotor1.pos.y = pos[1] + rotor_pos[1]
self.render_rotor1.pos.z = pos[2] + rotor_pos[2]
else:
col = vpython.color.blue
s = IPsphere(pos=vpython.vec((tmp_len - 1) * vpython.cos(2 * i * vpython.pi / tmp_len), (tmp_len - 1) * vpython.sin(2 * i * vpython.pi / tmp_len), 2 * t), # noqa: E501
ip=r[i][0],
color=col)
for trlst in six.itervalues(tr3d):
if t <= len(trlst):
if trlst[t - 1] == i:
trlst[t - 1] = s
forecol = colgen(0.625, 0.4375, 0.25, 0.125)
for trlst in six.itervalues(tr3d):
col = vpython.vec(*next(forecol))
start = vpython.vec(0, 0, 0)
for ip in trlst:
vpython.cylinder(pos=start, axis=ip.pos - start, color=col, radius=0.2) # noqa: E501
start = ip.pos
vpython.rate(50)
# Keys handling
# TODO: there is currently no way of closing vpython correctly
# https://github.com/BruceSherwood/vpython-jupyter/issues/36
# def keyboard_press(ev):
# k = ev.key
# if k == "esc" or k == "q":
# pass # TODO: close
#
# vpython.scene.bind('keydown', keyboard_press)
# Mouse handling
def mouse_click(ev):
vel = np.array([state[7], state[8], state[9]]).flatten()
load_pos = np.array([state[10], state[11], state[12]]).flatten()
load_vel = np.array([state[13], state[14], state[15]]).flatten()
current_quat = Quaternion(att)
x_axis = current_quat.rotation_matrix.dot(np.array([1.0, 0.0, 0.0]))
y_axis = current_quat.rotation_matrix.dot(np.array([0.0, 1.0, 0.0]))
z_axis = current_quat.rotation_matrix.dot(np.array([0.0, 0.0, 1.0]))
tether_vec = load_pos - pos
if self.viewer is None:
self.viewer = canvas(title='Quadrotor 3D Slungload', width=640, height=480, center=vector(0, 0, 0), forward=vector(1, 1, -1), up=vector(0, 0, 1), background=color.white)
self.render_quad1 = box(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(x_axis[0],x_axis[1],x_axis[2]), length=0.2, height=0.05, width=0.05)
self.render_quad2 = box(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(y_axis[0],y_axis[1],y_axis[2]), length=0.2, height=0.05, width=0.05)
self.render_rotor1 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor2 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor3 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor4 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_velocity = pointer = arrow(pos=vector(pos[0],pos[1],0), axis=vector(vel[0],vel[1],vel[2]), shaftwidth=0.05, color=color.green)
self.render_ref = sphere(canvas = self.viewer, pos=vector(ref_pos[0], ref_pos[1], ref_pos[2]), radius=0.02, color=color.blue, make_trail = True)
self.render_tether = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(tether_vec[0],tether_vec[1],tether_vec[2]), radius=0.01, color=color.black)
self.render_load = sphere(canvas = self.viewer, pos=vector(load_pos[0], load_pos[1], load_pos[2]), radius=0.1, color=color.red, make_trail = True)
if self.state is None: return None
self.render_quad1.pos.x = pos[0]
self.render_quad1.pos.y = pos[1]
self.render_quad1.pos.z = pos[2]
self.render_quad2.pos.x = pos[0]
self.render_quad2.pos.y = pos[1]
self.render_quad2.pos.z = pos[2]
rotor_pos = 0.5*x_axis
self.render_rotor1.pos.x = pos[0] + rotor_pos[0]
else:
col = vpython.color.blue
s = IPsphere(pos=vpython.vec((tmp_len - 1) * vpython.cos(2 * i * vpython.pi / tmp_len), (tmp_len - 1) * vpython.sin(2 * i * vpython.pi / tmp_len), 2 * t), # noqa: E501
ip=r[i][0],
color=col)
for trlst in six.itervalues(tr3d):
if t <= len(trlst):
if trlst[t - 1] == i:
trlst[t - 1] = s
forecol = colgen(0.625, 0.4375, 0.25, 0.125)
for trlst in six.itervalues(tr3d):
col = vpython.vec(*next(forecol))
start = vpython.vec(0, 0, 0)
for ip in trlst:
vpython.cylinder(pos=start, axis=ip.pos - start, color=col, radius=0.2) # noqa: E501
start = ip.pos
vpython.rate(50)
# Keys handling
# TODO: there is currently no way of closing vpython correctly
# https://github.com/BruceSherwood/vpython-jupyter/issues/36
# def keyboard_press(ev):
# k = ev.key
# if k == "esc" or k == "q":
# pass # TODO: close
#
# vpython.scene.bind('keydown', keyboard_press)
# Mouse handling
def mouse_click(ev):
pos = np.array([state[0], state[1], state[2]]).flatten()
att = np.array([state[3], state[4], state[5], state[6]]).flatten()
vel = np.array([state[7], state[8], state[9]]).flatten()
current_quat = Quaternion(att)
x_axis = current_quat.rotation_matrix.dot(np.array([1.0, 0.0, 0.0]))
y_axis = current_quat.rotation_matrix.dot(np.array([0.0, 1.0, 0.0]))
z_axis = current_quat.rotation_matrix.dot(np.array([0.0, 0.0, 1.0]))
if self.viewer is None:
self.viewer = canvas(title='Quadrotor 3D', width=640, height=480, center=vector(0, 0, 2), forward=vector(1, 1, -0.5), up=vector(0, 0, 1), background=color.white, range=4.0, autoscale = False)
self.render_quad1 = box(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(x_axis[0],x_axis[1],x_axis[2]), length=0.2, height=0.05, width=0.05)
self.render_quad2 = box(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(y_axis[0],y_axis[1],y_axis[2]), length=0.2, height=0.05, width=0.05)
self.render_rotor1 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor2 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor3 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor4 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_velocity = pointer = arrow(pos=vector(pos[0],pos[1],0), axis=vector(vel[0],vel[1],vel[2]), shaftwidth=0.05, color=color.green)
self.render_ref = sphere(canvas = self.viewer, pos=vector(ref_pos[0], ref_pos[1], ref_pos[2]), radius=0.02, color=color.blue, make_trail = True)
grid_xy = make_grid(5, 100)
if self.state is None: return None
self.render_quad1.pos.x = pos[0]
self.render_quad1.pos.y = pos[1]
self.render_quad1.pos.z = pos[2]
self.render_quad2.pos.x = pos[0]
self.render_quad2.pos.y = pos[1]
self.render_quad2.pos.z = pos[2]
rotor_pos = 0.5*x_axis
self.render_rotor1.pos.x = pos[0] + rotor_pos[0]
load_pos = np.array([state[10], state[11], state[12]]).flatten()
load_vel = np.array([state[13], state[14], state[15]]).flatten()
current_quat = Quaternion(att)
x_axis = current_quat.rotation_matrix.dot(np.array([1.0, 0.0, 0.0]))
y_axis = current_quat.rotation_matrix.dot(np.array([0.0, 1.0, 0.0]))
z_axis = current_quat.rotation_matrix.dot(np.array([0.0, 0.0, 1.0]))
tether_vec = load_pos - pos
if self.viewer is None:
self.viewer = canvas(title='Quadrotor 3D Slungload', width=640, height=480, center=vector(0, 0, 0), forward=vector(1, 1, -1), up=vector(0, 0, 1), background=color.white)
self.render_quad1 = box(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(x_axis[0],x_axis[1],x_axis[2]), length=0.2, height=0.05, width=0.05)
self.render_quad2 = box(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(y_axis[0],y_axis[1],y_axis[2]), length=0.2, height=0.05, width=0.05)
self.render_rotor1 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor2 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor3 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor4 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_velocity = pointer = arrow(pos=vector(pos[0],pos[1],0), axis=vector(vel[0],vel[1],vel[2]), shaftwidth=0.05, color=color.green)
self.render_ref = sphere(canvas = self.viewer, pos=vector(ref_pos[0], ref_pos[1], ref_pos[2]), radius=0.02, color=color.blue, make_trail = True)
self.render_tether = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(tether_vec[0],tether_vec[1],tether_vec[2]), radius=0.01, color=color.black)
self.render_load = sphere(canvas = self.viewer, pos=vector(load_pos[0], load_pos[1], load_pos[2]), radius=0.1, color=color.red, make_trail = True)
if self.state is None: return None
self.render_quad1.pos.x = pos[0]
self.render_quad1.pos.y = pos[1]
self.render_quad1.pos.z = pos[2]
self.render_quad2.pos.x = pos[0]
self.render_quad2.pos.y = pos[1]
self.render_quad2.pos.z = pos[2]
rotor_pos = 0.5*x_axis
self.render_rotor1.pos.x = pos[0] + rotor_pos[0]
self.render_rotor1.pos.y = pos[1] + rotor_pos[1]
pos = np.array([state[0], state[1], state[2]]).flatten()
att = np.array([state[3], state[4], state[5], state[6]]).flatten()
vel = np.array([state[7], state[8], state[9]]).flatten()
current_quat = Quaternion(att)
x_axis = current_quat.rotation_matrix.dot(np.array([1.0, 0.0, 0.0]))
y_axis = current_quat.rotation_matrix.dot(np.array([0.0, 1.0, 0.0]))
z_axis = current_quat.rotation_matrix.dot(np.array([0.0, 0.0, 1.0]))
if self.viewer is None:
self.viewer = canvas(title='Quadrotor 3D', width=640, height=480, center=vector(0, 0, 2), forward=vector(1, 1, -0.5), up=vector(0, 0, 1), background=color.white, range=4.0, autoscale = False)
self.render_quad1 = box(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(x_axis[0],x_axis[1],x_axis[2]), length=0.2, height=0.05, width=0.05)
self.render_quad2 = box(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(y_axis[0],y_axis[1],y_axis[2]), length=0.2, height=0.05, width=0.05)
self.render_rotor1 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor2 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor3 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor4 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_velocity = pointer = arrow(pos=vector(pos[0],pos[1],0), axis=vector(vel[0],vel[1],vel[2]), shaftwidth=0.05, color=color.green)
self.render_ref = sphere(canvas = self.viewer, pos=vector(ref_pos[0], ref_pos[1], ref_pos[2]), radius=0.02, color=color.blue, make_trail = True)
grid_xy = make_grid(5, 100)
if self.state is None: return None
self.render_quad1.pos.x = pos[0]
self.render_quad1.pos.y = pos[1]
self.render_quad1.pos.z = pos[2]
self.render_quad2.pos.x = pos[0]
self.render_quad2.pos.y = pos[1]
self.render_quad2.pos.z = pos[2]
rotor_pos = 0.5*x_axis
self.render_rotor1.pos.x = pos[0] + rotor_pos[0]
self.render_rotor1.pos.y = pos[1] + rotor_pos[1]
att = np.array([state[3], state[4], state[5], state[6]]).flatten()
vel = np.array([state[7], state[8], state[9]]).flatten()
load_pos = np.array([state[10], state[11], state[12]]).flatten()
load_vel = np.array([state[13], state[14], state[15]]).flatten()
current_quat = Quaternion(att)
x_axis = current_quat.rotation_matrix.dot(np.array([1.0, 0.0, 0.0]))
y_axis = current_quat.rotation_matrix.dot(np.array([0.0, 1.0, 0.0]))
z_axis = current_quat.rotation_matrix.dot(np.array([0.0, 0.0, 1.0]))
tether_vec = load_pos - pos
if self.viewer is None:
self.viewer = canvas(title='Quadrotor 3D Slungload', width=640, height=480, center=vector(0, 0, 0), forward=vector(1, 1, -1), up=vector(0, 0, 1), background=color.white)
self.render_quad1 = box(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(x_axis[0],x_axis[1],x_axis[2]), length=0.2, height=0.05, width=0.05)
self.render_quad2 = box(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(y_axis[0],y_axis[1],y_axis[2]), length=0.2, height=0.05, width=0.05)
self.render_rotor1 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor2 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor3 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_rotor4 = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(0.01*z_axis[0],0.01*z_axis[1],0.01*z_axis[2]), radius=0.2, color=color.cyan, opacity=0.5)
self.render_velocity = pointer = arrow(pos=vector(pos[0],pos[1],0), axis=vector(vel[0],vel[1],vel[2]), shaftwidth=0.05, color=color.green)
self.render_ref = sphere(canvas = self.viewer, pos=vector(ref_pos[0], ref_pos[1], ref_pos[2]), radius=0.02, color=color.blue, make_trail = True)
self.render_tether = cylinder(canvas = self.viewer, pos=vector(pos[0],pos[1],0), axis=vector(tether_vec[0],tether_vec[1],tether_vec[2]), radius=0.01, color=color.black)
self.render_load = sphere(canvas = self.viewer, pos=vector(load_pos[0], load_pos[1], load_pos[2]), radius=0.1, color=color.red, make_trail = True)
if self.state is None: return None
self.render_quad1.pos.x = pos[0]
self.render_quad1.pos.y = pos[1]
self.render_quad1.pos.z = pos[2]
self.render_quad2.pos.x = pos[0]
self.render_quad2.pos.y = pos[1]
self.render_quad2.pos.z = pos[2]
rotor_pos = 0.5*x_axis