def double_integrator():
gr = graph.OpGraph('double_integrator')
gr.scalar('u')
gr.optimize('u')
gr.time_antiderivative('v', 'u')
gr.time_antiderivative('x', 'v')
return gr
def make_jet():
gr = graph.OpGraph()
# gr.vector('eps_dddot', 6)
# gr.vector('daccel_bias', 3)
# gr.vector('dgyro_bias', 3)
# gr.time_antiderivative('accel_bias', 'daccel_bias')
# gr.time_antiderivative('gyro_bias', 'dgyro_bias')
# gr.time_antiderivative('eps_ddot', 'eps_dddot')
# gr.vector('g_world', 3)
gr.se3('T_imu_from_vehicle')
gr.state(gr.vector('accel_bias', 3))
gr.state(gr.vector('gyro_bias', 3))
gr.state(gr.vector('eps_ddot', 6))
gr.state(gr.time_antiderivative('eps_dot', 'eps_ddot'))
w = gr.block('w', 'eps_dot', 3, 0, 3, 1)
v = gr.block('v', 'eps_dot', 0, 0, 3, 1)
# gr.state(gr.se3('T_body_from_world'))
# gr.time_antiderivative('T_body_from_world', 'eps_dot')
gr.state(gr.time_antiderivative('x_world', v))
gr.so3('R_world_from_body')
gr.state(gr.time_antiderivative('R_world_from_body', w))
return gr
def rotary_double_integrator():
gr = graph.OpGraph('double_integrator')
gr.vector('u', 3)
gr.optimize('u')
gr.time_antiderivative('w', 'u')
gr.so3('R')
gr.time_antiderivative('R', 'w')
return gr
def accel_observation_model(grx):
gr = graph.OpGraph()
gr.copy_types(grx)
generated_type = 'AccelMeasurement'
generated_func = 'observe_accel'
gr.register_group_type(generated_type, ['observed_acceleration'],
[op_defs.create_vector(3)])
state = gr.emplace('state', gr.group_types['State'])
parameters = gr.emplace('parameters', gr.group_types['Parameters'])
R_world_from_body = groups.extract_by_name(gr, 'R_world_from_body', state,
'R_world_from_body')
eps_ddot = groups.extract_by_name(gr, 'eps_ddot', state, 'eps_ddot')
eps_dot = groups.extract_by_name(gr, 'eps_dot', state, 'eps_dot')
accel_bias = groups.extract_by_name(gr, 'accel_bias', state, 'accel_bias')
imu_from_vehicle = groups.extract_by_name(gr, 'imu_from_vehicle',
parameters, 'T_imu_from_vehicle')
R_imu_from_vehicle = gr.rotation('R_imu_from_vehicle', imu_from_vehicle)
w = gr.block('w', eps_dot, 3, 0, 3, 1)
a_world = gr.block('a_world', eps_ddot, 0, 0, 3, 1)
R_imu_from_world = gr.mul('R_imu_from_world', R_imu_from_vehicle,
gr.inv('R_body_from_world', R_world_from_body))
a_imu = gr.mul('a_imu', R_imu_from_world, a_world)
g_world = gr.mul('g_world', gr.constant_vector('unit_z', 3, 'unitz'),
gr.constant_scalar('g_mpss', 9.81))
g_imu = gr.mul('g_imu', R_imu_from_world, g_world)
observed_acceleration = gr.reduce_binary_op('add', 'observed_acceleration',
[a_imu, accel_bias, g_imu])
accel_meas = gr.groupify('accel_meas', [observed_acceleration],
inherent_type=generated_type)
grx.add_graph_as_function(generated_func,
graph=gr,
output_sym=accel_meas,
input_order=[state, parameters])
# grx.add_function(
# 'observe_accel',
# returns=grx.group_types[generated_type],
# arguments=(
# grx.group_types['State'],
# grx.group_types['Parameters']
# )
# )
groups.create_group_diff(grx, generated_type)
add_error_model(grx, generated_type, generated_func)
def make_wrench():
gr = graph.OpGraph()
force = gr.vector('force_N', 3)
torque = gr.vector('torque_Nm', 3)
elements = [force, torque]
gr.register_group_type('Wrench', elements, gr.get_properties(elements))
wrench = gr.groupify('wrench', elements, inherent_type='Wrench')
groups.create_group_diff(gr, 'Wrench')
return gr
def simple_graph():
gr = graph.OpGraph('Simple')
a = gr.scalar('a')
gr.optimize(a)
b = gr.scalar('b')
gr.mul('ab', a, b)
d = gr.time_antiderivative('d', 'ab')
gr.time_antiderivative('e', d)
return gr
def make_vanes():
gr = graph.OpGraph()
vane_0 = gr.scalar('servo_0_angle_rad')
vane_1 = gr.scalar('servo_1_angle_rad')
vane_2 = gr.scalar('servo_2_angle_rad')
vane_3 = gr.scalar('servo_3_angle_rad')
vanes = [vane_0, vane_1, vane_2, vane_3]
gr.register_group_type('QuadraframeStatus', vanes, gr.get_properties(vanes))
groups.create_group_diff(gr, 'QuadraframeStatus')
return gr
def vectorspring():
gr = graph.OpGraph('VectorSpring')
k = gr.scalar('k')
imass = gr.inv('imass', gr.scalar('mass'))
a = gr.vector('a', 3)
v = gr.time_antiderivative('v', a)
x = gr.time_antiderivative('x', v)
f = gr.mul('f', k, x)
gr.mul('a', imass, f)
return gr
def controlled_vectorspring():
gr = graph.OpGraph('VectorSpring')
k = gr.scalar('k')
imass = gr.inv(gr.anon(), gr.scalar('mass'))
u = gr.vector('u', 3)
a = gr.vector('a', 3)
v = gr.time_antiderivative('v', a)
x = gr.time_antiderivative('x', v)
force = gr.add('force', gr.mul(gr.anon(), k, x), u)
gr.mul('a', imass, force)
return gr
def add_error_model(grx, group_type, model_name):
gr = graph.OpGraph('ErrorModel')
gr.copy_types(grx)
gr._copy_functions(grx)
state = gr.emplace('state', gr.group_types['State'])
meas = gr.emplace('meas', gr.group_types[group_type])
parameters = gr.emplace('parameters', gr.group_types['Parameters'])
expected = gr.func(model_name, 'expected', state, parameters)
error = gr.func('compute_delta', 'error', meas, expected)
grx.add_graph_as_function(model_name + "_error_model",
graph=gr,
output_sym=error,
input_order=[state, meas, parameters])
def gyro_observation_model(grx):
gr = graph.OpGraph()
gr.copy_types(grx)
state = gr.emplace('state', gr.group_types['State'])
parameters = gr.emplace('parameters', gr.group_types['Parameters'])
imu_from_vehicle = groups.extract_by_name(gr, 'imu_from_vehicle',
parameters, 'T_imu_from_vehicle')
R_world_from_body = groups.extract_by_name(gr, 'R_world_from_body', state,
'R_world_from_body')
eps_dot = groups.extract_by_name(gr, 'eps_dot', state, 'eps_dot')
gyro_bias = groups.extract_by_name(gr, 'gyro_bias', state, 'gyro_bias')
w_world = gr.block('w_world', eps_dot, 3, 0, 3, 1)
R_sensor_from_vehicle = gr.rotation('R_sensor_from_vehicle',
imu_from_vehicle)
R_sensor_from_world = gr.mul("R_sensor_from_world", R_sensor_from_vehicle,
gr.inv(gr.anon(), R_world_from_body))
w_imu = gr.mul(gr.anon(), R_sensor_from_world, w_world)
observed_w = gr.add('observed_w', w_imu, gyro_bias)
# observed_w = gr.sub('observed_w', gyro_bias, w_imu)
generated_type = 'GyroMeasurement'
generated_func = 'observe_gyro'
gr.register_group_type(generated_type, [observed_w],
[gr.get_properties(observed_w)])
gyro_meas = gr.groupify('gyro_meas', [observed_w],
inherent_type=generated_type)
grx.add_graph_as_function(generated_func,
graph=gr,
output_sym=gyro_meas,
input_order=[state, parameters])
groups.create_group_diff(grx, generated_type)
add_error_model(grx, generated_type, generated_func)
return gr
def make_jet():
gr = graph.OpGraph()
gr.scalar('mass')
gr.vector('external_force', 3)
gr.so3('R_world_from_body')
gr.optimize(gr.vector('q', 3))
gr.time_antiderivative('w', 'q')
# TODO: Add subtraction so we can do damping
# gr.mul('damped_w', 'w_damping', 'w')
# gr.add('')
gr.time_antiderivative('R_world_from_body', 'w')
# Or make a dummy we can use
gr.add_graph_as_function('force_from_throttle', make_force_fcn(), 'out')
gr.optimize(gr.scalar('throttle_dot'))
gr.time_antiderivative('throttle_pct', 'throttle_dot')
gr.func('force_from_throttle', 'thrust', 'throttle_pct')
gr.vector('unit_z', 3)
gr.mul('force_world', 'R_world_from_body',
gr.mul('body_force', 'thrust', 'unit_z'))
# gr.identity('force_world', 'unit_z')
gr.add('net_force_world', 'force_world', 'external_force')
gr.mul('a', gr.inv('inv_mass', 'mass'), 'net_force_world')
gr.vector('v', 3)
gr.time_antiderivative('v', 'a')
gr.time_antiderivative('x', 'v')
gr.warnings()
return gr
def fiducial_observation_model(grx):
gr = graph.OpGraph()
gr.copy_types(grx)
state = gr.emplace('state', gr.group_types['State'])
parameters = gr.emplace('parameters', gr.group_types['Parameters'])
# imu_from_vehicle = groups.extract_by_name(gr, 'imu_from_vehicle', parameters, 'T_imu_from_vehicle')
# world_from_vehicle = camera_from_fiducial
# camera_from_fiducial =
gr.register_group_type('FiducialMeasurement', [camera_from_world],
[gr.get_properties(camera_from_world)])
grx.add_graph_as_function('observe_gyro',
graph=gr,
output_sym=observed_w,
input_order=[state, parameters])
add_error_model(grx, 'FiducialMeasurement', 'observe_fiducial')
return gr
def make_force_fcn():
gr = graph.OpGraph('ForceFromThrottle')
gr.scalar('throttle')
gr.identity('out', 'throttle')
return gr