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
Showing
with 2371 additions and 959 deletions
......@@ -21,75 +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 "utils/signal-helper.hh"
#include "utils/logger.hh"
#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
/* --- CONSTRUCTOR ---- */
JointPositionController( const std::string & name );
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
void init(const unsigned & n);
class JOINTPOSITIONCONTROLLER_EXPORT JointPositionController
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
/* --- 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);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
DECLARE_SIGNAL_OUT(dqRef, dynamicgraph::Vector);
/* --- CONSTRUCTOR ---- */
JointPositionController(const std::string& name);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
void init(const unsigned& n);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("[JointPositionController-"+name+"] "+msg, t, file, line);
}
/* --- 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__
//=====================================================================================================
//
// 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 <sot/talos_balance/utils/signal-helper.hh>
#include <sot/talos_balance/utils/vector-conversions.hh>
#include <sot/talos_balance/utils/logger.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);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("[MadgwickAHRS-"+name+"] "+msg, t, file, line);
}
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__
......@@ -21,50 +21,46 @@
#include <Eigen/Core>
#ifdef EIGEN_RUNTIME_NO_MALLOC
#define EIGEN_MALLOC_ALLOWED Eigen::internal::set_is_malloc_allowed(true);
#define EIGEN_MALLOC_NOT_ALLOWED Eigen::internal::set_is_malloc_allowed(false);
#define EIGEN_MALLOC_ALLOWED Eigen::internal::set_is_malloc_allowed(true);
#define EIGEN_MALLOC_NOT_ALLOWED Eigen::internal::set_is_malloc_allowed(false);
#else
#define EIGEN_MALLOC_ALLOWED
#define EIGEN_MALLOC_NOT_ALLOWED
#define EIGEN_MALLOC_ALLOWED
#define EIGEN_MALLOC_NOT_ALLOWED
#endif
namespace dynamicgraph
{
namespace sot
{
namespace talos_balance
{
namespace math
{
typedef double Scalar;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> Vector;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> Matrix;
typedef Eigen::VectorXi VectorXi;
typedef Eigen::Matrix<bool,Eigen::Dynamic,1> VectorXb;
typedef Eigen::Matrix<Scalar,3,1> Vector3;
typedef Eigen::Matrix<Scalar,6,1> Vector6;
typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic> Matrix3x;
typedef Eigen::Ref<Vector3> RefVector3;
typedef const Eigen::Ref<const Vector3> ConstRefVector3;
typedef Eigen::Ref<Vector> RefVector;
typedef const Eigen::Ref<const Vector> ConstRefVector;
typedef Eigen::Ref<Matrix> RefMatrix;
typedef const Eigen::Ref<const Matrix> ConstRefMatrix;
typedef std::size_t Index;
// Forward declaration of constraints
class ConstraintBase;
class ConstraintEquality;
class ConstraintInequality;
class ConstraintBound;
}
}
}
}
#endif // ifndef __invdyn_math_fwd_hpp__
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
namespace math {
typedef double Scalar;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::VectorXi VectorXi;
typedef Eigen::Matrix<bool, Eigen::Dynamic, 1> VectorXb;
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic> Matrix3x;
typedef Eigen::Ref<Vector3> RefVector3;
typedef const Eigen::Ref<const Vector3> ConstRefVector3;
typedef Eigen::Ref<Vector> RefVector;
typedef const Eigen::Ref<const Vector> ConstRefVector;
typedef Eigen::Ref<Matrix> RefMatrix;
typedef const Eigen::Ref<const Matrix> ConstRefMatrix;
typedef std::size_t Index;
// Forward declaration of constraints
class ConstraintBase;
class ConstraintEquality;
class ConstraintInequality;
class ConstraintBound;
} // namespace math
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // ifndef __invdyn_math_fwd_hpp__
/*
* Copyright 2015, Andrea Del Prete, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_torque_control_parameter_server_H__
#define __sot_torque_control_parameter_server_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (__sot_torque_parameter_server_H__)
# define SOTParameterServer_EXPORT __declspec(dllexport)
# else
# define SOTParameterServer_EXPORT __declspec(dllimport)
# endif
#else
# define SOTParameterServer_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <sot/talos_balance/utils/signal-helper.hh>
// #include <dynamic-graph/signal-helper.h>
//#include <sot/talos_balance/utils/vector-conversions.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/talos_balance/utils/logger.hh>
// #include <dynamic-graph/logger.h>
//#include <sot/core/robot-utils.hh>
#include <sot/core/robot-utils.hh>
#include <map>
#include "boost/assign.hpp"
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <sot/talos_balance/robot/robot-wrapper.hh>
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/// Number of time step to transition from one ctrl mode to another
#define CTRL_MODE_TRANSITION_TIME_STEP 1000.0
class SOTParameterServer_EXPORT ParameterServer
:public::dynamicgraph::Entity
{
typedef ParameterServer EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
ParameterServer( const std::string & name);
/// Initialize
/// @param dt: control interval
/// @param urdfFile: path to the URDF model of the robot
void init(const double & dt,
const std::string & urdfFile,
const std::string & robotRef);
/* --- SIGNALS --- */
/* --- COMMANDS --- */
/// Commands related to joint name and joint id
void setNameToId(const std::string& jointName, const double & jointId);
void setJointLimitsFromId(const double &jointId,
const double &lq, const double &uq);
/// Command related to ForceUtil
void setForceLimitsFromId(const double &jointId,
const dynamicgraph::Vector &lq,
const dynamicgraph::Vector &uq);
void setForceNameToForceId(const std::string& forceName,
const double & forceId);
/// Commands related to FootUtil
void setRightFootSoleXYZ(const dynamicgraph::Vector &);
void setRightFootForceSensorXYZ(const dynamicgraph::Vector &);
void setFootFrameName(const std::string &, const std::string &);
void setImuJointName(const std::string &);
void displayRobotUtil();
/// Set the mapping between urdf and sot.
void setJoints(const dynamicgraph::Vector &);
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("[ParameterServer-"+name+"] "+msg, t, file, line);
}
protected:
RobotUtil * m_robot_util;
dynamicgraph::sot::talos_balance::robots::RobotWrapper * m_robot;
bool m_initSucceeded; /// true if the entity has been successfully initialized
double m_dt; /// control loop time period
bool m_emergency_stop_triggered; /// true if an emergency condition as been triggered either by an other entity, or by control limit violation
bool m_is_first_iter; /// true at the first iteration, false otherwise
int m_iter;
double m_sleep_time; /// time to sleep at every iteration (to slow down simulation)
bool convertJointNameToJointId(const std::string& name, unsigned int& id);
bool isJointInRange(unsigned int id, double q);
void updateJointCtrlModesOutputSignal();
}; // class ParameterServer
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_control_manager_H__
/*
* 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_pose_quaternion_to_matrix_homo_H__
#define __sot_talos_balance_pose_quaternion_to_matrix_homo_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define POSEQUATERNIONTOMATRIXHOMO_EXPORT __declspec(dllexport)
#else
#define POSEQUATERNIONTOMATRIXHOMO_EXPORT __declspec(dllimport)
#endif
#else
#define POSEQUATERNIONTOMATRIXHOMO_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 POSEQUATERNIONTOMATRIXHOMO_EXPORT PoseQuaternionToMatrixHomo
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
PoseQuaternionToMatrixHomo(const std::string& name);
void init() {}
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(sin, ::dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(sout, MatrixHomogeneous);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
}; // class PoseQuaternionToMatrixHomo
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_pose_quaternion_to_matrix_homo_H__
This diff is collapsed.
/*
* 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_quat_to_euler_H__
#define __sot_talos_balance_quat_to_euler_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define QUATTOEULER_EXPORT __declspec(dllexport)
#else
#define QUATTOEULER_EXPORT __declspec(dllimport)
#endif
#else
#define QUATTOEULER_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 QUATTOEULER_EXPORT QuatToEuler : public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
QuatToEuler(const std::string& name);
void init() {}
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(quaternion, ::dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(euler, ::dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
}; // class QuatToEuler
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_quat_to_euler_H__
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.