uwv_dynamic_model.orogen

name "uwv_dynamic_model"

using_library "uwv_dynamic_model"
using_library "numeric" # Imports SavitzkyGolayFilter's library
import_types_from "base"
import_types_from "uwv_dynamic_model/DataTypes.hpp"
import_types_from "uwv_dynamic_modelTypes.hpp"

typekit.export_types 'uwv_dynamic_model/DynamicStates'

task_context "Task" do
    needs_configuration

    #*************************
    #***    PROPERTIES     ***
    #*************************

    # Parameters of the motion model
    property("model_parameters", "uwv_dynamic_model/UWVParameters")

    # Simulations per cycle.
    property("sim_per_cycle", "int", 10)

    # Name of the source frame
    property("source_frame", "std/string", "body")

    # Name of the target frame
    property("target_frame", "std/string", "world")

    # Velocity uncertainty values. This values are integrated in order to obtain the pose uncertainty
    property("velocity_uncertainty", "base/Vector6d")


    #*************************
    #***     I/O PORTS     ***
    #*************************

    # Thrusters' commands
    input_port("cmd_in", "/base/LinearAngular6DCommand")

    # The new states after the thrusters' commands are applied
    output_port("pose_samples", "/base/samples/RigidBodyState")

    # Effort and acceleration values
    output_port("secondary_states", "uwv_dynamic_model/SecondaryStates")


    #*************************
    #***      OTHERS       ***
    #*************************

    operation("resetStates")

    operation("setStates").
        argument("pose_state", "/base/samples/RigidBodyState")

    runtime_states :SIMULATING
    exception_states :INPUT_TIMESTAMP_NOT_SET, :COMMAND_WITH_REPEATED_TIMESTAMP

    periodic 0.01
end


# The aim of VelocityEstimator is to be a simple Model Based Velocity Estimator.
#
# It integrates sensors data direct to the model's states,
# without using any filter, like Kalman-Filter.
#
# It is based on the assumption that orientation, angular velocity
# and depth are provided by sensors at a high rate and precision.
#
# For acoustic sensor, i.e. dvl, whose latency time is significant,
# the component replays the model with storaged states and commands,
# correcting model's uncertainty with measured data.
#
# For the particular case of vertical velocity, depth sample is
# derived (using a Savitzky Golay filter), replacing data from dvl
# for this degree of freedom.
#
task_context "VelocityEstimator" do
    subclasses "Task"

    #*************************
    #***    PROPERTIES     ***
    #*************************

    # Savitzky Golay filter properties
    # Used in deriving depth data for getting vertical velocity
    #
    # Order of polynomial
    property("polynomial_order", "int", 3)

    # Half_witdh. (half_width*2 +1) number of samples used.
    property("half_width", "int", 16)

    # Least_square point used. Choose between [-half_witdh, half_witdh]
    property("least_square_point", "int", 16)

    #*************************
    #***     I/O PORTS     ***
    #*************************

    # DVL's linear velocities samples
    input_port("dvl_samples", "/base/samples/RigidBodyState")

    # Orientation samples
    input_port("orientation_samples", "/base/samples/RigidBodyState")

    # IMU's angular velocities samples
    input_port("imu_samples","/base/samples/IMUSensors")

    # Pressure sensor's depth samples. Derivated for vertical velocity estimation.
    input_port("depth_samples", "/base/samples/RigidBodyState")

    #*************************
    #***  TRANSFORMATIONS  ***
    #*************************

    transformer do
        transform "imu", "body"
        transform "pressure_sensor", "body"
        transform "dvl", "body"
        max_latency 0.05
    end

    runtime_states :MISSING_TRANSFORMATION

end