Commit 47d11bb9 authored by Olivier Stasse's avatar Olivier Stasse

Remove imu_offset_compensation and parameter-server

parent 70fff29c
/*
* Copyright 2017, Andrea Del Prete, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_torque_control_imu_offset_compensation_H__
#define __sot_torque_control_imu_offset_compensation_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (imu_offset_compensation_EXPORTS)
# define SOTIMUOFFSETCOMPENSATION_EXPORT __declspec(dllexport)
# else
# define SOTIMUOFFSETCOMPENSATION_EXPORT __declspec(dllimport)
# endif
#else
# define SOTIMUOFFSETCOMPENSATION_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTIMUOFFSETCOMPENSATION_EXPORT ImuOffsetCompensation
:public::dynamicgraph::Entity
{
typedef ImuOffsetCompensation EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
typedef Eigen::Vector3d Vector3;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
ImuOffsetCompensation( const std::string & name );
/* --- COMMANDS --- */
void init(const double& dt);
void update_offset(const double & duration);
void setGyroDCBlockerParameter(const double & alpha);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(accelerometer_in, dynamicgraph::Vector); /// raw accelerometer data
DECLARE_SIGNAL_IN(gyrometer_in, dynamicgraph::Vector); /// raw gyrometer data
DECLARE_SIGNAL_OUT(accelerometer_out, dynamicgraph::Vector); /// compensated accelerometer data
DECLARE_SIGNAL_OUT(gyrometer_out, dynamicgraph::Vector); /// compensated gyrometer data
protected:
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
/* --- METHODS --- */
void update_offset_impl(int iter);
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
double m_dt; /// sampling time in seconds
int m_update_cycles_left; /// number of update cycles left
int m_update_cycles; /// total number of update cycles to perform
double m_a_gyro_DC_blocker; /// filter parameter to remove DC from gyro online (should be close to <1.0 and equal to 1.0 for disabling)
Vector3 m_gyro_offset; /// gyrometer offset
Vector3 m_acc_offset; /// accelerometer offset
Vector3 m_gyro_sum; /// tmp variable to store the sum of the gyro measurements during update phase
Vector3 m_acc_sum; /// tmp variable to store the sum of the acc measurements during update phase
}; // class ImuOffsetCompensation
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_imu_offset_compensation_H__
/*
* Copyright 2015, Andrea Del Prete, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_torque_control_parameter_server_H__
#define __sot_torque_control_parameter_server_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (__sot_torque_parameter_server_H__)
# define SOTParameterServer_EXPORT __declspec(dllexport)
# else
# define SOTParameterServer_EXPORT __declspec(dllimport)
# endif
#else
# define SOTParameterServer_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
#include <map>
#include "boost/assign.hpp"
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <sot/talos_balance/robot/robot-wrapper.hh>
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/// Number of time step to transition from one ctrl mode to another
#define CTRL_MODE_TRANSITION_TIME_STEP 1000.0
class SOTParameterServer_EXPORT ParameterServer
:public::dynamicgraph::Entity
{
typedef ParameterServer EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
ParameterServer( const std::string & name);
/// Initialize
/// @param dt: control interval
/// @param urdfFile: path to the URDF model of the robot
void init(const double & dt,
const std::string & urdfFile,
const std::string & robotRef);
/* --- SIGNALS --- */
/* --- COMMANDS --- */
/// Commands related to joint name and joint id
void setNameToId(const std::string& jointName, const unsigned int & jointId);
void setJointLimitsFromId(const unsigned int &jointId, const double &lq, const double &uq);
/// Command related to ForceUtil
void setForceLimitsFromId(const unsigned int & jointId, const dynamicgraph::Vector &lq, const dynamicgraph::Vector &uq);
void setForceNameToForceId(const std::string& forceName, const unsigned int & forceId);
/// Commands related to FootUtil
void setRightFootSoleXYZ(const dynamicgraph::Vector &);
void setRightFootForceSensorXYZ(const dynamicgraph::Vector &);
void setFootFrameName(const std::string &, const std::string &);
void setImuJointName(const std::string &);
void displayRobotUtil();
/// Set the mapping between urdf and sot.
void setJoints(const dynamicgraph::Vector &);
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
protected:
RobotUtilShrPtr m_robot_util;
dynamicgraph::sot::talos_balance::robots::RobotWrapper * m_robot;
bool m_initSucceeded; /// true if the entity has been successfully initialized
double m_dt; /// control loop time period
bool m_emergency_stop_triggered; /// true if an emergency condition as been triggered either by an other entity, or by control limit violation
bool m_is_first_iter; /// true at the first iteration, false otherwise
int m_iter;
double m_sleep_time; /// time to sleep at every iteration (to slow down simulation)
bool convertJointNameToJointId(const std::string& name, unsigned int& id);
bool isJointInRange(unsigned int id, double q);
void updateJointCtrlModesOutputSignal();
}; // class ParameterServer
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_control_manager_H__
#include <sot/talos_balance/imu_offset_compensation.hh>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
#include <sot/core/stop-watch.hh>
namespace dynamicgraph
{
namespace sot
{
namespace talos_balance
{
namespace dg = ::dynamicgraph;
using namespace dg;
using namespace dg::command;
using namespace std;
#define CALIBRATION_FILE_NAME "/opt/imu_calib.txt"
#define PROFILE_IMU_OFFSET_COMPENSATION_COMPUTATION "ImuOffsetCompensation computation"
#define INPUT_SIGNALS m_accelerometer_inSIN << m_gyrometer_inSIN
#define OUTPUT_SIGNALS m_accelerometer_outSOUT << m_gyrometer_outSOUT
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef ImuOffsetCompensation EntityClassName;
/* --- DG FACTORY ---------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ImuOffsetCompensation,
"ImuOffsetCompensation");
/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
ImuOffsetCompensation::ImuOffsetCompensation(const std::string& name)
: Entity(name)
,CONSTRUCT_SIGNAL_IN( accelerometer_in, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN( gyrometer_in, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_OUT(accelerometer_out, dynamicgraph::Vector, m_accelerometer_inSIN)
,CONSTRUCT_SIGNAL_OUT(gyrometer_out, dynamicgraph::Vector, m_gyrometer_inSIN)
,m_initSucceeded(false)
,m_dt(0.001)
,m_update_cycles_left(0)
,m_update_cycles(0)
,m_a_gyro_DC_blocker(1.0)
{
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
m_gyro_offset.setZero();
m_acc_offset.setZero();
m_gyro_sum.setZero();
m_acc_sum.setZero();
/* Commands. */
addCommand("init",
makeCommandVoid1(*this, &ImuOffsetCompensation::init,
docCommandVoid1("Initialize the entity.",
"Timestep in seconds (double)")));
addCommand("update_offset",
makeCommandVoid1(*this, &ImuOffsetCompensation::update_offset,
docCommandVoid1("Update the IMU offsets.",
"Duration of the update phase in seconds (double)")));
addCommand("setGyroDCBlockerParameter",
makeCommandVoid1(*this, &ImuOffsetCompensation::setGyroDCBlockerParameter,
docCommandVoid1("Set DC Blocker filter parameter.",
"alpha (double)")));
}
/* ------------------------------------------------------------------- */
/* --- COMMANDS ------------------------------------------------------ */
/* ------------------------------------------------------------------- */
void ImuOffsetCompensation::init(const double& dt)
{
if(dt<=0.0)
return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
m_dt = dt;
m_initSucceeded = true;
// try to read IMU calibration data from file
std::ifstream infile;
infile.open(CALIBRATION_FILE_NAME, std::ios::in);
if(!infile.is_open())
return SEND_MSG("Error trying to read calibration results from file "+toString(CALIBRATION_FILE_NAME), MSG_TYPE_ERROR);
double z=0;
int i=0;
while(infile>>z)
{
m_gyro_offset(i) = z;
i++;
if(i==3)
break;
}
if(i!=3)
{
m_gyro_offset.setZero();
return SEND_MSG("Error trying to read gyro offset from file "+toString(CALIBRATION_FILE_NAME)+". Not enough values: "+toString(i), MSG_TYPE_ERROR);
}
i=0;
while(infile>>z)
{
m_acc_offset(i) = z;
i++;
if(i==3)
break;
}
if(i!=3)
{
m_gyro_offset.setZero();
m_acc_offset.setZero();
return SEND_MSG("Error trying to read acc offset from file "+toString(CALIBRATION_FILE_NAME)+". Not enough values: "+toString(i), MSG_TYPE_ERROR);
}
SEND_MSG("Offset read finished:\n* acc offset: "+toString(m_acc_offset.transpose())+
"\n* gyro offset: "+toString(m_gyro_offset.transpose()), MSG_TYPE_INFO);
}
void ImuOffsetCompensation::setGyroDCBlockerParameter(const double & alpha)
{
if(alpha>1.0 || alpha<=0.0)
return SEND_MSG("GyroDCBlockerParameter must be > 0 and <= 1", MSG_TYPE_ERROR);
m_a_gyro_DC_blocker = alpha;
}
void ImuOffsetCompensation::update_offset(const double& duration)
{
if(duration<m_dt)
return SEND_MSG("Duration must be greater than the time step", MSG_TYPE_ERROR);
m_update_cycles = int(duration/m_dt);
m_update_cycles_left = m_update_cycles;
}
void ImuOffsetCompensation::update_offset_impl(int iter)
{
const dynamicgraph::Vector& accelerometer = m_accelerometer_inSIN(iter);
const dynamicgraph::Vector& gyrometer = m_gyrometer_inSIN(iter);
m_acc_sum += accelerometer;
m_gyro_sum += gyrometer;
m_update_cycles_left--;
if(m_update_cycles_left==0)
{
Vector3 g, new_acc_offset, new_gyro_offset;
g<<0.0, 0.0, 9.81;
new_acc_offset = (m_acc_sum /m_update_cycles) - g;
new_gyro_offset = m_gyro_sum/m_update_cycles;
m_acc_sum.setZero();
m_gyro_sum.setZero();
SEND_MSG("Offset computation finished:"+
("\n* old acc offset: "+toString(m_acc_offset.transpose()))+
"\n* new acc offset: "+toString(new_acc_offset.transpose())+
"\n* old gyro offset: "+toString(m_gyro_offset.transpose())+
"\n* new gyro offset: "+toString(new_gyro_offset.transpose()), MSG_TYPE_INFO);
m_acc_offset = new_acc_offset;
m_gyro_offset = new_gyro_offset;
// save to text file
ofstream aof(CALIBRATION_FILE_NAME);
if(!aof.is_open())
return SEND_MSG("Error trying to save calibration results on file "+toString(CALIBRATION_FILE_NAME), MSG_TYPE_ERROR);
for(unsigned long int i=0;i<3;i++)
aof << m_gyro_offset[i] << " " ;
aof << std::endl;
for(unsigned long int i=0;i<3;i++)
aof << m_acc_offset[i] << " " ;
aof << std::endl;
aof.close();
SEND_MSG("IMU calibration data saved to file "+toString(CALIBRATION_FILE_NAME), MSG_TYPE_INFO);
}
}
/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
/* ------------------------------------------------------------------- */
DEFINE_SIGNAL_OUT_FUNCTION(accelerometer_out, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal accelerometer before initialization!");
return s;
}
if(m_update_cycles_left>0)
update_offset_impl(iter);
const dynamicgraph::Vector& accelerometer = m_accelerometer_inSIN(iter);
if(s.size()!=3)
s.resize(3);
s = accelerometer - m_acc_offset;
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(gyrometer_out, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal gyrometer before initialization!");
return s;
}
const dynamicgraph::Vector& gyrometer = m_gyrometer_inSIN(iter);
if(s.size()!=3)
s.resize(3);
//estimate bias online with the assumption that average angular velocity should be zero.
if (m_a_gyro_DC_blocker !=1.0)
m_gyro_offset = m_gyro_offset*m_a_gyro_DC_blocker + (1.-m_a_gyro_DC_blocker)*gyrometer;
s = gyrometer - m_gyro_offset;
return s;
}
/* ------------------------------------------------------------------- */
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */
void ImuOffsetCompensation::display(std::ostream& os) const
{
os << "ImuOffsetCompensation "<<getName();
try
{
getProfiler().report_all(3, os);
}
catch (ExceptionSignal e) {}
}
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
This diff is collapsed.
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment