Skip to content
GitLab
Commit 13a508d3 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Remove files already present in sot-core.

parent 79073023
Hide whitespace changes
Inline Side-by-side
include/sot/torque_control/madgwickahrs.hh deleted 100644 → 0
View file @ 79073023
//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date Author Notes
// 29/09/2011 SOH Madgwick Initial release
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
// 11/05/2017 T Flayols Make it a dynamic graph entity
//
//=====================================================================================================
/*
* Copyright 2017, Thomas Flayols, 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_madgwickahrs_H__
#define __sot_torque_control_madgwickahrs_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (madgwickahrs_EXPORTS)
# define SOTMADGWICKAHRS_EXPORT __declspec(dllexport)
# else
# define SOTMADGWICKAHRS_EXPORT __declspec(dllimport)
# endif
#else
# define SOTMADGWICKAHRS_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <map>
#include "boost/assign.hpp"
/* HELPER */
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
#define betaDef 0.01f // 2 * proportional g
namespace dynamicgraph {
namespace sot {
namespace torque_control {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTMADGWICKAHRS_EXPORT MadgwickAHRS
:public::dynamicgraph::Entity
{
typedef MadgwickAHRS EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
MadgwickAHRS( const std::string & name );
void init(const double& dt);
void set_beta(const double & beta);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector); /// ax ay az in m.s-2
DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector); /// gx gy gz in rad.s-1
DECLARE_SIGNAL_OUT(imu_quat, dynamicgraph::Vector); /// Estimated orientation of IMU as a quaternion
protected:
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
/* --- METHODS --- */
float invSqrt(float x);
void madgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) ;
//void madgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
Entity::sendMsg("[MadgwickAHRS-"+name+"] "+msg, t, file, line);
}
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
volatile float m_beta; /// 2 * proportional gain (Kp)
volatile float m_q0, m_q1, m_q2 , m_q3; /// quaternion of sensor frame
float m_sampleFreq; /// sample frequency in Hz
}; // class MadgwickAHRS
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_madgwickahrs_H__
include/sot/torque_control/utils/causal-filter.hh deleted 100644 → 0
View file @ 79073023
/*
* Copyright 2017-, Rohan Budhirja, 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/>.
*/
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <Eigen/Core>
class CausalFilter
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** --- CONSTRUCTOR ---- */
CausalFilter(const double &timestep,
const int& xSize,
const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator);
void get_x_dx_ddx(const Eigen::VectorXd& base_x,
Eigen::VectorXd& x_output_dx_ddx);
void switch_filter(const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator);
private:
double m_dt; /// sampling timestep of the input signal
int m_x_size;
Eigen::VectorXd::Index m_filter_order_m;
Eigen::VectorXd::Index m_filter_order_n;
Eigen::VectorXd m_filter_numerator;
Eigen::VectorXd m_filter_denominator;
bool first_sample;
int pt_numerator;
int pt_denominator;
Eigen::MatrixXd input_buffer;
Eigen::MatrixXd output_buffer;
}; // class CausalFilter
include/sot/torque_control/utils/stop-watch.hh deleted 100644 → 0
View file @ 79073023
/*
Copyright (c) 2010-2013 Tommaso Urli
Tommaso Urli tommaso.urli@uniud.it University of Udine
Permission is hereby granted, free of charge, to any person obtaining
a copy of this software and associated documentation files (the
"Software"), to deal in the Software without restriction, including
without limitation the rights to use, copy, modify, merge, publish,
distribute, sublicense, and/or sell copies of the Software, and to
permit persons to whom the Software is furnished to do so, subject to
the following conditions:
The above copyright notice and this permission notice shall be
included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef __sot_torque_control_stopwatch_H__
#define __sot_torque_control_stopwatch_H__
#include "sot/torque_control/utils/Stdafx.hh"
#ifndef WIN32
/* The classes below are exported */
#pragma GCC visibility push(default)
#endif
// Generic stopwatch exception class
struct StopwatchException
{
public:
StopwatchException(std::string error) : error(error) { }
std::string error;
};
enum StopwatchMode
{
NONE = 0, // Clock is not initialized
CPU_TIME = 1, // Clock calculates time ranges using ctime and CLOCKS_PER_SEC
REAL_TIME = 2 // Clock calculates time by asking the operating system how
// much real time passed
};
#define STOP_WATCH_MAX_NAME_LENGTH 80
/**
@brief A class representing a stopwatch.
@code
Stopwatch swatch();
@endcode
The Stopwatch class can be used to measure execution time of code,
algorithms, etc., // TODO: he Stopwatch can be initialized in two
time-taking modes, CPU time and real time:
@code
swatch.set_mode(REAL_TIME);
@endcode
CPU time is the time spent by the processor on a certain piece of code,
while real time is the real amount of time taken by a certain piece of
code to execute (i.e. in general if you are doing hard work such as
image or video editing on a different process the measured time will
probably increase).
How does it work? Basically, one wraps the code to be measured with the
following method calls:
@code
swatch.start("My astounding algorithm");
// Hic est code
swatch.stop("My astounding algorithm");
@endcode
A string representing the code ID is provided so that nested portions of
code can be profiled separately:
@code
swatch.start("My astounding algorithm");
swatch.start("My astounding algorithm - Super smart init");
// Initialization
swatch.stop("My astounding algorithm - Super smart init");
swatch.start("My astounding algorithm - Main loop");
// Loop
swatch.stop("My astounding algorithm - Main loop");
swatch.stop("My astounding algorithm");
@endcode
Note: ID strings can be whatever you like, in the previous example I have
used "My astounding algorithm - *" only to enforce the fact that the
measured code portions are part of My astounding algorithm, but there's no
connection between the three measurements.
If the code for a certain task is scattered through different files or
portions of the same file one can use the start-pause-stop method:
@code
swatch.start("Setup");
// First part of setup
swatch.pause("Setup");
swatch.start("Main logic");
// Main logic
swatch.stop("Main logic");
swatch.start("Setup");
// Cleanup (part of the setup)
swatch.stop("Setup");
@endcode
Finally, to report the results of the measurements just run:
@code
swatch.report("Code ID");
@endcode
Thou can also provide an additional std::ostream& parameter to report() to
redirect the logging on a different output. Also, you can use the
get_total/min/max/average_time() methods to get the individual numeric data,
without all the details of the logging. You can also extend Stopwatch to
implement your own logging syntax.
To report all the measurements:
@code
swatch.report_all();
@endcode
Same as above, you can redirect the output by providing a std::ostream&
parameter.
*/
class Stopwatch {
public:
/** Constructor */
Stopwatch(StopwatchMode _mode=NONE);
/** Destructor */
~Stopwatch();
/** Tells if a performance with a certain ID exists */
bool performance_exists(std::string perf_name);
/** Initialize stopwatch to use a certain time taking mode */
void set_mode(StopwatchMode mode);
/** Start the stopwatch related to a certain piece of code */
void start(std::string perf_name);
/** Stops the stopwatch related to a certain piece of code */
void stop(std::string perf_name);
/** Stops the stopwatch related to a certain piece of code */
void pause(std::string perf_name);
/** Reset a certain performance record */
void reset(std::string perf_name);
/** Resets all the performance records */
void reset_all();
/** Dump the data of a certain performance record */
void report(std::string perf_name, int precision=2,
std::ostream& output = std::cout);
/** Dump the data of all the performance records */
void report_all(int precision=2, std::ostream& output = std::cout);
/** Returns total execution time of a certain performance */
long double get_total_time(std::string perf_name);
/** Returns average execution time of a certain performance */
long double get_average_time(std::string perf_name);
/** Returns minimum execution time of a certain performance */
long double get_min_time(std::string perf_name);
/** Returns maximum execution time of a certain performance */
long double get_max_time(std::string perf_name);
/** Return last measurement of a certain performance */
long double get_last_time(std::string perf_name);
/** Return the time since the start of the last measurement of a given
performance. */
long double get_time_so_far(std::string perf_name);
/** Turn off clock, all the Stopwatch::* methods return without doing
anything after this method is called. */
void turn_off();
/** Turn on clock, restore clock operativity after a turn_off(). */
void turn_on();
/** Take time, depends on mode */
long double take_time();
protected:
/** Struct to hold the performance data */
struct PerformanceData {
PerformanceData() :
clock_start(0),
total_time(0),
min_time(0),
max_time(0),
last_time(0),
paused(false),
stops(0) {
}
/** Start time */
long double clock_start;
/** Cumulative total time */
long double total_time;
/** Minimum time */
long double min_time;
/** Maximum time */
long double max_time;
/** Last time */
long double last_time;
/** Tells if this performance has been paused, only for internal use */
bool paused;
/** How many cycles have been this stopwatch executed? */
int stops;
};
/** Flag to hold the clock's status */
bool active;
/** Time taking mode */
StopwatchMode mode;
/** Pointer to the dynamic structure which holds the collection of performance
data */
std::map<std::string, PerformanceData >* records_of;
};
Stopwatch& getProfiler();
#ifndef WIN32
#pragma GCC visibility pop
#endif
#endif
src/causal-filter.cpp deleted 100644 → 0
View file @ 79073023
/*
* Copyright 2017-, Rohan Budhiraja 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/>.
*/
#include <iostream>
#include <sot/torque_control/utils/causal-filter.hh>
/*
Filter data with an IIR or FIR filter.
Filter a data sequence, x, using a digital filter. The filter is a direct form II transposed implementation of the standard difference equation.
This means that the filter implements:
a[0]*y[N] = b[0]*x[N] + b[1]*x[N-1] + ... + b[m-1]*x[N-(m-1)]
- a[1]*y[N-1] - ... - a[n-1]*y[N-(n-1)]
where m is the degree of the numerator, n is the degree of the denominator, and N is the sample number
*/
CausalFilter::CausalFilter(const double &timestep,
const int& xSize,
const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator)
: m_dt(timestep)
, m_x_size(xSize)
, m_filter_order_m(filter_numerator.size())
, m_filter_order_n(filter_denominator.size())
, m_filter_numerator(filter_numerator)
, m_filter_denominator(filter_denominator)
, first_sample(true)
, pt_numerator(0)
, pt_denominator(0)
, input_buffer(Eigen::MatrixXd::Zero(xSize, filter_numerator.size()))
, output_buffer(Eigen::MatrixXd::Zero(xSize,filter_denominator.size()-1))
{
assert(timestep>0.0 && "Timestep should be > 0");
assert(m_filter_numerator.size() == m_filter_order_m);
assert(m_filter_denominator.size() == m_filter_order_n);
}
void CausalFilter::get_x_dx_ddx(const Eigen::VectorXd& base_x,
Eigen::VectorXd& x_output_dx_ddx)
{
//const dynamicgraph::Vector &base_x = m_xSIN(iter);
if(first_sample)
{
for(int i=0;i<m_filter_order_m; i++)
input_buffer.col(i) = base_x;
for(int i=0;i<m_filter_order_n-1; i++)
output_buffer.col(i) = base_x*m_filter_numerator.sum()/m_filter_denominator.sum();
first_sample = false;
}
input_buffer.col(pt_numerator) = base_x;
Eigen::VectorXd b(m_filter_order_m);
Eigen::VectorXd a(m_filter_order_n-1);
b.head(pt_numerator+1) = m_filter_numerator.head(pt_numerator+1).reverse();
b.tail(m_filter_order_m-pt_numerator-1) =
m_filter_numerator.tail(m_filter_order_m-pt_numerator-1).reverse();
a.head(pt_denominator+1) = m_filter_denominator.segment(1, pt_denominator+1).reverse();
a.tail(m_filter_order_n-pt_denominator-2) =
m_filter_denominator.tail(m_filter_order_n-pt_denominator-2).reverse();
x_output_dx_ddx.head(m_x_size) = (input_buffer*b-output_buffer*a)/m_filter_denominator[0];
//Finite Difference
Eigen::VectorXd::Index pt_denominator_prev = (pt_denominator == 0) ? m_filter_order_n-2 : pt_denominator-1;
x_output_dx_ddx.segment(m_x_size,m_x_size) = (x_output_dx_ddx.head(m_x_size)-output_buffer.col(pt_denominator))/m_dt;
x_output_dx_ddx.tail(m_x_size) = (x_output_dx_ddx.head(m_x_size)-2*output_buffer.col(pt_denominator)+output_buffer.col(pt_denominator_prev))/m_dt/m_dt;
pt_numerator = (pt_numerator+1) < m_filter_order_m ? (pt_numerator+1) : 0;
pt_denominator = (pt_denominator+1) < m_filter_order_n-1 ? (pt_denominator+1) : 0;
output_buffer.col(pt_denominator) = x_output_dx_ddx.head(m_x_size);
return;
}
void CausalFilter::switch_filter(const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator)
{
Eigen::VectorXd::Index filter_order_m = filter_numerator.size();
Eigen::VectorXd::Index filter_order_n = filter_denominator.size();
Eigen::VectorXd current_x(input_buffer.col(pt_numerator));
input_buffer.resize(Eigen::NoChange, filter_order_m);
output_buffer.resize(Eigen::NoChange, filter_order_n-1);
for(int i=0;i<filter_order_m; i++)
input_buffer.col(i) = current_x;
for(int i=0;i<filter_order_n-1; i++)
output_buffer.col(i) = current_x*filter_numerator.sum()/filter_denominator.sum();
m_filter_order_m = filter_order_m;
m_filter_numerator.resize(filter_order_m);
m_filter_numerator = filter_numerator;
m_filter_order_n = filter_order_n;
m_filter_denominator.resize(filter_order_n);
m_filter_denominator = filter_denominator;
pt_numerator = 0;
pt_denominator = 0;
return;
}
src/madgwickahrs.cpp deleted 100644 → 0
View file @ 79073023
//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date Author Notes
// 29/09/2011 SOH Madgwick Initial release
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
// 11/05/2017 T Flayols Make it a dynamic-graph entity
//
//=====================================================================================================
#include <dynamic-graph/factory.h>
#include <sot/core/debug.hh>
#include <sot/torque_control/madgwickahrs.hh>
#include <sot/torque_control/commands-helper.hh>
#include <sot/torque_control/utils/stop-watch.hh>
namespace dynamicgraph
{
namespace sot
{
namespace torque_control
{
namespace dg = ::dynamicgraph;
using namespace dg;
using namespace dg::command;
using namespace std;
using namespace dg::sot;
typedef dg::sot::Vector6d Vector6;
#define PROFILE_MADGWICKAHRS_COMPUTATION "MadgwickAHRS computation"
#define INPUT_SIGNALS m_accelerometerSIN << m_gyroscopeSIN
#define OUTPUT_SIGNALS m_imu_quatSOUT
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef MadgwickAHRS EntityClassName;
/* --- DG FACTORY ---------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MadgwickAHRS,
"MadgwickAHRS");