114 lines
3.1 KiB
C++
114 lines
3.1 KiB
C++
|
/*
|
|||
|
|
* ForceEstimation.hh
|
|||
|
|
*
|
|||
|
|
* Created on: 4 <EFBFBD><EFBFBD><EFBFBD>. 2022 <EFBFBD>.
|
|||
|
|
* Author: titov
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
#ifndef UMLIBRARY_PROCESSING_FORCEESTIMATION_HH_
|
|||
|
|
#define UMLIBRARY_PROCESSING_FORCEESTIMATION_HH_
|
|||
|
|
|
|||
|
|
#include "../systemic/ISignal.hh"
|
|||
|
|
#include "../systemic/IProcess.hh"
|
|||
|
|
#include "acs/VectorAsyncInterface.hh"
|
|||
|
|
|
|||
|
|
namespace processing {
|
|||
|
|
|
|||
|
|
//!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
class ForceEstimation : public systemic::IProcess {
|
|||
|
|
|
|||
|
|
systemic::ISignal & speed;
|
|||
|
|
systemic::ISignal & position;
|
|||
|
|
systemic::ISignal & force;
|
|||
|
|
|
|||
|
|
float force_prev = 0.0f;
|
|||
|
|
|
|||
|
|
struct Estimator : vector::ITechValue {
|
|||
|
|
|
|||
|
|
float force_est = 0.0f;
|
|||
|
|
float khardness_est = 0.0f;
|
|||
|
|
float position_est = 0.0f;
|
|||
|
|
float touch_est = 0.0f;
|
|||
|
|
|
|||
|
|
float L1P = 0.0f;
|
|||
|
|
float L2P = 0.0f;
|
|||
|
|
|
|||
|
|
float L1N = 0.0f;
|
|||
|
|
float L2N = 0.0f;
|
|||
|
|
|
|||
|
|
float permit_error = 0.0f;
|
|||
|
|
float intolerable_displacement = 0.0f;
|
|||
|
|
|
|||
|
|
float dead_displacement = 0.0f;
|
|||
|
|
|
|||
|
|
float prev_displacement = 0.0f;
|
|||
|
|
float prev_force_error = 0.0f;
|
|||
|
|
|
|||
|
|
void estimate( float value_position, float value_force );
|
|||
|
|
float predict( float value_position );
|
|||
|
|
void reset();
|
|||
|
|
|
|||
|
|
float transmission_ratio;
|
|||
|
|
float _1_transmission_ratio;
|
|||
|
|
|
|||
|
|
void set( float khardness ) {
|
|||
|
|
khardness_est = khardness * transmission_ratio;
|
|||
|
|
intolerable_displacement = permit_error / khardness_est;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
float get() const {
|
|||
|
|
return khardness_est * _1_transmission_ratio;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
} est;
|
|||
|
|
|
|||
|
|
float force_est;
|
|||
|
|
float force_rate_est;
|
|||
|
|
|
|||
|
|
float _Ts = 0.0f;
|
|||
|
|
float delay;
|
|||
|
|
float min_force;
|
|||
|
|
|
|||
|
|
public:
|
|||
|
|
struct Setting {
|
|||
|
|
|
|||
|
|
float min_force; //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (<28>).
|
|||
|
|
float khardness; //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (<28>/<2F>).
|
|||
|
|
float force_delay; //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (<28>).
|
|||
|
|
|
|||
|
|
float transmission_ratio; //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (<28>/<2F><><EFBFBD>).
|
|||
|
|
|
|||
|
|
float L1P; //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float L2P; //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
|
|||
|
|
float L1N; //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float L2N; //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
|
|||
|
|
float permit_error; //!<<3C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
|
|||
|
|
Setting();
|
|||
|
|
bool isValid();
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
void configure( Setting & config );
|
|||
|
|
|
|||
|
|
public:
|
|||
|
|
ForceEstimation( systemic::ISignal & speed, systemic::ISignal & force, systemic::ISignal & position );
|
|||
|
|
|
|||
|
|
const float & getEstimatedForce() const;
|
|||
|
|
const float & getEstimatedForceRate() const;
|
|||
|
|
const float & getEstimatedKhardness() const;
|
|||
|
|
|
|||
|
|
vector::ITechValue & getKhardness();
|
|||
|
|
|
|||
|
|
void process();
|
|||
|
|
void setSampleTime( float ts_in_second );
|
|||
|
|
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
#endif /* UMLIBRARY_PROCESSING_FORCEESTIMATION_HH_ */
|