adap_parameters_estimator.orogen

name "adap_parameters_estimator"



 using_library "adap_parameters_estimator"
 using_library "adap_samples_input"
 #using_library "motion_model"
 
 import_types_from "adap_parameters_estimator/adap_parameters.hpp"
 import_types_from "adap_parameters_estimator/adap_dataTypes.h"
 import_types_from "adap_samples_input/samples_dataType.h"
 #import_types_from "motion_model/MotionModel.hpp"

 import_types_from "base"


task_context "Task" do
    
    needs_configuration

   #############################################################
   #
   # Properties
   #
   #############################################################       
   
   # Gain of model, must be a negative values. 	
   property "gA", "base::VectorXd"			
   # Gain of parameters, must have positive values.  							
   property "gLambda", "base::MatrixXd"	
   # Matrix of trhuster. In case the forces and torques are not directly considered  							
   property "thrusterMatrix", "base::MatrixXd"	
   # If true, the input forces are the direct forces and torques applied in AUV. If false, the input are the forces applied by the thruster
   property "body_forces", "bool", true  		
   # Degree of freedom to be analyzed. If UNINITIALISED the method should identify (TODO: check it)				
   property "dofs", "adap_parameters_estimator::DOFS", :SURGE	
   # Sample time. Use for filter purpouse and integrator 		
   property "sTime", "double", 0.01
   # Frequencie of input thruster. Important for filter the parameters 									
   property "ftau", "double", 0 	
   # If true, use data from "dynamic_samples". If false use raw data from speed_samples and thruster_samples
   property "aligned_data", "bool", true         

   #############################################################
   #
   # Methods
   #
   #############################################################       
   	
	#operation("get_deltaV").
    #  returns("double")
    	
	
   #############################################################
   #
   # Input
   #
   #############################################################	
		
    
    input_port "thruster_samples", "base/samples/Joints"
	    doc "Samples of thruster actions. Forces and torques applied in AUV"
	
    input_port "speed_samples", "base/samples/RigidBodyState"
      doc "Measured input containing velocities"
		
    input_port "dynamic_samples", "adap_samples_input::DynamicAUV"
      doc "Measured input containing acceleration, velocity and force, in a aligned way"	

   #############################################################
   #
   # Output
   #
   #############################################################
   
   
    output_port "parameters", "adap_parameters_estimator/Parameters"
    	doc "paremeters of the dynamic motion model"
    	
    output_port "deltaV", "double"
    	doc "error of the real velocity and the estimated velocity of the adap_method"
    
  	output_port "normDeltaV", "double"
    	doc "normalized error of the real velocity and the estimated velocity of the adap_method"

    
    port_driven "dynamic_samples"
end




task_context "AdapModelEstimation" do
    
    needs_configuration

   #############################################################
   #
   # Properties
   #
   #############################################################       
   
   # Gain of model, must be a negative values.  
   property "gA", "base::VectorXd"      
   # Gain of parameters, must have positive values.               
   property "gLambda", "base::MatrixXd"    
   # Degree of freedom to be analyzed. If UNINITIALISED the method should identify (TODO: check it)       
   property "dofs", "adap_parameters_estimator::DOFS", :SURGE 
   # Sample time. Use for filter purpouse and integrator.    
   property "sTime", "double", 0.01
   # Frequencie of sinusoidal input forces. Use for filter purpouse.                   
   property "ftau", "double", 0   

 
   #############################################################
   #
   # Input
   #
   #############################################################  
    
    
    input_port "forces_samples", "base/samples/Joints"
      doc "Samples of thruster actions. Forces and torques applied in AUV, in body-frame"
  
    input_port "pose_samples", "base/samples/RigidBodyState"
      doc "Measured velocities of AUV in body-frame"

   #############################################################
   #
   # Output
   #
   #############################################################
   
   
    output_port "parameters", "adap_parameters_estimator/Parameters"
      doc "paremeters of the dynamic motion model"
      
    output_port "deltaV", "double"
      doc "error of the real velocity and the estimated velocity of the adap_method"
    
    output_port "normDeltaV", "double"
      doc "normalized error of the real velocity and the estimated velocity of the adap_method"

   
    #******************************
    #** Aggregator Parameters ***** 
    #******************************
  stream_aligner do
             
       align_port("pose_samples", 0.01)
       align_port("forces_samples", 0.01)
       max_latency(0.1)
    end   
   
    periodic 0.01 
end


