199 lines
5.2 KiB
C++
199 lines
5.2 KiB
C++
|
/*
|
|||
|
|
* ForceEstimation.cpp
|
|||
|
|
*
|
|||
|
|
* Created on: 4 <EFBFBD><EFBFBD><EFBFBD>. 2022 <EFBFBD>.
|
|||
|
|
* Author: titov
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
#include "ForceEstimation.hh"
|
|||
|
|
|
|||
|
|
processing::ForceEstimation::ForceEstimation(systemic::ISignal & speed,
|
|||
|
|
systemic::ISignal & force,
|
|||
|
|
systemic::ISignal & position ) :
|
|||
|
|
speed(speed), force(force), position(position), est() {}
|
|||
|
|
|
|||
|
|
const float & processing::ForceEstimation::getEstimatedForce() const {
|
|||
|
|
|
|||
|
|
return force_est;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
const float & processing::ForceEstimation::getEstimatedForceRate() const {
|
|||
|
|
|
|||
|
|
return force_rate_est;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
const float & processing::ForceEstimation::getEstimatedKhardness() const {
|
|||
|
|
|
|||
|
|
return est.khardness_est;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
vector::ITechValue & processing::ForceEstimation::getKhardness() {
|
|||
|
|
|
|||
|
|
return est;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void processing::ForceEstimation::configure( Setting & config ) {
|
|||
|
|
|
|||
|
|
delay = config.force_delay;
|
|||
|
|
min_force = config.min_force;
|
|||
|
|
|
|||
|
|
est.L1N = config.L1N;
|
|||
|
|
est.L2N = config.L2N;
|
|||
|
|
|
|||
|
|
est.L1P = config.L1P;
|
|||
|
|
est.L2P = config.L2P;
|
|||
|
|
|
|||
|
|
est.permit_error = config.permit_error;
|
|||
|
|
|
|||
|
|
est.transmission_ratio = config.transmission_ratio;
|
|||
|
|
est._1_transmission_ratio = 1.0f / config.transmission_ratio;
|
|||
|
|
|
|||
|
|
est.set( config.khardness );
|
|||
|
|
|
|||
|
|
const float minimal_force_delta = 2.0f;
|
|||
|
|
est.dead_displacement = minimal_force_delta / ( config.khardness * config.transmission_ratio );
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void processing::ForceEstimation::process() {
|
|||
|
|
|
|||
|
|
using namespace std;
|
|||
|
|
|
|||
|
|
float force_value = force;
|
|||
|
|
|
|||
|
|
if( isfinite(force_value) ) {
|
|||
|
|
|
|||
|
|
if( force_value > min_force ) {
|
|||
|
|
|
|||
|
|
if( force_prev != force_value )
|
|||
|
|
est.estimate( position, force_value );
|
|||
|
|
|
|||
|
|
force_est = est.predict( position + speed * delay );
|
|||
|
|
|
|||
|
|
|
|||
|
|
force_prev = force_value;
|
|||
|
|
|
|||
|
|
} else {
|
|||
|
|
|
|||
|
|
force_prev = 0.0f;
|
|||
|
|
force_est = 0.0f;
|
|||
|
|
|
|||
|
|
est.force_est = min_force;
|
|||
|
|
est.position_est = position;
|
|||
|
|
|
|||
|
|
est.prev_displacement = 0.0f;
|
|||
|
|
est.prev_force_error = 0.0f;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
force_rate_est = speed * est.khardness_est;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
processing::ForceEstimation::Setting::Setting() :
|
|||
|
|
min_force(-INFINITY), khardness(-INFINITY), force_delay(-INFINITY), transmission_ratio(0.0f),
|
|||
|
|
L1P(-INFINITY), L2P(-INFINITY), L1N(-INFINITY), L2N(-INFINITY), permit_error(-INFINITY) {}
|
|||
|
|
|
|||
|
|
bool processing::ForceEstimation::Setting::isValid() {
|
|||
|
|
using namespace std;
|
|||
|
|
|
|||
|
|
using namespace std;
|
|||
|
|
|
|||
|
|
const Setting non_valid;
|
|||
|
|
|
|||
|
|
if( min_force < 0.0f or not isfinite(min_force) )
|
|||
|
|
min_force = non_valid.min_force;
|
|||
|
|
|
|||
|
|
if( khardness < 0.0f or not isfinite(khardness) )
|
|||
|
|
khardness = non_valid.khardness;
|
|||
|
|
|
|||
|
|
if( force_delay < 0.0f or not isfinite(force_delay) )
|
|||
|
|
force_delay = non_valid.force_delay;
|
|||
|
|
|
|||
|
|
if( transmission_ratio <= 0.0f or not isfinite(transmission_ratio) )
|
|||
|
|
transmission_ratio = non_valid.transmission_ratio;
|
|||
|
|
|
|||
|
|
if( L1P < 0.0f or not isfinite(L1P) )
|
|||
|
|
L1P = non_valid.L1P;
|
|||
|
|
|
|||
|
|
if( L2P < 0.0f or not isfinite(L2P) )
|
|||
|
|
L2P = non_valid.L2P;
|
|||
|
|
|
|||
|
|
if( L1N < 0.0f or not isfinite(L1N) )
|
|||
|
|
L1P = non_valid.L1N;
|
|||
|
|
|
|||
|
|
if( L2N < 0.0f or not isfinite(L2N) )
|
|||
|
|
L2N = non_valid.L2N;
|
|||
|
|
|
|||
|
|
if( permit_error <= 0.0f or not isfinite(permit_error) )
|
|||
|
|
permit_error = non_valid.permit_error;
|
|||
|
|
|
|||
|
|
return min_force != non_valid.min_force and
|
|||
|
|
khardness != non_valid.khardness and
|
|||
|
|
force_delay != non_valid.force_delay and
|
|||
|
|
transmission_ratio != non_valid.transmission_ratio and
|
|||
|
|
L1P != non_valid.L1P and
|
|||
|
|
L2P != non_valid.L2P and
|
|||
|
|
L1N != non_valid.L1N and
|
|||
|
|
L2N != non_valid.L2N and
|
|||
|
|
permit_error != non_valid.permit_error;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void processing::ForceEstimation::setSampleTime( float ts_in_second ) {
|
|||
|
|
|
|||
|
|
_Ts = ts_in_second;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void processing::ForceEstimation::Estimator::estimate(float value_position,
|
|||
|
|
float value_force) {
|
|||
|
|
|
|||
|
|
using namespace std;
|
|||
|
|
|
|||
|
|
float displacement = value_position - position_est;
|
|||
|
|
float force_perdicted = force_est + khardness_est * displacement;
|
|||
|
|
float force_error = value_force - force_perdicted;
|
|||
|
|
|
|||
|
|
float L1 = ( displacement > 0.0f ? L1P : L1N );
|
|||
|
|
float L2 = fabsf(displacement) > dead_displacement ? ( displacement > 0.0f ? L2P : L2N ) : 0.0f;
|
|||
|
|
|
|||
|
|
//Update logic
|
|||
|
|
if( fabsf(force_error) < permit_error and
|
|||
|
|
fabsf(displacement) < intolerable_displacement ) {
|
|||
|
|
|
|||
|
|
khardness_est = khardness_est + force_error * L2;
|
|||
|
|
force_est = force_perdicted + force_error * L1;
|
|||
|
|
position_est = value_position;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//Reset logic on big-error
|
|||
|
|
if( ( fabsf(displacement) >= intolerable_displacement
|
|||
|
|
and fabsf(prev_displacement) >= intolerable_displacement ) or
|
|||
|
|
( fabsf(force_error) >= permit_error
|
|||
|
|
and fabsf(prev_force_error) >= permit_error ) ) {
|
|||
|
|
|
|||
|
|
force_est = value_force;
|
|||
|
|
position_est = value_position;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
prev_displacement = displacement;
|
|||
|
|
prev_force_error = force_error ;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
float processing::ForceEstimation::Estimator::predict(float value_position) {
|
|||
|
|
|
|||
|
|
float displacement = value_position - position_est;
|
|||
|
|
return force_est + displacement * khardness_est;
|
|||
|
|
|
|||
|
|
}
|