Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • lscherrer/sot-talos-balance
  • imaroger/sot-talos-balance
  • pfernbac/sot-talos-balance
  • ostasse/sot-talos-balance
  • gsaurel/sot-talos-balance
  • loco-3d/sot-talos-balance
6 results
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 1395 additions and 877 deletions
include/sot/talos_balance/control-manager.hh deleted 100644 → 0
View file @ 874589ab
/*
* 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_control_manager_H__
#define __sot_torque_control_control_manager_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (__sot_torque_control_control_manager_H__)
# define SOTCONTROLMANAGER_EXPORT __declspec(dllexport)
# else
# define SOTCONTROLMANAGER_EXPORT __declspec(dllimport)
# endif
#else
# define SOTCONTROLMANAGER_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"
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 CtrlMode
{
public:
int id;
std::string name;
CtrlMode(): id(-1), name("None"){}
CtrlMode(int id, const std::string& name):
id(id), name(name) {}
};
std::ostream& operator<<( std::ostream& os, const CtrlMode& s )
{
os<<s.id<<"_"<<s.name;
return os;
}
class SOTCONTROLMANAGER_EXPORT ControlManager
:public::dynamicgraph::Entity
{
typedef Eigen::VectorXd::Index Index;
typedef ControlManager EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
ControlManager( 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 & robotRef);
/* --- SIGNALS --- */
std::vector<dynamicgraph::SignalPtr<dynamicgraph::Vector,int>*> m_ctrlInputsSIN;
std::vector< dynamicgraph::SignalPtr<bool,int>* > m_emergencyStopVector; /// emergency stop inputs. If one is true, control is set to zero forever
std::vector<dynamicgraph::Signal<dynamicgraph::Vector,int>*> m_jointsCtrlModesSOUT;
DECLARE_SIGNAL_IN(u_max, dynamicgraph::Vector); /// max motor control
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector); /// raw motor control
DECLARE_SIGNAL_OUT(u_safe, dynamicgraph::Vector); /// safe motor control
/* --- COMMANDS --- */
/// Commands related to the control mode.
void addCtrlMode(const std::string& name);
void ctrlModes();
void getCtrlMode(const std::string& jointName);
void setCtrlMode(const std::string& jointName, const std::string& ctrlMode);
void setCtrlMode(const int jid, const CtrlMode& cm);
void resetProfiler();
/// Commands related to joint name and joint id
// void setNameToId(const std::string& jointName, const double & jointId);
// void setJointLimitsFromId(const double &jointId, const double &lq, const double &uq);
/// Set the mapping between urdf and sot.
// void setJoints(const dynamicgraph::Vector &);
// void setStreamPrintPeriod(const double & s);
void setSleepTime(const double &seconds);
void addEmergencyStopSIN(const std::string& name);
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
protected:
RobotUtil * m_robot_util;
unsigned int m_numDofs;
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)
std::vector<std::string> m_ctrlModes; /// existing control modes
std::vector<CtrlMode> m_jointCtrlModes_current; /// control mode of the joints
std::vector<CtrlMode> m_jointCtrlModes_previous; /// previous control mode of the joints
std::vector<int> m_jointCtrlModesCountDown; /// counters used for the transition between two ctrl modes
bool convertStringToCtrlMode(const std::string& name, CtrlMode& cm);
bool convertJointNameToJointId(const std::string& name, unsigned int& id);
//bool isJointInRange(unsigned int id, double q);
void updateJointCtrlModesOutputSignal();
}; // class ControlManager
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_control_manager_H__
include/sot/talos_balance/admittance-controller.hh → include/sot/talos_balance/coupled-admittance-controller.hh
View file @ e217f893
......@@ -21,75 +21,74 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (admittance_controller_EXPORTS)
# define ADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
# else
# define ADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(admittance_controller_EXPORTS)
#define COUPLEDADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
#else
# define ADMITTANCECONTROLLER_EXPORT
#define COUPLEDADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define COUPLEDADMITTANCECONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class ADMITTANCECONTROLLER_EXPORT AdmittanceController
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
class COUPLEDADMITTANCECONTROLLER_EXPORT CoupledAdmittanceController
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
AdmittanceController( const std::string & name );
/* --- CONSTRUCTOR ---- */
CoupledAdmittanceController(const std::string& name);
void init(const double & dt, const unsigned & n);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(kSum, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kDiff, dynamicgraph::Vector);
void setPosition(const dynamicgraph::Vector & position);
DECLARE_SIGNAL_IN(tauL, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(tauR, dynamicgraph::Vector);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(state, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(tau, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(tauDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(tauDesL, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(tauDesR, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dqRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(qRef, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(tauSum, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(tauDiff, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
DECLARE_SIGNAL_INNER(tauDesSum, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(tauDesDiff, dynamicgraph::Vector);
bool m_useState;
protected:
int m_n;
bool m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_Kp;
dynamicgraph::Vector m_q; // internal state
double m_dt;
DECLARE_SIGNAL_INNER(dqRefSum, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(dqRefDiff, dynamicgraph::Vector);
}; // class AdmittanceController
DECLARE_SIGNAL_OUT(dqRefL, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dqRefR, dynamicgraph::Vector);
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
}; // class AdmittanceController
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_admittance_controller_H__
#endif // #ifndef __sot_talos_balance_admittance_controller_H__
include/sot/talos_balance/dcm-com-controller.hh
View file @ e217f893
......@@ -21,78 +21,76 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (dcm_com_controller_EXPORTS)
# define DCMCOMCONTROLLER_EXPORT __declspec(dllexport)
# else
# define DCMCOMCONTROLLER_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(dcm_com_controller_EXPORTS)
#define DCMCOMCONTROLLER_EXPORT __declspec(dllexport)
#else
# define DCMCOMCONTROLLER_EXPORT
#define DCMCOMCONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define DCMCOMCONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class DCMCOMCONTROLLER_EXPORT DcmComController
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --- CONSTRUCTOR ---- */
DcmComController( const std::string & name );
class DCMCOMCONTROLLER_EXPORT DcmComController : public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
void init(const double & dt);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
void resetDcmIntegralError();
/* --- CONSTRUCTOR ---- */
DcmComController(const std::string& name);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(decayFactor, double);
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(mass, double);
DECLARE_SIGNAL_IN(dcm, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dcmDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(comDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ddcomDes, dynamicgraph::Vector);
void init(const double& dt);
DECLARE_SIGNAL_OUT(ddcomRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmpRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector);
void resetDcmIntegralError();
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(decayFactor, double);
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(mass, double);
DECLARE_SIGNAL_IN(dcm, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dcmDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(comDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ddcomDes, dynamicgraph::Vector);
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_dcmIntegralError; // internal state
double m_dt;
DECLARE_SIGNAL_OUT(ddcomRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmpRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector);
}; // class DcmComController
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_dcmIntegralError; // internal state
double m_dt;
}; // class DcmComController
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_dcm_com_controller_H__
#endif // #ifndef __sot_talos_balance_dcm_com_controller_H__
include/sot/talos_balance/dcm-controller.hh
View file @ e217f893
......@@ -21,77 +21,77 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (dcm_controller_EXPORTS)
# define DCMCONTROLLER_EXPORT __declspec(dllexport)
# else
# define DCMCONTROLLER_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(dcm_controller_EXPORTS)
#define DCMCONTROLLER_EXPORT __declspec(dllexport)
#else
# define DCMCONTROLLER_EXPORT
#define DCMCONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define DCMCONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class DCMCONTROLLER_EXPORT DcmController
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --- CONSTRUCTOR ---- */
DcmController( const std::string & name );
class DCMCONTROLLER_EXPORT DcmController : public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
void init(const double & dt);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
void resetDcmIntegralError();
/* --- CONSTRUCTOR ---- */
DcmController(const std::string& name);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(decayFactor, double);
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(mass, double);
DECLARE_SIGNAL_IN(com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dcm, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dcmDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmpDes, dynamicgraph::Vector);
void init(const double& dt);
DECLARE_SIGNAL_OUT(zmpRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector);
void resetDcmIntegralError();
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(Kz, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(decayFactor, double);
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(mass, double);
DECLARE_SIGNAL_IN(com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dcm, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dcmDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmpDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmp, dynamicgraph::Vector);
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_dcmIntegralError; // internal state
double m_dt;
DECLARE_SIGNAL_OUT(zmpRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector);
}; // class DcmController
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_dcmIntegralError; // internal state
double m_dt;
}; // class DcmController
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_dcm_controller_H__
#endif // #ifndef __sot_talos_balance_dcm_controller_H__
include/sot/talos_balance/dcm-estimator.hh
View file @ e217f893
/*
* Copyright 2019,
* Copyright 2019,
* LAAS-CNRS,
* Gepetto team
* Gepetto team
*
* This file is part of sot-talos-balance.
* See license file
* See license file
*/
#ifndef __sot_talos_balance_dcm_estimator_H__
......@@ -14,91 +14,90 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (dcm_estimator_EXPORTS)
# define DCMESTIMATOR_EXPORT __declspec(dllexport)
# else
# define DCMESTIMATOR_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(dcm_estimator_EXPORTS)
#define DCMESTIMATOR_EXPORT __declspec(dllexport)
#else
# define DCMESTIMATOR_EXPORT
#define DCMESTIMATOR_EXPORT __declspec(dllimport)
#endif
#else
#define DCMESTIMATOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinocchio first
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <boost/math/distributions/normal.hpp> // for normal_distribution
#include <map>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
#include "boost/assign.hpp"
#include <boost/math/distributions/normal.hpp> // for normal_distribution
/* Pinocchio */
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <sot/core/robot-utils.hh>
namespace dynamicgraph
{
namespace sot
{
namespace talos_balance
{
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class DCMESTIMATOR_EXPORT DcmEstimator
:public::dynamicgraph::Entity
{
typedef pinocchio::SE3 SE3;
typedef Eigen::Vector2d Vector2;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Vector4d Vector4;
typedef Vector6d Vector6;
typedef Vector7d Vector7;
typedef Eigen::Matrix3d Matrix3;
typedef boost::math::normal normal;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
DcmEstimator(const std::string & name );
void init(const double & dt, const std::string& urdfFile);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(c, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dc, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
RobotUtil* m_robot_util;
pinocchio::Data m_data; /// Pinocchio robot data
Eigen::VectorXd m_q_pin; /// robot configuration according to pinocchio convention
Eigen::VectorXd m_v_pin; /// robot velocities according to pinocchio convention
double m_dt; /// sampling time step
pinocchio::Model m_model; /// Pinocchio robot model
}; // class DCMEstimator
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#endif // #ifndef __sot_talos_balance_dcm_estimator_H__
class DCMESTIMATOR_EXPORT DcmEstimator : public ::dynamicgraph::Entity {
typedef pinocchio::SE3 SE3;
typedef Eigen::Vector2d Vector2;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Vector4d Vector4;
typedef Vector6d Vector6;
typedef Vector7d Vector7;
typedef Eigen::Matrix3d Matrix3;
typedef boost::math::normal normal;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
DcmEstimator(const std::string& name);
void init(const double& dt, const std::string& urdfFile);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(c, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dc, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
RobotUtilShrPtr m_robot_util;
pinocchio::Data m_data; /// Pinocchio robot data
Eigen::VectorXd
m_q_pin; /// robot configuration according to pinocchio convention
Eigen::VectorXd
m_v_pin; /// robot velocities according to pinocchio convention
double m_dt; /// sampling time step
pinocchio::Model m_model; /// Pinocchio robot model
}; // class DCMEstimator
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_dcm_estimator_H__
include/sot/talos_balance/delay.hh 0 → 100644
View file @ e217f893
// Copyright 2018 CNRS - Airbus SAS
// Author: Joseph Mirabel
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef __sot_talos_balance_delay_H__
#define __sot_talos_balance_delay_H__
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal.h>
#if defined(WIN32)
#if defined(delay_EXPORTS)
#define DELAY_EXPORT __declspec(dllexport)
#else
#define DELAY_EXPORT __declspec(dllimport)
#endif
#else
#define DELAY_EXPORT
#endif
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/// Delay
template <typename Value, typename Time = int>
class DELAY_EXPORT Delay : public dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
Delay(const std::string& name)
: Entity(name),
sin(NULL, "Delay(" + name + ")::input(unspecified)::sin"),
currentOUT("Delay(" + name + ")::output(unspecified)::current"),
previousOUT("Delay(" + name + ")::output(unspecified)::previous") {
currentOUT.setFunction(boost::bind(&Delay::current, this, _1, _2));
previousOUT.setFunction(boost::bind(&Delay::previous, this, _1, _2));
signalRegistration(sin << currentOUT << previousOUT);
using command::makeCommandVoid1;
std::string docstring =
"\n"
" Set current value in memory\n";
addCommand("setMemory",
makeCommandVoid1(*this, &Delay::setMemory, docstring));
}
~Delay() {}
/// Header documentation of the python class
virtual std::string getDocString() const {
return "Enable delayed signal in SoT.\n"
"Signal previous at time t+1 will return the value of signal "
"current "
" at time <= t (the last time signal current was called).";
}
void setMemory(const Value& val) { mem = val; }
private:
Value& current(Value& res, const Time& time) {
previousOUT.access(time);
res = sin.access(time);
mem = res;
return res;
}
Value& previous(Value& res, const Time&) {
res = mem;
return res;
}
Value mem;
SignalPtr<Value, Time> sin;
Signal<Value, Time> currentOUT, previousOUT;
};
typedef Delay<Vector, int> DelayVector;
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_simple_pid_H__
include/sot/talos_balance/distribute-wrench.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance 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-talos-balance 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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_distribute_wrench_H__
#define __sot_talos_balance_distribute_wrench_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define DISTRIBUTE_WRENCH_EXPORT __declspec(dllexport)
#else
#define DISTRIBUTE_WRENCH_EXPORT __declspec(dllimport)
#endif
#else
#define DISTRIBUTE_WRENCH_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinochcio first
#include <dynamic-graph/signal-helper.h>
#include <eiquadprog/eiquadprog-fast.hpp>
#include <map>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/multibody/model.hpp>
#include <sot/core/robot-utils.hh>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class DISTRIBUTE_WRENCH_EXPORT DistributeWrench
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
DistributeWrench(const std::string& name);
void init(const std::string& robotName);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(wrenchDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(rho, double);
DECLARE_SIGNAL_IN(phase, int);
DECLARE_SIGNAL_IN(frictionCoefficient, double);
DECLARE_SIGNAL_IN(wSum, double);
DECLARE_SIGNAL_IN(wNorm, double);
DECLARE_SIGNAL_IN(wRatio, double);
DECLARE_SIGNAL_IN(wAnkle, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(kinematics_computations, int);
DECLARE_SIGNAL_INNER(qp_computations, int);
DECLARE_SIGNAL_OUT(wrenchLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ankleWrenchLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(surfaceWrenchLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(copLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ankleWrenchRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(surfaceWrenchRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(copRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmpRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(emergencyStop, bool);
public:
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
Eigen::Vector3d computeCoP(const dynamicgraph::Vector& wrench,
const pinocchio::SE3& pose) const;
void set_right_foot_sizes(const dynamicgraph::Vector& s);
void set_left_foot_sizes(const dynamicgraph::Vector& s);
double m_eps;
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
pinocchio::Model m_model; /// Pinocchio robot model
pinocchio::Data m_data; /// Pinocchio robot data
RobotUtilShrPtr m_robot_util;
// pinocchio::SE3 m_ankle_M_ftSens; /// ankle to F/T sensor
// transformation
pinocchio::SE3 m_ankle_M_sole; /// ankle to sole transformation
pinocchio::FrameIndex m_left_foot_id;
pinocchio::FrameIndex m_right_foot_id;
pinocchio::SE3 m_contactLeft;
pinocchio::SE3 m_contactRight;
Eigen::Matrix<double, 6, 1> m_wrenchLeft;
Eigen::Matrix<double, 6, 1> m_wrenchRight;
Eigen::Vector4d m_left_foot_sizes; /// sizes of the left foot (pos x, neg x,
/// pos y, neg y)
Eigen::Vector4d m_right_foot_sizes; /// sizes of the left foot (pos x, neg x,
/// pos y, neg y)
void computeWrenchFaceMatrix(const double mu);
Eigen::Matrix<double, 16, 6> m_wrenchFaceMatrix; // for modelling contact
eiquadprog::solvers::EiquadprogFast m_qp1; // saturate wrench
eiquadprog::solvers::EiquadprogFast m_qp2; // distribute wrench
// QP problem matrices -- SSP
Eigen::MatrixXd m_Q1;
Eigen::VectorXd m_C1;
Eigen::MatrixXd m_Aeq1;
Eigen::VectorXd m_Beq1;
Eigen::MatrixXd m_Aineq1;
Eigen::VectorXd m_Bineq1;
Eigen::VectorXd m_result1;
// QP problem matrices -- DSP
Eigen::MatrixXd m_Q2;
Eigen::VectorXd m_C2;
Eigen::MatrixXd m_Aeq2;
Eigen::VectorXd m_Beq2;
Eigen::MatrixXd m_Aineq2;
Eigen::VectorXd m_Bineq2;
Eigen::VectorXd m_result2;
double m_wSum;
double m_wNorm;
double m_wRatio;
Eigen::VectorXd m_wAnkle;
void distributeWrench(const Eigen::VectorXd& wrenchDes, const double rho,
const double mu);
void saturateWrench(const Eigen::VectorXd& wrenchDes, const int phase,
const double mu);
bool m_emergency_stop_triggered;
}; // class DistributeWrench
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_distribute_wrench_H__
include/sot/talos_balance/dummy-dcm-estimator.hh
View file @ e217f893
......@@ -21,67 +21,66 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (dummy_dcm_estimator_EXPORTS)
# define DUMMYDCMESTIMATOR_EXPORT __declspec(dllexport)
# else
# define DUMMYDCMESTIMATOR_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(dummy_dcm_estimator_EXPORTS)
#define DUMMYDCMESTIMATOR_EXPORT __declspec(dllexport)
#else
# define DUMMYDCMESTIMATOR_EXPORT
#define DUMMYDCMESTIMATOR_EXPORT __declspec(dllimport)
#endif
#else
#define DUMMYDCMESTIMATOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class DUMMYDCMESTIMATOR_EXPORT DummyDcmEstimator
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --- CONSTRUCTOR ---- */
DummyDcmEstimator( const std::string & name );
class DUMMYDCMESTIMATOR_EXPORT DummyDcmEstimator
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
void init();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(mass, double);
DECLARE_SIGNAL_IN(com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(momenta, dynamicgraph::Vector);
/* --- CONSTRUCTOR ---- */
DummyDcmEstimator(const std::string& name);
DECLARE_SIGNAL_OUT(dcm, dynamicgraph::Vector);
void init();
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(mass, double);
DECLARE_SIGNAL_IN(com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(momenta, dynamicgraph::Vector);
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
DECLARE_SIGNAL_OUT(dcm, dynamicgraph::Vector);
}; // class DummyDcmEstimator
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
}; // class DummyDcmEstimator
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_dummy_dcm_estimator_H__
#endif // #ifndef __sot_talos_balance_dummy_dcm_estimator_H__
include/sot/talos_balance/dummy-walking-pattern-generator.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance 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-talos-balance 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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_dummy_walking_pattern_generator_H__
#define __sot_talos_balance_dummy_walking_pattern_generator_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(dummy_walking_pattern_generator_EXPORTS)
#define DUMMYWALKINGPATTERNGENERATOR_EXPORT __declspec(dllexport)
#else
#define DUMMYWALKINGPATTERNGENERATOR_EXPORT __declspec(dllimport)
#endif
#else
#define DUMMYWALKINGPATTERNGENERATOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinocchio first
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-helper.h>
#include <map>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class DUMMYWALKINGPATTERNGENERATOR_EXPORT DummyWalkingPatternGenerator
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
DummyWalkingPatternGenerator(const std::string& name);
void init();
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(rho, double);
DECLARE_SIGNAL_IN(phase, int);
DECLARE_SIGNAL_IN(footLeft, MatrixHomogeneous);
DECLARE_SIGNAL_IN(footRight, MatrixHomogeneous);
DECLARE_SIGNAL_IN(waist, MatrixHomogeneous);
DECLARE_SIGNAL_IN(com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(vcom, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(acom, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(referenceFrame, MatrixHomogeneous);
DECLARE_SIGNAL_INNER(rf, MatrixHomogeneous);
DECLARE_SIGNAL_OUT(comDes, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(vcomDes, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(acomDes, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dcmDes, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmpDes, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(footLeftDes, MatrixHomogeneous);
DECLARE_SIGNAL_OUT(footRightDes, MatrixHomogeneous);
DECLARE_SIGNAL_OUT(waistDes, MatrixHomogeneous);
DECLARE_SIGNAL_OUT(omegaDes, double);
DECLARE_SIGNAL_OUT(rhoDes, double);
DECLARE_SIGNAL_OUT(phaseDes, int);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector actInv(MatrixHomogeneous m, dynamicgraph::Vector v);
MatrixHomogeneous actInv(MatrixHomogeneous m1, MatrixHomogeneous m2);
}; // class DummyWalkingPatternGenerator
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_dummy_walking_pattern_generator_H__
include/sot/talos_balance/euler-to-quat.hh
View file @ e217f893
......@@ -21,64 +21,59 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (position_controller_EXPORTS)
# define EULERTOQUAT_EXPORT __declspec(dllexport)
# else
# define EULERTOQUAT_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define EULERTOQUAT_EXPORT __declspec(dllexport)
#else
# define EULERTOQUAT_EXPORT
#define EULERTOQUAT_EXPORT __declspec(dllimport)
#endif
#else
#define EULERTOQUAT_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
#include <sot/core/matrix-geometry.hh>
#include <dynamic-graph/linear-algebra.h>
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include "boost/assign.hpp"
class EULERTOQUAT_EXPORT EulerToQuat
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --- CONSTRUCTOR ---- */
EulerToQuat( const std::string & name );
class EULERTOQUAT_EXPORT EulerToQuat : public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
void init() {}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(euler, ::dynamicgraph::Vector);
/* --- CONSTRUCTOR ---- */
EulerToQuat(const std::string& name);
DECLARE_SIGNAL_OUT(quaternion, ::dynamicgraph::Vector);
void init() {}
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
}; // class EulerToQuat
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(euler, ::dynamicgraph::Vector);
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
DECLARE_SIGNAL_OUT(quaternion, ::dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
}; // class EulerToQuat
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_euler_to_quat_H__
#endif // #ifndef __sot_talos_balance_euler_to_quat_H__
include/sot/talos_balance/example.hh
View file @ e217f893
......@@ -21,69 +21,69 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (position_controller_EXPORTS)
# define EXAMPLE_EXPORT __declspec(dllexport)
# else
# define EXAMPLE_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define EXAMPLE_EXPORT __declspec(dllexport)
#else
# define EXAMPLE_EXPORT
#define EXAMPLE_EXPORT __declspec(dllimport)
#endif
#else
#define EXAMPLE_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinocchio first
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
#include <sot/core/robot-utils.hh>
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include "boost/assign.hpp"
class EXAMPLE_EXPORT Example
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --- CONSTRUCTOR ---- */
Example( const std::string & name );
class EXAMPLE_EXPORT Example : public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
void init(const std::string& robotName);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(firstAddend, double);
DECLARE_SIGNAL_IN(secondAddend, double);
/* --- CONSTRUCTOR ---- */
Example(const std::string& name);
DECLARE_SIGNAL_OUT(sum, double);
DECLARE_SIGNAL_OUT(nbJoints, int);
void init(const std::string& robotName);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(firstAddend, double);
DECLARE_SIGNAL_IN(secondAddend, double);
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
RobotUtil* m_robot_util;
DECLARE_SIGNAL_OUT(sum, double);
DECLARE_SIGNAL_OUT(nbJoints, int);
}; // class Example
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
RobotUtilShrPtr m_robot_util;
}; // class Example
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_example_H__
#endif // #ifndef __sot_talos_balance_example_H__
include/sot/talos_balance/filter-differentiator.hh deleted 100644 → 0
View file @ 874589ab
/*
* 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/>.
*/
#ifndef __sot_torque_control_FilterDifferentiator_H__
#define __sot_torque_control_FilterDifferentiator_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (low_pass_filter_EXPORTS)
# define SOTFILTERDIFFERENTIATOR_EXPORT __declspec(dllexport)
# else
# define SOTFILTERDIFFERENTIATOR_EXPORT __declspec(dllimport)
# endif
#else
# define SOTFILTERDIFFERENTIATOR_EXPORT
#endif
//#define VP_DEBUG 1 /// enable debug output
//#define VP_DEBUG_MODE 20
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* HELPER */
#include <dynamic-graph/signal-helper.h>
#include <sot/talos_balance/utils/stop-watch.hh>
#include <sot/talos_balance/utils/causal-filter.hh>
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/**
* This Entity takes as inputs a signal and applies a low pass filter
and computes finite difference derivative.
*/
class SOTFILTERDIFFERENTIATOR_EXPORT FilterDifferentiator
:public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public: /* --- SIGNALS --- */
DECLARE_SIGNAL_IN(x, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(x_filtered, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
/// The following inner signals are used because this entity has some output signals
/// whose related quantities are computed at the same time by the same algorithm
/// To avoid the risk of recomputing the same things twice, we create an inner signal that groups together
/// all the quantities that are computed together. Then the single output signals will depend
/// on this inner signal, which is the one triggering the computations.
/// Inner signals are not exposed, so that nobody can access them.
/// This signal contains the estimated positions, velocities and accelerations.
DECLARE_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector);
protected:
double m_dt; /// sampling timestep of the input signal
int m_x_size;
/// polynomial-fitting filters
CausalFilter* m_filter;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** --- CONSTRUCTOR ---- */
FilterDifferentiator( const std::string & name );
/** Initialize the FilterDifferentiator.
* @param timestep Period (in seconds) after which the sensors' data are updated.
* @param sigSize Size of the input signal.
* @param delay Delay (in seconds) introduced by the estimation.
* This should be a multiple of timestep.
* @note The estimationDelay is half of the length of the window used for the
* polynomial fitting. The larger the delay, the smoother the estimations.
*/
void init(const double &timestep,
const int& xSize,
const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator);
void switch_filter(const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator);
protected:
public: /* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
}; // class FilterDifferentiator
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_FilterDifferentiator_H__
include/sot/talos_balance/foot-force-difference-controller.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance 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-talos-balance 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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_foot_force_difference_controller_H__
#define __sot_talos_balance_foot_force_difference_controller_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(foot_force_difference_controller_EXPORTS)
#define FOOT_FORCE_DIFFERENCE_CONTROLLER_EXPORT __declspec(dllexport)
#else
#define FOOT_FORCE_DIFFERENCE_CONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define FOOT_FORCE_DIFFERENCE_CONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-helper.h>
#include <map>
#include <sot/core/matrix-geometry.hh>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class FOOT_FORCE_DIFFERENCE_CONTROLLER_EXPORT FootForceDifferenceController
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
FootForceDifferenceController(const std::string& name);
void init();
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(phase, int);
DECLARE_SIGNAL_IN(gainSwing, double);
DECLARE_SIGNAL_IN(gainStance, double);
DECLARE_SIGNAL_IN(gainDouble, double);
DECLARE_SIGNAL_IN(dfzAdmittance, double);
DECLARE_SIGNAL_IN(vdcFrequency, double);
DECLARE_SIGNAL_IN(vdcDamping, double);
DECLARE_SIGNAL_IN(swingAdmittance, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrenchRightDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrenchLeftDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrenchRight, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrenchLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posRightDes, MatrixHomogeneous);
DECLARE_SIGNAL_IN(posLeftDes, MatrixHomogeneous);
DECLARE_SIGNAL_IN(posRight, MatrixHomogeneous);
DECLARE_SIGNAL_IN(posLeft, MatrixHomogeneous);
DECLARE_SIGNAL_INNER(dz_ctrl, double);
DECLARE_SIGNAL_INNER(dz_pos, double);
DECLARE_SIGNAL_OUT(vRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(vLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(gainRight, double);
DECLARE_SIGNAL_OUT(gainLeft, double);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
protected:
Eigen::Vector3d calcSwingAdmittance(
const dynamicgraph::Vector& wrench,
const dynamicgraph::Vector& swingAdmittance);
double m_eps;
bool
m_initSucceeded; /// true if the entity has been successfully initialized
}; // class FootForceDifferenceController
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_foot_force_difference_controller_H__
include/sot/talos_balance/ft-calibration.hh
View file @ e217f893
......@@ -2,91 +2,113 @@
* Copyright 2019
* LAAS-CNRS
* F. Bailly
* T. Flayols
*/
#ifndef __sot_torque_control_ft_calibration_H__
#define __sot_torque_control_ft_calibration_H__
#ifndef __sot_talos_balance_ft_calibration_H__
#define __sot_talos_balance_ft_calibration_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (__sot_talos_balance_ft_calibration_H__)
# define SOTFtCalibration_EXPORT __declspec(dllexport)
# else
# define SOTFtCalibration_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(__sot_talos_balance_ft_calibration_H__)
#define SOTFTCALIBRATION_EXPORT __declspec(dllexport)
#else
# define SOTPFtCalibration_EXPORT
#define SOTFTCALIBRATION_EXPORT __declspec(dllimport)
#endif
#else
#define SOTFTCALIBRATION_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinocchio first
#include <dynamic-graph/real-time-logger.h>
#include <dynamic-graph/signal-helper.h>
#include <map>
#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 ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTFtCalibration_EXPORT FtCalibration
:public::dynamicgraph::Entity
{
typedef FtCalibration EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
FtCalibration( const std::string & name);
/// Initialize
void init(const std::string & robotRef);
/* --- SIGNALS --- */
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(force_in, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(force_out, dynamicgraph::Vector);
/* --- COMMANDS --- */
/// Commands for setting the feet weight
void setRightFootWeight(const dynamicgraph::Vector &rightW);
void setLeftFootWeight(const dynamicgraph::Vector &leftW);
void displayRobotUtil();
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
protected:
RobotUtil * m_robot_util;
bool m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_right_foot_weight // weight of the right feet underneath the ft sensor
dynamicgraph::Vector m_left_foot_weight // weight of the left feet underneath the ft sensor
}; // class FtCalibration
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#endif // #ifndef __sot_talos_balance_ft_calibration_H__
class SOTFTCALIBRATION_EXPORT FtCalibration : public ::dynamicgraph::Entity {
// typedef FtCalibration EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
FtCalibration(const std::string &name);
/// Initialize
void init(const std::string &robotRef);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(right_foot_force_in, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(left_foot_force_in, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_force_out, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_force_out, dynamicgraph::Vector);
/* --- COMMANDS --- */
/// Commands for setting the feet weight
void setRightFootWeight(const double &rightW);
void setLeftFootWeight(const double &leftW);
/// Command to calibrate the foot sensors when the robot is standing in the
/// air with horizontal feet
void calibrateFeetSensor();
void displayRobotUtil();
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
/* --- TYPEDEFS ---- */
typedef Eigen::Matrix<double, 6, 1> Vector6d;
protected:
RobotUtilShrPtr m_robot_util;
int m_right_calibration_iter =
-1; /// Number of iteration left for calibration (-1= not cailbrated,
/// 0=caibration done)
int m_left_calibration_iter =
-1; /// Number of iteration left for calibration (-1= not cailbrated,
/// 0=caibration done)
Vector6d
m_right_FT_offset; /// Offset or bias to be removed from Right FT sensor
Vector6d
m_left_FT_offset; /// Offset or bias to be removed from Left FT sensor
Vector6d m_right_FT_offset_calibration_sum; /// Variable used durring average
/// computation of the offset
Vector6d m_left_FT_offset_calibration_sum; /// Variable used durring average
/// computation of the offset
bool
m_initSucceeded; /// true if the entity has been successfully initialized
Vector6d
m_right_foot_weight; // weight of the right feet underneath the ft sensor
Vector6d
m_left_foot_weight; // weight of the left feet underneath the ft sensor
}; // class FtCalibration
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_ft_calibration_H__
include/sot/talos_balance/ft-wrist-calibration.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2019
* LAAS-CNRS
* F. Bailly
* T. Flayol
* F. Risbourg
*/
#ifndef __sot_talos_balance_ft_wrist_calibration_H__
#define __sot_talos_balance_ft_wrist_calibration_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(__sot_talos_balance_ft_wrist_calibration_H__)
#define SOTFTWRISTCALIBRATION_EXPORT __declspec(dllexport)
#else
#define SOTFTWRISTCALIBRATION_EXPORT __declspec(dllimport)
#endif
#else
#define SOTFTWRISTCALIBRATION_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinocchio first
#include <dynamic-graph/real-time-logger.h>
#include <dynamic-graph/signal-helper.h>
#include <map>
#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/spatial/motion.hpp>
#include <pinocchio/spatial/se3.hpp>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
#include <sot/talos_balance/robot/robot-wrapper.hh>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTFTWRISTCALIBRATION_EXPORT FtWristCalibration
: public ::dynamicgraph::Entity {
// typedef FtCalibration EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
FtWristCalibration(const std::string &name);
/// Initialize
void init(const std::string &robotRef);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(rightWristForceIn, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(leftWristForceIn, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(rightWeight, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(leftWeight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(rightWristForceOut, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(leftWristForceOut, dynamicgraph::Vector);
/* --- COMMANDS --- */
/// Commands for setting the hand weight
void setRightHandConf(const double &rightW, const Vector &rightLeverArm);
void setLeftHandConf(const double &leftW, const Vector &leftLeverArm);
/// Command to calibrate the wrist sensors when the robot is in half sitting
/// with the hands aligned
void calibrateWristSensor();
/**
* @brief Set to true if you want to remove the weight from the force
*
* @param[in] removeWeight Boolean used to remove the weight
*/
void setRemoveWeight(const bool &removeWeight);
void displayRobotUtil();
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
/* --- TYPEDEFS ---- */
typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<double, 3, 1> Vector3d;
protected:
/// Robot Util instance to get the sensor frame
RobotUtilShrPtr m_robot_util;
/// Pinocchio robot model
pinocchio::Model m_model;
/// Pinocchio robot data
pinocchio::Data *m_data;
/// Id of the force sensor frame
pinocchio::FrameIndex m_rightSensorId;
/// Id of the joint of the end-effector
pinocchio::FrameIndex m_leftSensorId;
/// Number of iteration for right calibration (-2 = not calibrated, -1 =
/// caibration done)
int m_rightCalibrationIter = -2;
/// Number of iteration for right calibration (-2 = not calibrated, -1 =
/// caibration done)
int m_leftCalibrationIter = -2;
/// Offset or bias to be removed from Right FT sensor
Vector6d m_right_FT_offset;
/// Offset or bias to be removed from Left FT sensor
Vector6d m_left_FT_offset;
/// Variable used during average computation of the offset
Vector6d m_right_FT_offset_calibration_sum;
/// Variable used during average computation of the offset
Vector6d m_left_FT_offset_calibration_sum;
/// Variable used during average computation of the weight
Vector6d m_right_weight_calibration_sum;
/// Variable used during average computation of the weight
Vector6d m_left_weight_calibration_sum;
/// true if the entity has been successfully initialized
bool m_initSucceeded;
/// weight of the right hand
Vector6d m_rightHandWeight;
/// weight of the left hand
Vector6d m_leftHandWeight;
/// right hand lever arm
Vector3d m_rightLeverArm;
/// left hand lever arm
Vector3d m_leftLeverArm;
/// If true, the weight of the end effector is removed from the force
bool m_removeWeight;
}; // class FtWristCalibration
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_ft_wrist_calibration_H__
include/sot/talos_balance/hip-flexibility-compensation.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance 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-talos-balance 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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_hip_flexibility_compensation_H__
#define __sot_talos_balance_hip_flexibility_compensation_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(hip_flexibility_compensation_EXPORTS)
#define HIPFLEXIBILITYCOMPENSATION_EXPORT __declspec(dllexport)
#else
#define HIPFLEXIBILITYCOMPENSATION_EXPORT __declspec(dllimport)
#endif
#else
#define HIPFLEXIBILITYCOMPENSATION_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinocchio first
#include <dynamic-graph/signal-helper.h>
#include <map>
#include <sot/core/robot-utils.hh>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class HIPFLEXIBILITYCOMPENSATION_EXPORT HipFlexibilityCompensation
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
HipFlexibilityCompensation(const std::string& name);
/* --- SIGNALS --- */
/// \brief Walking phase
DECLARE_SIGNAL_IN(phase, int);
/// \brief Desired joint configuration of the robot
DECLARE_SIGNAL_IN(q_des, dynamicgraph::Vector);
/// \brief Current torque mesured at each joint
DECLARE_SIGNAL_IN(tau, dynamicgraph::Vector);
/// \brief Left flexibility correction for the angular computation
DECLARE_SIGNAL_IN(K_l, double);
/// \brief Right flexibility correction for the angular computation
DECLARE_SIGNAL_IN(K_r, double);
/// \brief Derivative gain (double) for the error
// DECLARE_SIGNAL_IN(K_d, double);
/// \brief Low pass filter of the signal tau
DECLARE_SIGNAL_OUT(tau_filt, dynamicgraph::Vector);
/// \brief Angular correction of the flexibility
DECLARE_SIGNAL_OUT(delta_q, dynamicgraph::Vector);
/// \brief Corrected desired joint configuration of the robot with
/// flexibility joint configuration q_cmd = q_des + RateLimiter(delta_q)
DECLARE_SIGNAL_OUT(q_cmd, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
/// \brief Initialize the entity
void init(const double& dt, const std::string& robotName);
/// \brief Set the LowPassFilter frequency for the torque computation.
void setTorqueLowPassFilterFrequency(const double& frequency);
/// \brief Set the value of the saturation for the angular correction
/// computation.
void setAngularSaturation(const double& saturation);
/// \brief Set the value of the limiter for the the rate limiter of delta_q.
void setRateLimiter(const double& rate);
/// \brief Compute the low pass filter of a signal given a frequency and the
/// previous signal.
dynamicgraph::Vector lowPassFilter(const double& frequency,
const dynamicgraph::Vector& signal,
dynamicgraph::Vector& previous_signal);
/// \brief Compute the limiter of a signal given the previous signal (based on
/// first derivative).
void rateLimiter(const dynamicgraph::Vector& signal,
dynamicgraph::Vector& previous_signal,
dynamicgraph::Vector& output);
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
// time step of the robot
double m_dt;
double m_torqueLowPassFilterFrequency;
double m_delta_q_saturation;
double m_rate_limiter;
dynamicgraph::Vector m_previous_delta_q;
dynamicgraph::Vector m_previous_tau;
dynamicgraph::Vector m_limitedSignal;
RobotUtilShrPtr m_robot_util;
}; // class HipFlexibilityCompensation
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_hip_flexibility_compensation_H__
include/sot/talos_balance/imu_offset_compensation.hh deleted 100644 → 0
View file @ 874589ab
/*
* 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
float 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__
include/sot/talos_balance/int-identity.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance 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-talos-balance 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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_int_identity_H__
#define __sot_talos_balance_int_identity_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define INT_IDENTITY_EXPORT __declspec(dllexport)
#else
#define INT_IDENTITY_EXPORT __declspec(dllimport)
#endif
#else
#define INT_IDENTITY_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class INT_IDENTITY_EXPORT IntIdentity : public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
IntIdentity(const std::string& name);
void init() {}
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(sin, int);
DECLARE_SIGNAL_OUT(sout, int);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
}; // class IntIdentity
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_int_identity_H__
include/sot/talos_balance/joint-position-controller.hh
View file @ e217f893
......@@ -21,69 +21,68 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (position_controller_EXPORTS)
# define JOINTPOSITIONCONTROLLER_EXPORT __declspec(dllexport)
# else
# define JOINTPOSITIONCONTROLLER_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define JOINTPOSITIONCONTROLLER_EXPORT __declspec(dllexport)
#else
# define JOINTPOSITIONCONTROLLER_EXPORT
#define JOINTPOSITIONCONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define JOINTPOSITIONCONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class JOINTPOSITIONCONTROLLER_EXPORT JointPositionController
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --- CONSTRUCTOR ---- */
JointPositionController( const std::string & name );
class JOINTPOSITIONCONTROLLER_EXPORT JointPositionController
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
void init(const unsigned & n);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(state, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(qDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dqDes, dynamicgraph::Vector);
/* --- CONSTRUCTOR ---- */
JointPositionController(const std::string& name);
DECLARE_SIGNAL_OUT(dqRef, dynamicgraph::Vector);
void init(const unsigned& n);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(state, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(qDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dqDes, dynamicgraph::Vector);
protected:
int m_n;
bool m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_Kp;
DECLARE_SIGNAL_OUT(dqRef, dynamicgraph::Vector);
}; // class JointPositionController
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
protected:
int m_n;
bool
m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_Kp;
}; // class JointPositionController
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_joint_position_controller_H__
#endif // #ifndef __sot_talos_balance_joint_position_controller_H__
include/sot/talos_balance/madgwickahrs.hh deleted 100644 → 0
View file @ 874589ab
//=====================================================================================================
//
// 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 <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <map>
#include "boost/assign.hpp"
#define betaDef 0.01f // 2 * proportional g
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- 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);
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
volatile float m_beta = betaDef; /// 2 * proportional gain (Kp)
volatile float m_q0 = 1.0f, m_q1 = 0.0f, m_q2 = 0.0f, m_q3 = 0.0f; /// quaternion of sensor frame
float m_sampleFreq = 512.0f; /// sample frequency in Hz
}; // class MadgwickAHRS
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_madgwickahrs_H__