task_context "ForceApplier" do    
    needs_configuration
     
     # The amount of actuators the vehicle has
     property "number_of_thruster", "int"
     # Convert thruster signal into forces, in positive direction. Should have the size of amount_of_actuators 
     property "thruster_coefficients_Pos", "base::VectorXd"
     # Convert thruster signal into forces, in negative direction. Should have the size of amount_of_actuators
     property "thruster_coefficients_Neg", "base::VectorXd"    
     # Matrix of trhuster. Identifie the forces and torques aplied for each of n trhuster
     property "TCM", "base::MatrixXd" 
     # If left empty, uses RAW by default
     property "control_modes", "std::vector<base::JointState::MODE>" 
     # In case the control mode is PWM, used to convert the signal into DC Voltage  
     property "thrusterVoltage", "double", 0  
     # Names of the thrusters
     # Leave empty to use no names
     property "names", "std::vector<std::string>"  
       
    
     ## Thrusters signal    
     input_port('thruster_samples', '/base/samples/Joints').
        doc("thrusters signal")    
     
     ## Forces and torques applied in the vehicle in body-frame    
     output_port('forces', '/base/samples/Joints').
        doc("Forces applied in the auv. Convert the input signal into force and applied the thruster matrix.")  
     ## Forces and signals applied for each thruster in the vehicle    
     output_port('thruster_forces', '/base/samples/Joints').
        doc("Forces and signals applied by each thruster in the auv.")         
   
     exception_states :WRONG_SIZE_OF_CONTROLMODES, :WRONG_SIZE_OF_THRUSTER_COEFFICIENTS, :WRONG_SIZE_OF_NAMES,
                      :WRONG_SIZE_OF_THRUSTER_MATRIX, :UNSET_THRUSTER_INPUT, :UNEXPECTED_THRUSTER_INPUT 
           
     port_driven 'thruster_samples'
end


task_context "ForceApplierModel" do    
    needs_configuration
     
     # The amount of actuators the vehicle has
     property "number_of_thruster", "int"
     # Convert thruster signal into forces, in positive direction. Should have the size of amount_of_actuators 
     property "thruster_coefficients_Pos", "base::VectorXd"
     # Convert thruster signal into forces, in negative direction. Should have the size of amount_of_actuators
     property "thruster_coefficients_Neg", "base::VectorXd"    
     # Matrix of trhuster. Identifie the forces and torques aplied for each of n trhuster
     property "TCM", "base::MatrixXd" 
     # If left empty, uses RAW by default
     property "control_modes", "std::vector<base::JointState::MODE>" 
     # In case the control mode is PWM, used to convert the signal into DC Voltage  
     property "thrusterVoltage", "double", 0  
     # Names of the thrusters
     # Leave empty to use no names
     property "names", "std::vector<std::string>"  
     # Offset error of thruster forces found in motion identification. Added to the resulted forces
     property "offset_forces", "base::VectorXd"  
       
    
     ## Thrusters signal    
     input_port('thruster_samples', '/base/samples/Joints').
        doc("thrusters signal")    
     
     ## Forces and torques applied in the vehicle in body-frame    
     output_port('forces', '/base/samples/Joints').
        doc("Forces applied in the auv. Convert the input signal into force and applied the thruster matrix.")  
     ## Forces and signals applied for each thruster in the vehicle    
     output_port('thruster_forces', '/base/samples/Joints').
        doc("Forces and signals applied by each thruster in the auv.")         
   
     exception_states :WRONG_SIZE_OF_CONTROLMODES, :WRONG_SIZE_OF_THRUSTER_COEFFICIENTS, :WRONG_SIZE_OF_NAMES,
                      :WRONG_SIZE_OF_THRUSTER_MATRIX, :UNSET_THRUSTER_INPUT, :UNEXPECTED_THRUSTER_INPUT 
           
     port_driven 'thruster_samples'
end






##########################################################################################
##########################################################################################

task_context "Evaluation" do
    
  needs_configuration
  #************************************************************************************
  #*****                              PROPERTIES                                  *****
  #************************************************************************************
	
	# define degree of freedom to be evaluate
  property "dof", "adap_parameters_estimator::DOFS", :SURGE

  #************************************************************************************
  #*****                             INPUT PORTS                                  *****
  #************************************************************************************

  #Input ports receive the RBS from the model and from the measured velocity and will compare the accurace of the model's parameters.
  input_port  "model_velocity", "/base/samples/RigidBodyState"
  input_port  "measured_velocity", "/base/samples/RigidBodyState"

  #************************************************************************************
  #*****                            OUTPUT PORTS                                  *****
  #************************************************************************************

  #This output port will provide the error between the two velocities.
  output_port  "error_velocity", "double"
  
  #This output port will provide the Mean Avarage Error (MAE)= mean(|error_velocity|).
  output_port  "mae_velocity", "double"
  
  #This output port will provide the normalized MAE = mean(|error_velocity|) / mean(|velocity_measured|).
  output_port  "norm_mae_velocity", "double"

  #******************************
  #** Aggregator Parameters ***** 
  #******************************
  stream_aligner do
           
     align_port("measured_velocity", 0.09)
     align_port("model_velocity", 0.09)
     max_latency(0.2)
  end   
 
  periodic 0.01
  
end