#!/usr/bin/python
#
# example_1.py - This example demostrates how a simple controller can be
# can be incorporated into the flight simulation.
#
# Will Dickson 03/17/2006
# ------------------------------------------------------------------------
import ode
from fmech.sim_util import read_config
from fmech import sim_fly, sim_log
import ctl_examp
# Read configuration file
config = read_config('config.txt')
# Get simulation parameters
dt = config['time_step']
t_stop = config['stop_time']
grav_const = config['grav_const']
# Create world and simulated fly
t = 0.0
world = ode.World()
world.setGravity((0.0, 0.0, -grav_const))
fly = sim_fly(world, config)
# Set CM poition to origin
fly.setPosition((0, 0, 0))
# Initialize log
log = sim_log(t, config)
from fmech.sim_util import quat2mat
from fmech.sim_util import read_config
from write_image import write_image
# Rendering step
step = 1
# Get name of log file
log_file = sys.argv[1]
# Camera position
cam_pos = (20, 0, 0)
cam_fpt = (0, 0, 0)
# Polyhedra data files
config = read_config()
body_stl_file = config['body_stl_file']
wing_stl_file = config['wing_stl_file']
def get_transform(p, q):
"""
Gets the vtk transformation for the given position vector
and orientation quaternion.
"""
mat4x4 = vtk.vtkMatrix4x4()
mat4x4.Identity()
# Set rotation from quaternion
r = quat2mat(q)
cnt = 0