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 2371 additions and 959 deletions
include/sot/talos_balance/joint-position-controller.hh
View file @ e217f893
......@@ -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__
include/sot/talos_balance/madgwickahrs.hh deleted 100644 → 0
View file @ 22590e8b
//=====================================================================================================
//
// 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__
include/sot/talos_balance/math/fwd.hh
View file @ e217f893
......@@ -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__
include/sot/talos_balance/nd-trajectory-generator.hh
View file @ e217f893
/*
* Copyright 2017,Thomas Flayols, LAAS-CNRS
* File derived from sot-torque-control package available on
* File derived from sot-torque-control package available on
* https://github.com/stack-of-tasks/sot-torque-control
*
* This file is part of sot-talos-balance.
......@@ -23,173 +23,175 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (nd_position_controller_EXPORTS)
# define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllexport)
# else
# define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(nd_position_controller_EXPORTS)
#define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllexport)
#else
# define SOTNDTRAJECTORYGENERATOR_EXPORT
#define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllimport)
#endif
#else
#define SOTNDTRAJECTORYGENERATOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <parametric-curves/spline.hpp>
#include <map>
#include <parametric-curves/constant.hpp>
#include <parametric-curves/text-file.hpp>
#include <parametric-curves/minimum-jerk.hpp>
#include <parametric-curves/linear-chirp.hpp>
#include <parametric-curves/infinite-sinusoid.hpp>
#include <parametric-curves/infinite-const-acc.hpp>
#include <parametric-curves/infinite-sinusoid.hpp>
#include <parametric-curves/linear-chirp.hpp>
#include <parametric-curves/minimum-jerk.hpp>
#include <parametric-curves/spline.hpp>
#include <parametric-curves/text-file.hpp>
#include <sot/core/matrix-geometry.hh>
#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"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTNDTRAJECTORYGENERATOR_EXPORT NdTrajectoryGenerator
:public::dynamicgraph::Entity
{
typedef NdTrajectoryGenerator EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
NdTrajectoryGenerator( const std::string & name );
void init(const double& dt, const unsigned int& n);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(initial_value, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(trigger, bool);
DECLARE_SIGNAL(x, OUT, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
protected:
DECLARE_SIGNAL_OUT_FUNCTION(x, dynamicgraph::Vector);
public:
/* --- COMMANDS --- */
void playTrajectoryFile(const std::string& fileName);
void playSpline(const std::string& fileName);
void setSpline(const std::string& filename, const double& timeToInitConf);
void startSpline();
/** Print the current value of the specified component. */
void getValue(const int& id);
/** Move a component to a position with a minimum-jerk trajectory.
* @param id integer index.
* @param xFinal The desired final position of the component.
* @param time The time to go from the current position to xFinal [sec].
*/
void move(const int& id, const double& xFinal, const double& time);
/** Start an infinite sinusoidal trajectory.
* @param id integer index.
* @param xFinal The position of the component corresponding to the max amplitude of the sinusoid.
* @param time The time to go from the current position to xFinal [sec].
*/
void startSinusoid(const int& id, const double& xFinal, const double& time);
/** Start an infinite triangle trajectory.
* @param id integer index.
* @param xFinal The position of the component corresponding to the max amplitude of the trajectory.
* @param time The time to go from the current position to xFinal [sec].
*/
//void startTriangle(const int& id, const double& xFinal, const double& time, const double& Tacc);
/** Start an infinite trajectory with piece-wise constant acceleration.
* @param id integer index.
* @param xFinal The position of the component corresponding to the max amplitude of the trajectory.
* @param time The time to go from the current position to xFinal [sec].
* @param Tacc The time during witch acceleration is keept constant [sec].
*/
void startConstAcc(const int& id, const double& xFinal, const double& time);
/** Start a linear-chirp trajectory, that is a sinusoidal trajectory with frequency
* being a linear function of time.
* @param id integer index.
* @param xFinal The position of the component corresponding to the max amplitude of the sinusoid [rad].
* @param f0 The initial (min) frequency of the sinusoid [Hz]
* @param f1 The final (max) frequency of the sinusoid [Hz]
* @param time The time to get from f0 to f1 [sec]
*/
void startLinearChirp(const int& id, const double& xFinal, const double& f0, const double& f1, const double& time);
/** Stop the motion of the specified component. If id is -1
* it stops the trajectory of all the vector.
* @param id integer index.
* */
void stop(const int& id);
/* --- 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("[NdTrajectoryGenerator-"+name+"] "+msg, t, file, line);
}
protected:
enum JTG_Status
{
JTG_STOP,
JTG_SINUSOID,
JTG_MIN_JERK,
JTG_LIN_CHIRP,
//JTG_TRIANGLE,
JTG_CONST_ACC,
JTG_TEXT_FILE,
JTG_SPLINE
};
bool m_initSucceeded; /// true if the entity has been successfully initialized
bool m_firstIter; /// true if it is the first iteration, false otherwise
double m_dt; /// control loop time step.
double m_t; /// current control loop time.
unsigned int m_n; /// size of ouput vector
unsigned int m_iterLast; /// last iter index
bool m_splineReady; /// true if the spline has been successfully loaded.
std::vector<JTG_Status> m_status; /// status of the component
std::vector<parametriccurves::AbstractCurve<double, Eigen::Vector1d>* > m_currentTrajGen;
std::vector<parametriccurves::Constant<double, 1>* > m_noTrajGen;
std::vector<parametriccurves::MinimumJerk<double, 1>* > m_minJerkTrajGen;
std::vector<parametriccurves::InfiniteSinusoid<double,1>* > m_sinTrajGen;
std::vector<parametriccurves::LinearChirp<double,1>*> m_linChirpTrajGen;
//std::vector<parametriccurves::InfiniteTriangular<double,1>* > m_triangleTrajGen;
std::vector<parametriccurves::InfiniteConstAcc<double,1>* > m_constAccTrajGen;
parametriccurves::TextFile<double, Eigen::Dynamic>* m_textFileTrajGen;
parametriccurves::Spline<double, Eigen::Dynamic>* m_splineTrajGen;
}; // class NdTrajectoryGenerator
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_nd_trajectory_generator_H__
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTNDTRAJECTORYGENERATOR_EXPORT NdTrajectoryGenerator
: public ::dynamicgraph::Entity {
typedef NdTrajectoryGenerator EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
NdTrajectoryGenerator(const std::string& name);
void init(const double& dt, const unsigned int& n);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(initial_value, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(trigger, bool);
DECLARE_SIGNAL(x, OUT, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
protected:
DECLARE_SIGNAL_OUT_FUNCTION(x, dynamicgraph::Vector);
public:
/* --- COMMANDS --- */
void playTrajectoryFile(const std::string& fileName);
void playSpline(const std::string& fileName);
void setSpline(const std::string& filename, const double& timeToInitConf);
void startSpline();
/** Print the current value of the specified component. */
void getValue(const int& id);
/** Move a component to a position with a minimum-jerk trajectory.
* @param id integer index.
* @param xFinal The desired final position of the component.
* @param time The time to go from the current position to xFinal [sec].
*/
void move(const int& id, const double& xFinal, const double& time);
/** Instantaneously set a component to a given position.
* @param id integer index.
* @param xVal The desired position of the component.
*/
void set(const int& id, const double& xVal);
/** Start an infinite sinusoidal trajectory.
* @param id integer index.
* @param xFinal The position of the component corresponding to the max
* amplitude of the sinusoid.
* @param time The time to go from the current position to xFinal [sec].
*/
void startSinusoid(const int& id, const double& xFinal, const double& time);
/** Start an infinite triangle trajectory.
* @param id integer index.
* @param xFinal The position of the component corresponding to the max
* amplitude of the trajectory.
* @param time The time to go from the current position to xFinal [sec].
*/
// void startTriangle(const int& id, const double& xFinal, const double& time,
// const double& Tacc);
/** Start an infinite trajectory with piece-wise constant acceleration.
* @param id integer index.
* @param xFinal The position of the component corresponding to the max
* amplitude of the trajectory.
* @param time The time to go from the current position to xFinal [sec].
* @param Tacc The time during witch acceleration is keept constant [sec].
*/
void startConstAcc(const int& id, const double& xFinal, const double& time);
/** Start a linear-chirp trajectory, that is a sinusoidal trajectory with
* frequency being a linear function of time.
* @param id integer index.
* @param xFinal The position of the component corresponding to the max
* amplitude of the sinusoid [rad].
* @param f0 The initial (min) frequency of the sinusoid [Hz]
* @param f1 The final (max) frequency of the sinusoid [Hz]
* @param time The time to get from f0 to f1 [sec]
*/
void startLinearChirp(const int& id, const double& xFinal, const double& f0,
const double& f1, const double& time);
/** Stop the motion of the specified component. If id is -1
* it stops the trajectory of all the vector.
* @param id integer index.
* */
void stop(const int& id);
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
protected:
enum JTG_Status {
JTG_STOP,
JTG_SINUSOID,
JTG_MIN_JERK,
JTG_LIN_CHIRP,
// JTG_TRIANGLE,
JTG_CONST_ACC,
JTG_TEXT_FILE,
JTG_SPLINE
};
bool
m_initSucceeded; /// true if the entity has been successfully initialized
bool m_firstIter; /// true if it is the first iteration, false otherwise
double m_dt; /// control loop time step.
double m_t; /// current control loop time.
unsigned int m_n; /// size of ouput vector
unsigned int m_iterLast; /// last iter index
bool m_splineReady; /// true if the spline has been successfully loaded.
std::vector<JTG_Status> m_status; /// status of the component
std::vector<
parametriccurves::AbstractCurve<double, dynamicgraph::sot::Vector1d>*>
m_currentTrajGen;
std::vector<parametriccurves::Constant<double, 1>*> m_noTrajGen;
std::vector<parametriccurves::MinimumJerk<double, 1>*> m_minJerkTrajGen;
std::vector<parametriccurves::InfiniteSinusoid<double, 1>*> m_sinTrajGen;
std::vector<parametriccurves::LinearChirp<double, 1>*> m_linChirpTrajGen;
// std::vector<parametriccurves::InfiniteTriangular<double,1>* >
// m_triangleTrajGen;
std::vector<parametriccurves::InfiniteConstAcc<double, 1>*> m_constAccTrajGen;
parametriccurves::TextFile<double, Eigen::Dynamic>* m_textFileTrajGen;
parametriccurves::Spline<double, Eigen::Dynamic>* m_splineTrajGen;
}; // class NdTrajectoryGenerator
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_nd_trajectory_generator_H__
include/sot/talos_balance/parameter-server.hh deleted 100644 → 0
View file @ 22590e8b
/*
* 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__
include/sot/talos_balance/pose-quaternion-to-matrix-homo.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_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__
include/sot/talos_balance/qualisys-client.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_qualisys_client_H__
#define __sot_talos_balance_qualisys_client_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define QUALISYS_CLIENT_EXPORT __declspec(dllexport)
#else
#define QUALISYS_CLIENT_EXPORT __declspec(dllimport)
#endif
#else
#define QUALISYS_CLIENT_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinocchio first
#include <dynamic-graph/signal-helper.h>
#include <boost/chrono.hpp>
#include <boost/thread.hpp>
#include <map>
#include <sot/core/robot-utils.hh>
#include "boost/assign.hpp"
#include "sot/talos_balance/sdk_qualisys/Markup.h"
#include "sot/talos_balance/sdk_qualisys/Network.h"
#include "sot/talos_balance/sdk_qualisys/RTPacket.h"
#include "sot/talos_balance/sdk_qualisys/RTProtocol.h"
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class QUALISYS_CLIENT_EXPORT QualisysClient : public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
QualisysClient(const std::string& name);
void init();
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(dummy, double);
/* --- COMMANDS --- */
void registerRigidBody(const std::string& RBname);
void setMocapIPAdress(const std::string& ipAdress);
void getRigidBodyList(); /// If connected, return the list of all the rigid
/// bodies available.
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
protected:
bool m_initSucceeded; // true if the entity has been successfully initialized
bool m_printRigidBodyList = false;
std::vector<std::string>
m_RBnames; // vector of names of registered rigid bodies
std::vector<dg::Vector> m_RBpositions; // vector of rigid bodies positions
void manageNetworkFrame();
boost::thread m_thread{&QualisysClient::manageNetworkFrame, this};
boost::mutex m_mutex;
std::string m_serverAddr = "127.0.0.1";
dg::Vector& readGenericRigidBody(const int RBidx, dg::Vector& res,
const int& time);
}; // class QualisysClient
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_qualisys_client_H__
include/sot/talos_balance/quat-to-euler.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_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__
include/sot/talos_balance/robot-utils.hh deleted 100644 → 0
View file @ 22590e8b
/*
* Copyright 2017, 2019
* LAAS-CNRS
* A. Del Prete, T. Flayols, O. Stasse, F. Bailly
*
* This file is part of sot-core.
* See license file.
*/
#ifndef __sot_torque_control_common_H__
#define __sot_torque_control_common_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (hrp2_common_EXPORTS)
# define ROBOT_UTILS_COMMON_EXPORT __declspec(dllexport)
# else
# define ROBOT_UTILS_COMMON_EXPORT __declspec(dllimport)
# endif
#else
# define ROBOT_UTILS_COMMON_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <iostream>
#include <dynamic-graph/linear-algebra.h>
#include <sot/talos_balance/utils/signal-helper.hh>
#include <sot/talos_balance/utils/vector-conversions.hh>
#include <map>
#include "boost/assign.hpp"
#include <sot/talos_balance/utils/logger.hh>
namespace dg = ::dynamicgraph;
using namespace dg;
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
struct JointLimits
{
double upper;
double lower;
JointLimits():
upper(0.0),
lower(0.0)
{}
JointLimits(double l, double u):
upper(u),
lower(l)
{}
};
typedef Eigen::VectorXd::Index Index;
struct ForceLimits
{
Eigen::VectorXd upper;
Eigen::VectorXd lower;
ForceLimits():
upper(Eigen::Vector6d::Zero()),
lower(Eigen::Vector6d::Zero())
{}
ForceLimits(const Eigen::VectorXd& l, const Eigen::VectorXd& u):
upper(u),
lower(l)
{}
void display(std::ostream &os) const;
};
struct ForceUtil
{
std::map<Index,ForceLimits> m_force_id_to_limits;
std::map<std::string,Index> m_name_to_force_id;
std::map<Index,std::string> m_force_id_to_name;
Index m_Force_Id_Left_Hand, m_Force_Id_Right_Hand,
m_Force_Id_Left_Foot, m_Force_Id_Right_Foot;
void set_name_to_force_id(const std::string & name,
const Index &force_id);
void set_force_id_to_limits(const Index &force_id,
const dg::Vector &lf,
const dg::Vector &uf);
void create_force_id_to_name_map();
Index get_id_from_name(const std::string &name);
const std::string & get_name_from_id(Index idx);
std::string cp_get_name_from_id(Index idx);
const ForceLimits & get_limits_from_id(Index force_id);
ForceLimits cp_get_limits_from_id(Index force_id);
Index get_force_id_left_hand()
{
return m_Force_Id_Left_Hand;
}
void set_force_id_left_hand(Index anId)
{
m_Force_Id_Left_Hand = anId;
}
Index get_force_id_right_hand()
{
return m_Force_Id_Right_Hand;
}
void set_force_id_right_hand( Index anId)
{
m_Force_Id_Right_Hand = anId;
}
Index get_force_id_left_foot()
{
return m_Force_Id_Left_Foot;
}
void set_force_id_left_foot(Index anId)
{
m_Force_Id_Left_Foot = anId;
}
Index get_force_id_right_foot()
{
return m_Force_Id_Right_Foot;
}
void set_force_id_right_foot( Index anId)
{
m_Force_Id_Right_Foot = anId;
}
void display(std::ostream & out) const;
}; // struct ForceUtil
struct FootUtil
{
/// Position of the foot soles w.r.t. the frame of the foot
dynamicgraph::Vector m_Right_Foot_Sole_XYZ;
/// Position of the force/torque sensors w.r.t. the frame of the hosting link
dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ;
std::string m_Left_Foot_Frame_Name;
std::string m_Right_Foot_Frame_Name;
void display(std::ostream & os) const;
};
struct RobotUtil
{
public:
RobotUtil();
/// Forces data
ForceUtil m_force_util;
/// Foot information
FootUtil m_foot_util;
/// Map from the urdf index to the SoT index.
std::vector<Index> m_urdf_to_sot;
/// Nb of Dofs for the robot.
long unsigned int m_nbJoints;
/// Map from the name to the id.
std::map<std::string,Index> m_name_to_id;
/// The map between id and name
std::map<Index,std::string> m_id_to_name;
/// The joint limits map.
std::map<Index,JointLimits> m_limits_map;
/// The name of the joint IMU is attached to
std::string m_imu_joint_name;
/// This method creates the map between id and name.
/// It is called each time a new link between id and name is inserted
/// (i.e. when set_name_to_id is called).
void create_id_to_name_map();
/// URDF file path
std::string m_urdf_filename;
dynamicgraph::Vector m_dgv_urdf_to_sot;
/** Given a joint name it finds the associated joint id.
* If the specified joint name is not found it returns -1;
* @param name Name of the joint to find.
* @return The id of the specified joint, -1 if not found. */
const Index get_id_from_name(const std::string &name);
/** Given a joint id it finds the associated joint name.
* If the specified joint is not found it returns "Joint name not found";
* @param id Id of the joint to find.
* @return The name of the specified joint, "Joint name not found" if not found. */
/// Get the joint name from its index
const std::string & get_name_from_id(Index id);
/// Set relation between the name and the SoT id
void set_name_to_id(const std::string &jointName,
const Index & jointId);
/// Set the map between urdf index and sot index
void set_urdf_to_sot(const std::vector<Index> &urdf_to_sot);
void set_urdf_to_sot(const dg::Vector &urdf_to_sot);
/// Set the limits (lq,uq) for joint idx
void set_joint_limits_for_id(const Index &idx,
const double &lq,
const double &uq);
bool joints_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot);
bool joints_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
bool velocity_urdf_to_sot(Eigen::ConstRefVector q_urdf,
Eigen::ConstRefVector v_urdf, Eigen::RefVector v_sot);
bool velocity_sot_to_urdf(Eigen::ConstRefVector q_urdf,
Eigen::ConstRefVector v_sot, Eigen::RefVector v_urdf);
bool config_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot);
bool config_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
bool base_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot);
bool base_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
/** Given a joint id it finds the associated joint limits.
* If the specified joint is not found it returns JointLimits(0,0).
* @param id Id of the joint to find.
* @return The limits of the specified joint, JointLimits(0,0) if not found. */
const JointLimits & get_joint_limits_from_id(Index id);
JointLimits cp_get_joint_limits_from_id(Index id);
// void sendMsg(const std::string& msg,
// MsgType t=MSG_TYPE_INFO,
// const char* file="", int line=0)
// {
// getLogger().sendMsg("[FromURDFToSoT] "+msg, t, file, line);
// }
void display(std::ostream &os) const;
}; // struct RobotUtil
RobotUtil * RefVoidRobotUtil();
RobotUtil * getRobotUtil(std::string &robotName);
bool isNameInRobotUtil(std::string &robotName);
RobotUtil * createRobotUtil(std::string &robotName);
bool base_se3_to_sot(Eigen::ConstRefVector pos,
Eigen::ConstRefMatrix R,
Eigen::RefVector q_sot);
}
} // namespace sot
} // namespace dynamicgraph
#endif // sot_torque_control_common_h_
include/sot/talos_balance/robot/fwd.hh
View file @ e217f893
......@@ -17,17 +17,14 @@
#ifndef __invdyn_robots_fwd_hpp__
#define __invdyn_robots_fwd_hpp__
namespace dynamicgraph
{
namespace sot
{
namespace talos_balance
{
namespace robots
{
class RobotWrapper;
}
}
}
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
namespace talos_balance {
namespace robots {
class RobotWrapper;
}
#endif // ifndef __invdyn_robots_fwd_hpp__
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // ifndef __invdyn_robots_fwd_hpp__
include/sot/talos_balance/robot/robot-wrapper.hh
View file @ e217f893
......@@ -18,164 +18,140 @@
#ifndef __invdyn_robot_wrapper_hpp__
#define __invdyn_robot_wrapper_hpp__
#include "sot/talos_balance/math/fwd.hh"
#include "sot/talos_balance/robot/fwd.hh"
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/spatial/fwd.hpp>
#include <string>
#include <vector>
namespace dynamicgraph
{
namespace sot
{
namespace talos_balance
{
namespace robots
{
///
/// \brief Wrapper for a robot based on pinocchio
///
class RobotWrapper
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::Scalar Scalar;
typedef pinocchio::Model Model;
typedef pinocchio::Data Data;
typedef pinocchio::Motion Motion;
typedef pinocchio::Frame Frame;
typedef pinocchio::SE3 SE3;
typedef math::Vector Vector;
typedef math::Vector3 Vector3;
typedef math::Vector6 Vector6;
typedef math::Matrix Matrix;
typedef math::Matrix3x Matrix3x;
typedef math::RefVector RefVector;
typedef math::ConstRefVector ConstRefVector;
RobotWrapper(const std::string & filename,
const std::vector<std::string> & package_dirs,
bool verbose=false);
RobotWrapper(const std::string & filename,
const std::vector<std::string> & package_dirs,
const pinocchio::JointModelVariant & rootJoint,
bool verbose=false);
virtual int nq() const;
virtual int nv() const;
///
/// \brief Accessor to model.
///
/// \returns a const reference on the model.
///
const Model & model() const;
Model & model();
void computeAllTerms(Data & data, const Vector & q, const Vector & v) const;
const Vector & rotor_inertias() const;
const Vector & gear_ratios() const;
bool rotor_inertias(ConstRefVector rotor_inertias);
bool gear_ratios(ConstRefVector gear_ratios);
void com(const Data & data,
RefVector com_pos,
RefVector com_vel,
RefVector com_acc) const;
const Vector3 & com(const Data & data) const;
const Vector3 & com_vel(const Data & data) const;
const Vector3 & com_acc(const Data & data) const;
const Matrix3x & Jcom(const Data & data) const;
const Matrix & mass(const Data & data);
const Vector & nonLinearEffects(const Data & data) const;
const SE3 & position(const Data & data,
const Model::JointIndex index) const;
const Motion & velocity(const Data & data,
const Model::JointIndex index) const;
const Motion & acceleration(const Data & data,
const Model::JointIndex index) const;
void jacobianWorld(const Data & data,
const Model::JointIndex index,
Data::Matrix6x & J) const;
void jacobianLocal(const Data & data,
const Model::JointIndex index,
Data::Matrix6x & J) const;
SE3 framePosition(const Data & data,
const Model::FrameIndex index) const;
void framePosition(const Data & data,
const Model::FrameIndex index,
SE3 & framePosition) const;
Motion frameVelocity(const Data & data,
const Model::FrameIndex index) const;
void frameVelocity(const Data & data,
const Model::FrameIndex index,
Motion & frameVelocity) const;
Motion frameAcceleration(const Data & data,
const Model::FrameIndex index) const;
void frameAcceleration(const Data & data,
const Model::FrameIndex index,
Motion & frameAcceleration) const;
Motion frameClassicAcceleration(const Data & data,
const Model::FrameIndex index) const;
void frameClassicAcceleration(const Data & data,
const Model::FrameIndex index,
Motion & frameAcceleration) const;
void frameJacobianWorld(const Data & data,
const Model::FrameIndex index,
Data::Matrix6x & J) const;
void frameJacobianLocal(const Data & data,
const Model::FrameIndex index,
Data::Matrix6x & J) const;
protected:
void updateMd();
/// \brief Robot model.
Model m_model;
std::string m_model_filename;
bool m_verbose;
Vector m_rotor_inertias;
Vector m_gear_ratios;
Vector m_Md; /// diagonal part of inertia matrix due to rotor inertias
Matrix m_M; /// inertia matrix including rotor inertias
};
} // namespace robots
} // namespace talos_balance
}
}
#endif // ifndef __invdyn_robot_wrapper_hpp__
#include "sot/talos_balance/math/fwd.hh"
#include "sot/talos_balance/robot/fwd.hh"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
namespace robots {
///
/// \brief Wrapper for a robot based on pinocchio
///
class RobotWrapper {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::Scalar Scalar;
typedef pinocchio::Model Model;
typedef pinocchio::Data Data;
typedef pinocchio::Motion Motion;
typedef pinocchio::Frame Frame;
typedef pinocchio::SE3 SE3;
typedef math::Vector Vector;
typedef math::Vector3 Vector3;
typedef math::Vector6 Vector6;
typedef math::Matrix Matrix;
typedef math::Matrix3x Matrix3x;
typedef math::RefVector RefVector;
typedef math::ConstRefVector ConstRefVector;
RobotWrapper(const std::string& filename,
const std::vector<std::string>& package_dirs,
bool verbose = false);
RobotWrapper(const std::string& filename,
const std::vector<std::string>& package_dirs,
const pinocchio::JointModelVariant& rootJoint,
bool verbose = false);
virtual int nq() const;
virtual int nv() const;
///
/// \brief Accessor to model.
///
/// \returns a const reference on the model.
///
const Model& model() const;
Model& model();
void computeAllTerms(Data& data, const Vector& q, const Vector& v) const;
const Vector& rotor_inertias() const;
const Vector& gear_ratios() const;
bool rotor_inertias(ConstRefVector rotor_inertias);
bool gear_ratios(ConstRefVector gear_ratios);
void com(const Data& data, RefVector com_pos, RefVector com_vel,
RefVector com_acc) const;
const Vector3& com(const Data& data) const;
const Vector3& com_vel(const Data& data) const;
const Vector3& com_acc(const Data& data) const;
const Matrix3x& Jcom(const Data& data) const;
const Matrix& mass(const Data& data);
const Vector& nonLinearEffects(const Data& data) const;
const SE3& position(const Data& data, const Model::JointIndex index) const;
const Motion& velocity(const Data& data, const Model::JointIndex index) const;
const Motion& acceleration(const Data& data,
const Model::JointIndex index) const;
void jacobianWorld(const Data& data, const Model::JointIndex index,
Data::Matrix6x& J) const;
void jacobianLocal(const Data& data, const Model::JointIndex index,
Data::Matrix6x& J) const;
SE3 framePosition(const Data& data, const Model::FrameIndex index) const;
void framePosition(const Data& data, const Model::FrameIndex index,
SE3& framePosition) const;
Motion frameVelocity(const Data& data, const Model::FrameIndex index) const;
void frameVelocity(const Data& data, const Model::FrameIndex index,
Motion& frameVelocity) const;
Motion frameAcceleration(const Data& data,
const Model::FrameIndex index) const;
void frameAcceleration(const Data& data, const Model::FrameIndex index,
Motion& frameAcceleration) const;
Motion frameClassicAcceleration(const Data& data,
const Model::FrameIndex index) const;
void frameClassicAcceleration(const Data& data, const Model::FrameIndex index,
Motion& frameAcceleration) const;
void frameJacobianWorld(Data& data, const Model::FrameIndex index,
Data::Matrix6x& J) const;
void frameJacobianLocal(Data& data, const Model::FrameIndex index,
Data::Matrix6x& J) const;
protected:
void updateMd();
/// \brief Robot model.
Model m_model;
std::string m_model_filename;
bool m_verbose;
Vector m_rotor_inertias;
Vector m_gear_ratios;
Vector m_Md; /// diagonal part of inertia matrix due to rotor inertias
Matrix m_M; /// inertia matrix including rotor inertias
};
} // namespace robots
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // ifndef __invdyn_robot_wrapper_hpp__
include/sot/talos_balance/round-double-to-int.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_round_double_to_int_H__
#define __sot_talos_balance_round_double_to_int_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define ROUND_DOUBLE_TO_INT_EXPORT __declspec(dllexport)
#else
#define ROUND_DOUBLE_TO_INT_EXPORT __declspec(dllimport)
#endif
#else
#define ROUND_DOUBLE_TO_INT_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class ROUND_DOUBLE_TO_INT_EXPORT RoundDoubleToInt
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
RoundDoubleToInt(const std::string& name);
void init() {}
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(sin, double);
DECLARE_SIGNAL_OUT(sout, int);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
}; // class RoundDoubleToInt
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_round_double_to_int_H__
include/sot/talos_balance/saturation.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_saturation_H__
#define __sot_talos_balance_saturation_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(saturation_EXPORTS)
#define SATURATION_EXPORT __declspec(dllexport)
#else
#define SATURATION_EXPORT __declspec(dllimport)
#endif
#else
#define SATURATION_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SATURATION_EXPORT Saturation : public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
Saturation(const std::string &name);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(x, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(y, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(k, double);
DECLARE_SIGNAL_IN(xLim, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(yLim, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(yOut, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
}; // class Saturation
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_saturation_H__
include/sot/talos_balance/sdk_qualisys/Markup.h 0 → 100644
View file @ e217f893
#ifndef QUALISYS_NBC_NBC_MARKUP_H_INCLUDED
#define QUALISYS_NBC_NBC_MARKUP_H_INCLUDED
// Markup.h: interface for the NBC_CMarkup class.
//
// NBC_CMarkup Release 6.5 Lite
// Copyright (C) 1999-2003 First Objective Software, Inc. All rights reserved
// This entire notice must be retained in this source code
// Redistributing this source code requires written permission
// This software is provided "as is", with no warranty.
// Latest fixes enhancements and documentation at www.firstobject.com
#include <string>
#include <vector>
#ifdef _DEBUG
#define _DS(i) (i ? &((const char*)m_csDoc.c_str())[m_aPos[i].nStartL] : 0)
#define MARKUP_SETDEBUGSTATE \
m_pMainDS = _DS(m_iPos); \
m_pChildDS = _DS(m_iPosChild)
#else
#define MARKUP_SETDEBUGSTATE
#endif
class CMarkup {
public:
CMarkup() {
SetDoc(NULL);
mnIndent = 4;
};
CMarkup(const char* szDoc) { SetDoc(szDoc); };
CMarkup(const CMarkup& markup) { *this = markup; };
void operator=(const CMarkup& markup);
virtual ~CMarkup(){};
// Settings
void SetIndent(int nIndent = 4);
// Create
std::string GetDoc() const { return m_csDoc; };
bool AddElem(const char* szName, const char* szData = NULL) {
return x_AddElem(szName, szData, false, false);
};
bool AddChildElem(const char* szName, const char* szData = NULL) {
return x_AddElem(szName, szData, false, true);
};
bool AddAttrib(const char* szAttrib, const char* szValue) {
return x_SetAttrib(m_iPos, szAttrib, szValue);
};
bool AddChildAttrib(const char* szAttrib, const char* szValue) {
return x_SetAttrib(m_iPosChild, szAttrib, szValue);
};
bool SetAttrib(const char* szAttrib, const char* szValue) {
return x_SetAttrib(m_iPos, szAttrib, szValue);
};
bool SetChildAttrib(const char* szAttrib, const char* szValue) {
return x_SetAttrib(m_iPosChild, szAttrib, szValue);
};
// Navigate
bool SetDoc(const char* szDoc);
bool IsWellFormed();
bool FindElem(const char* szName = NULL);
bool FindChildElem(const char* szName = NULL);
bool IntoElem();
bool OutOfElem();
void ResetChildPos() { x_SetPos(m_iPosParent, m_iPos, 0); };
void ResetMainPos() { x_SetPos(m_iPosParent, 0, 0); };
void ResetPos() { x_SetPos(0, 0, 0); };
std::string GetTagName() const;
std::string GetChildTagName() const { return x_GetTagName(m_iPosChild); };
std::string GetData() const { return x_GetData(m_iPos); };
std::string GetChildData() const { return x_GetData(m_iPosChild); };
std::string GetAttrib(const char* szAttrib) const {
return x_GetAttrib(m_iPos, szAttrib);
};
std::string GetChildAttrib(const char* szAttrib) const {
return x_GetAttrib(m_iPosChild, szAttrib);
};
std::string GetError() const { return m_csError; };
static std::string Format(const char* fmt, ...);
enum MarkupNodeType {
MNT_ELEMENT = 1, // 0x01
MNT_TEXT = 2, // 0x02
MNT_WHITESPACE = 4, // 0x04
MNT_CDATA_SECTION = 8, // 0x08
MNT_PROCESSING_INSTRUCTION = 16, // 0x10
MNT_COMMENT = 32, // 0x20
MNT_DOCUMENT_TYPE = 64, // 0x40
MNT_EXCLUDE_WHITESPACE = 123, // 0x7b
};
protected:
#ifdef _DEBUG
const char* m_pMainDS;
const char* m_pChildDS;
#endif
std::string m_csDoc;
std::string m_csError;
struct ElemPos {
ElemPos() { Clear(); };
ElemPos(const ElemPos& pos) { *this = pos; };
bool IsEmptyElement() const { return (nStartR == nEndL + 1); };
void Clear() {
nStartL = 0;
nStartR = 0;
nEndL = 0;
nEndR = 0;
nReserved = 0;
iElemParent = 0;
iElemChild = 0;
iElemNext = 0;
};
void AdjustStart(int n) {
nStartL += n;
nStartR += n;
};
void AdjustEnd(int n) {
nEndL += n;
nEndR += n;
};
int nStartL;
int nStartR;
int nEndL;
int nEndR;
int nReserved;
int iElemParent;
int iElemChild;
int iElemNext;
};
std::vector<ElemPos> m_aPos;
int m_iPosParent;
int m_iPos;
int m_iPosChild;
int m_iPosFree;
int m_nNodeType;
struct TokenPos {
TokenPos(const char* sz) {
Clear();
szDoc = sz;
};
bool IsValid() const { return (nL <= nR); };
void Clear() {
nL = 0;
nR = -1;
nNext = 0;
bIsString = false;
};
bool Match(const char* szName) const;
int nL;
int nR;
int nNext;
const char* szDoc;
bool bIsString;
};
void x_SetPos(int iPosParent, int iPos, int iPosChild) {
m_iPosParent = iPosParent;
m_iPos = iPos;
m_iPosChild = iPosChild;
m_nNodeType = iPos ? MNT_ELEMENT : 0;
MARKUP_SETDEBUGSTATE;
};
int x_GetFreePos();
int x_ReleasePos();
int x_ParseElem(int iPos);
int x_ParseError(const char* szError, const char* szName = NULL);
static bool x_FindChar(const char* szDoc, int& nChar, char c);
static bool x_FindAny(const char* szDoc, int& nChar);
static bool x_FindToken(TokenPos& token);
std::string x_GetToken(const TokenPos& token) const;
int x_FindElem(int iPosParent, int iPos, const char* szPath);
std::string x_GetTagName(int iPos) const;
std::string x_GetData(int iPos) const;
std::string x_GetAttrib(int iPos, const char* szAttrib) const;
bool x_AddElem(const char* szName, const char* szValue, bool bInsert,
bool bAddChild);
bool x_FindAttrib(TokenPos& token, const char* szAttrib = NULL) const;
bool x_SetAttrib(int iPos, const char* szAttrib, const char* szValue);
void x_LocateNew(int iPosParent, int& iPosRel, int& nOffset, int nLength,
int nFlags);
int x_ParseNode(TokenPos& token);
void x_DocChange(int nLeft, int nReplace, const std::string& csInsert);
void x_Adjust(int iPos, int nShift, bool bAfterPos = false);
std::string x_TextToDoc(const char* szText, bool bAttrib = false) const;
std::string x_TextFromDoc(int nLeft, int nRight) const;
protected:
char mtIndent[1000];
int mnIndent;
private:
std::string Mid(const std::string& tStr, int nFirst) const;
std::string Mid(const std::string& tStr, int nFirst, int nCount) const;
char* GetBuffer(std::string& tStr, int nMinLen = -1) const;
void ReleaseBuffer(std::string& tStr, int nNewLen = -1) const;
};
#endif /* QUALISYS_NBC_NBC_MARKUP_H_INCLUDED */
include/sot/talos_balance/sdk_qualisys/Network.h 0 → 100644
View file @ e217f893
#ifndef NETWORK_H
#define NETWORK_H
#ifdef _WIN32
#define WIN32_LEAN_AND_MEAN
#include <winsock2.h>
#else
#define INVALID_SOCKET -1
#define SOCKET int
#endif
class CNetwork {
public:
CNetwork();
~CNetwork();
bool Connect(const char* pServerAddr, unsigned short nPort);
void Disconnect();
bool Connected() const;
bool CreateUDPSocket(unsigned short& nUDPPort, bool bBroadcast = false);
int Receive(char* rtDataBuff, int nDataBufSize, bool bHeader, int nTimeout,
unsigned int* ipAddr = nullptr);
bool Send(const char* pSendBuf, int nSize);
bool SendUDPBroadcast(const char* pSendBuf, int nSize, short nPort,
unsigned int nFilterAddr = 0);
char* GetErrorString();
int GetError() const;
bool IsLocalAddress(unsigned int nAddr) const;
unsigned short GetUdpServerPort();
unsigned short GetUdpBroadcastServerPort();
private:
bool InitWinsock();
void SetErrorString();
unsigned short GetUdpServerPort(SOCKET nSocket);
private:
SOCKET mSocket;
SOCKET mUDPSocket;
SOCKET mUDPBroadcastSocket;
char mErrorStr[256];
unsigned long mLastError;
};
#endif
include/sot/talos_balance/sdk_qualisys/RTPacket.h 0 → 100644
View file @ e217f893
#ifndef RTPACKET_H
#define RTPACKET_H
#include <stdint.h>
#include <stdio.h>
#ifdef EXPORT_DLL
#define DLL_EXPORT __declspec(dllexport)
#else
#define DLL_EXPORT
#endif
#define MAJOR_VERSION 1
#define MINOR_VERSION 19
#define MAX_CAMERA_COUNT 256
#define MAX_ANALOG_DEVICE_COUNT 64
#define MAX_FORCE_PLATE_COUNT 64
#define MAX_GAZE_VECTOR_COUNT 64
#define MAX_TIMECODE_COUNT 3
#define MAX_SKELETON_COUNT 10
class DLL_EXPORT CRTPacket {
public:
enum EPacketType {
PacketError = 0,
PacketCommand = 1,
PacketXML = 2,
PacketData = 3,
PacketNoMoreData = 4,
PacketC3DFile = 5,
PacketEvent = 6,
PacketDiscover = 7,
PacketQTMFile = 8,
PacketNone = 9
};
enum EComponentType {
Component3d = 1,
Component3dNoLabels = 2,
ComponentAnalog = 3,
ComponentForce = 4,
Component6d = 5,
Component6dEuler = 6,
Component2d = 7,
Component2dLin = 8,
Component3dRes = 9,
Component3dNoLabelsRes = 10,
Component6dRes = 11,
Component6dEulerRes = 12,
ComponentAnalogSingle = 13,
ComponentImage = 14,
ComponentForceSingle = 15,
ComponentGazeVector = 16,
ComponentTimecode = 17,
ComponentSkeleton = 18,
ComponentNone = 19
};
enum EImageFormat {
FormatRawGrayscale = 0,
FormatRawBGR = 1,
FormatJPG = 2,
FormatPNG = 3
};
enum EEvent {
EventConnected = 1,
EventConnectionClosed = 2,
EventCaptureStarted = 3,
EventCaptureStopped = 4,
EventCaptureFetchingFinished = 5, // Not used in version 1.10 and later
EventCalibrationStarted = 6,
EventCalibrationStopped = 7,
EventRTfromFileStarted = 8,
EventRTfromFileStopped = 9,
EventWaitingForTrigger = 10,
EventCameraSettingsChanged = 11,
EventQTMShuttingDown = 12,
EventCaptureSaved = 13,
EventReprocessingStarted = 14,
EventReprocessingStopped = 15,
EventTrigger = 16,
EventNone = 17 // Must be the last. Not actually an event. Just used to
// cont number of events.
};
enum ETimecodeType {
TimecodeSMPTE = 0,
TimecodeIRIG = 1,
TimecodeCamerTime = 2
};
struct SForce {
float fForceX;
float fForceY;
float fForceZ;
float fMomentX;
float fMomentY;
float fMomentZ;
float fApplicationPointX;
float fApplicationPointY;
float fApplicationPointZ;
};
struct SGazeVector {
float fX;
float fY;
float fZ;
float fPosX;
float fPosY;
float fPosZ;
};
struct SSkeletonSegment {
unsigned int id;
float positionX;
float positionY;
float positionZ;
float rotationX;
float rotationY;
float rotationZ;
float rotationW;
};
public:
CRTPacket(int nMajorVersion = MAJOR_VERSION,
int nMinorVersion = MINOR_VERSION, bool bBigEndian = false);
void GetVersion(unsigned int &nMajorVersion, unsigned int &nMinorVersion);
void SetVersion(unsigned int nMajorVersion, unsigned int nMinorVersion);
bool GetEndianness();
void SetEndianness(bool bBigEndian);
void ClearData();
void SetData(char *ptr);
void GetData(char *&ptr, unsigned int &nSize);
unsigned int GetSize();
EPacketType GetType();
unsigned long long GetTimeStamp();
unsigned int GetFrameNumber();
static unsigned int GetSize(char *pData, bool bBigEndian = false);
static EPacketType GetType(char *pData, bool bBigEndian = false);
static unsigned long long GetTimeStamp(char *pData, bool bBigEndian = false);
static unsigned int GetFrameNumber(char *pData, bool bBigEndian = false);
unsigned int GetComponentCount();
unsigned int GetComponentSize(EComponentType eComponent);
char *GetErrorString();
char *GetCommandString();
static char *GetCommandString(char *pData, bool bBigEndian = false);
char *GetXMLString();
bool GetEvent(EEvent &eEvent);
static bool GetEvent(EEvent &eEvent, char *pData, bool bBigEndian = false);
short GetDiscoverResponseBasePort();
static short GetDiscoverResponseBasePort(char *pData,
bool bBigEndian = false);
unsigned short GetDropRate();
unsigned short GetOutOfSyncRate();
unsigned int Get2DCameraCount();
unsigned int Get2DMarkerCount(unsigned int nCameraIndex);
unsigned char Get2DStatusFlags(unsigned int nCameraIndex);
bool Get2DMarker(unsigned int nCameraIndex, unsigned int nMarkerIndex,
unsigned int &nX, unsigned int &nY,
unsigned short &nXDiameter, unsigned short &nYDiameter);
unsigned int Get2DLinCameraCount();
unsigned int Get2DLinMarkerCount(unsigned int nCameraIndex);
unsigned char Get2DLinStatusFlags(unsigned int nCameraIndex);
bool Get2DLinMarker(unsigned int nCameraIndex, unsigned int nMarkerIndex,
unsigned int &nX, unsigned int &nY,
unsigned short &nXDiameter, unsigned short &nYDiameter);
unsigned int Get3DMarkerCount();
bool Get3DMarker(unsigned int nMarkerIndex, float &fX, float &fY, float &fZ);
unsigned int Get3DResidualMarkerCount();
bool Get3DResidualMarker(unsigned int nMarkerIndex, float &fX, float &fY,
float &fZ, float &fResidual);
unsigned int Get3DNoLabelsMarkerCount();
bool Get3DNoLabelsMarker(unsigned int nMarkerIndex, float &fX, float &fY,
float &fZ, unsigned int &nId);
unsigned int Get3DNoLabelsResidualMarkerCount();
bool Get3DNoLabelsResidualMarker(unsigned int nMarkerIndex, float &fX,
float &fY, float &fZ, unsigned int &nId,
float &fResidual);
unsigned int Get6DOFBodyCount();
bool Get6DOFBody(unsigned int nBodyIndex, float &fX, float &fY, float &fZ,
float afRotMatrix[9]);
unsigned int Get6DOFResidualBodyCount();
bool Get6DOFResidualBody(unsigned int nBodyIndex, float &fX, float &fY,
float &fZ, float afRotMatrix[9], float &fResidual);
unsigned int Get6DOFEulerBodyCount();
bool Get6DOFEulerBody(unsigned int nBodyIndex, float &fX, float &fY,
float &fZ, float &fAng1, float &fAng2, float &fAng3);
unsigned int Get6DOFEulerResidualBodyCount();
bool Get6DOFEulerResidualBody(unsigned int nBodyIndex, float &fX, float &fY,
float &fZ, float &fAng1, float &fAng2,
float &fAng3, float &fResidual);
unsigned int GetGazeVectorCount();
unsigned int GetGazeVectorSampleCount(unsigned int nVectorIndex);
unsigned int GetGazeVectorSampleNumber(
unsigned int nVectorIndex); // Returns 0 if no sample was found.
bool GetGazeVector(unsigned int nVectorIndex, unsigned int nSampleIndex,
SGazeVector &nGazeVector);
bool GetGazeVector(unsigned int nVectorIndex, SGazeVector *pGazeVectorBuf,
unsigned int nBufSize);
unsigned int GetTimecodeCount();
bool GetTimecodeType(unsigned int nTimecodeIndex,
CRTPacket::ETimecodeType &timecodeType);
bool GetTimecodeSMPTE(unsigned int nTimecodeIndex, int &hours, int &minutes,
int &seconds, int &frame);
bool GetTimecodeIRIG(unsigned int nTimecodeIndex, int &year, int &day,
int &hours, int &minutes, int &seconds, int &tenths);
bool GetTimecodeCameraTime(unsigned int nTimecodeIndex,
unsigned long long &cameraTime);
unsigned int GetImageCameraCount();
unsigned int GetImageCameraId(unsigned int nCameraIndex);
bool GetImageFormat(unsigned int nCameraIndex, EImageFormat &eImageFormat);
bool GetImageSize(unsigned int nCameraIndex, unsigned int &nWidth,
unsigned int &nHeight);
bool GetImageCrop(unsigned int nCameraIndex, float &fCropLeft,
float &fCropTop, float &fCropRight, float &fCropBottom);
unsigned int GetImageSize(unsigned int nCameraIndex);
unsigned int GetImage(unsigned int nCameraIndex, char *pDataBuf,
unsigned int nBufSize);
unsigned int GetAnalogDeviceCount();
unsigned int GetAnalogDeviceId(unsigned int nDeviceIndex);
unsigned int GetAnalogChannelCount(unsigned int nDeviceIndex);
unsigned int GetAnalogSampleCount(unsigned int nDeviceIndex);
unsigned int GetAnalogSampleNumber(
unsigned int nDeviceIndex); // Returns 0 if no sample was found.
unsigned int GetAnalogData(unsigned int nDeviceIndex, float *pDataBuf,
unsigned int nBufSize);
unsigned int GetAnalogData(unsigned int nDeviceIndex,
unsigned int nChannelIndex, float *pDataBuf,
unsigned int nBufSize);
bool GetAnalogData(unsigned int nDeviceIndex, unsigned int nChannelIndex,
unsigned int nSampleIndex, float &fAnalogValue);
unsigned int GetAnalogSingleDeviceCount();
unsigned int GetAnalogSingleDeviceId(unsigned int nDeviceIndex);
unsigned int GetAnalogSingleChannelCount(unsigned int nDeviceIndex);
unsigned int GetAnalogSingleData(unsigned int nDeviceIndex, float *pDataBuf,
unsigned int nBufSize);
bool GetAnalogSingleData(unsigned int nDeviceIndex,
unsigned int nChannelIndex, float &fValue);
unsigned int GetForcePlateCount();
unsigned int GetForcePlateId(unsigned int nPlateIndex);
unsigned int GetForceCount(unsigned int nPlateIndex);
unsigned int GetForceNumber(
unsigned int nPlateIndex); // Returns 0 if no force was found.
unsigned int GetForceData(unsigned int nPlateIndex, SForce *pForceBuf,
unsigned int nBufSize);
bool GetForceData(unsigned int nPlateIndex, unsigned int nForceIndex,
SForce &sForce);
unsigned int GetForceSinglePlateCount();
unsigned int GetForceSinglePlateId(unsigned int nPlateIndex);
bool GetForceSingleData(unsigned int nPlateIndex, SForce &pForce);
unsigned int GetSkeletonCount();
unsigned int GetSkeletonSegmentCount(unsigned int nSkeletonIndex);
bool GetSkeletonSegments(unsigned int nSkeletonIndex,
SSkeletonSegment *segmentBuf, unsigned int nBufSize);
bool GetSkeletonSegment(unsigned int nSkeletonIndex, unsigned segmentIndex,
SSkeletonSegment &segment);
private:
float SetByteOrder(float *pfData);
double SetByteOrder(double *pfData);
short SetByteOrder(short *pnData);
unsigned short SetByteOrder(unsigned short *pnData);
long SetByteOrder(long *pnData);
int SetByteOrder(int *pnData);
unsigned int SetByteOrder(unsigned int *pnData);
long long SetByteOrder(long long *pnData);
unsigned long long SetByteOrder(unsigned long long *pnData);
private:
char *mpData;
char *mpComponentData[ComponentNone];
char *mp2DData[MAX_CAMERA_COUNT];
char *mp2DLinData[MAX_CAMERA_COUNT];
char *mpImageData[MAX_CAMERA_COUNT];
char *mpAnalogData[MAX_ANALOG_DEVICE_COUNT];
char *mpAnalogSingleData[MAX_ANALOG_DEVICE_COUNT];
char *mpForceData[MAX_FORCE_PLATE_COUNT];
char *mpForceSingleData[MAX_FORCE_PLATE_COUNT];
char *mpGazeVectorData[MAX_GAZE_VECTOR_COUNT];
char *mpTimecodeData[MAX_TIMECODE_COUNT];
char *mpSkeletonData[MAX_SKELETON_COUNT];
unsigned int mnComponentCount;
EComponentType meComponentType;
unsigned int mn2DCameraCount;
unsigned int mn2DLinCameraCount;
unsigned int mnImageCameraCount;
unsigned int mnAnalogDeviceCount;
unsigned int mnAnalogSingleDeviceCount;
unsigned int mnForcePlateCount;
unsigned int mnForceSinglePlateCount;
unsigned int mnGazeVectorCount;
unsigned int mnTimecodeCount;
unsigned int mSkeletonCount;
int mnMajorVersion;
int mnMinorVersion;
bool mbBigEndian;
}; // RTPacket
#endif // RTPACKET_H
include/sot/talos_balance/sdk_qualisys/RTProtocol.h 0 → 100644
View file @ e217f893
#ifndef RTPROTOCOL_H
#define RTPROTOCOL_H
#include <map>
#include <string>
#include <vector>
#include "Network.h"
#include "RTPacket.h"
#ifdef _WIN32
#pragma warning(disable : 4251)
#endif
#ifdef EXPORT_DLL
#define DLL_EXPORT __declspec(dllexport)
#else
#define DLL_EXPORT
#endif
#define DEFAULT_AUTO_DESCOVER_PORT 22226
#define WAIT_FOR_DATA_TIMEOUT 5000000 // 5 s
class CMarkup;
class DLL_EXPORT CRTProtocol {
public:
static const unsigned int cComponent3d = 0x000001;
static const unsigned int cComponent3dNoLabels = 0x000002;
static const unsigned int cComponentAnalog = 0x000004;
static const unsigned int cComponentForce = 0x000008;
static const unsigned int cComponent6d = 0x000010;
static const unsigned int cComponent6dEuler = 0x000020;
static const unsigned int cComponent2d = 0x000040;
static const unsigned int cComponent2dLin = 0x000080;
static const unsigned int cComponent3dRes = 0x000100;
static const unsigned int cComponent3dNoLabelsRes = 0x000200;
static const unsigned int cComponent6dRes = 0x000400;
static const unsigned int cComponent6dEulerRes = 0x000800;
static const unsigned int cComponentAnalogSingle = 0x001000;
static const unsigned int cComponentImage = 0x002000;
static const unsigned int cComponentForceSingle = 0x004000;
static const unsigned int cComponentGazeVector = 0x008000;
static const unsigned int cComponentTimecode = 0x010000;
static const unsigned int cComponentSkeleton = 0x020000;
struct SComponentOptions {
SComponentOptions()
: mAnalogChannels(nullptr), mSkeletonGlobalData(false) {}
char* mAnalogChannels;
bool mSkeletonGlobalData;
};
enum EStreamRate {
RateNone = 0,
RateAllFrames = 1,
RateFrequency = 2,
RateFrequencyDivisor = 3
};
enum ECameraModel {
ModelMacReflex = 0,
ModelProReflex120 = 1,
ModelProReflex240 = 2,
ModelProReflex500 = 3,
ModelProReflex1000 = 4,
ModelOqus100 = 5,
ModelOqus300 = 6,
ModelOqus300Plus = 7,
ModelOqus400 = 8,
ModelOqus500 = 9,
ModelOqus200C = 10,
ModelOqus500Plus = 11,
ModelOqus700 = 12,
ModelOqus700Plus = 13,
ModelOqus600Plus = 14,
ModelMiqusM1 = 15,
ModelMiqusM3 = 16,
ModelMiqusM5 = 17,
ModelMiqusSyncUnit = 18,
ModelMiqusVideo = 19,
ModelMiqusVideoColor = 20
};
enum ECameraSystemType { Unknown = 0, Oqus = 1, Miqus = 2 };
enum ECameraMode { ModeMarker = 0, ModeMarkerIntensity = 1, ModeVideo = 2 };
enum EVideoResolution {
VideoResolution1080p = 0,
VideoResolution720p = 1,
VideoResolution540p = 2,
VideoResolution480p = 3,
VideoResolutionNone = 4
};
enum EVideoAspectRatio {
VideoAspectRatio16x9 = 0,
VideoAspectRatio4x3 = 1,
VideoAspectRatio1x1 = 2,
VideoAspectRatioNone = 3
};
enum ESyncOutFreqMode {
ModeShutterOut = 1, // A pulse per frame is sent
ModeMultiplier,
ModeDivisor,
ModeActualFreq,
ModeMeasurementTime,
ModeFixed100Hz
};
enum ESignalSource {
SourceControlPort = 0,
SourceIRReceiver = 1,
SourceSMPTE = 2,
SourceVideoSync = 3,
SourceIRIG = 4
};
enum EAxis { XPos = 0, XNeg = 1, YPos = 2, YNeg = 3, ZPos = 4, ZNeg = 5 };
enum EProcessingActions {
ProcessingNone = 0x0000,
ProcessingTracking2D = 0x0001,
ProcessingTracking3D = 0x0002,
ProcessingTwinSystemMerge = 0x0004,
ProcessingSplineFill = 0x0008,
ProcessingAIM = 0x0010,
Processing6DOFTracking = 0x0020,
ProcessingForceData = 0x0040,
ProcessingGazeVector = 0x0080,
ProcessingExportTSV = 0x0100,
ProcessingExportC3D = 0x0200,
ProcessingExportMatlabFile = 0x0800,
ProcessingExportAviFile = 0x1000,
ProcessingPreProcess2D = 0x2000
};
struct SPoint {
float fX;
float fY;
float fZ;
};
struct SDiscoverResponse {
char message[128];
unsigned int nAddr;
unsigned short nBasePort;
};
private:
struct SSettingsGeneralCamera {
unsigned int nID;
ECameraModel eModel;
bool bUnderwater;
bool bSupportsHwSync;
unsigned int nSerial;
ECameraMode eMode;
EVideoResolution eVideoResolution;
EVideoAspectRatio eVideoAspectRatio;
unsigned int nVideoFrequency;
unsigned int nVideoExposure; // Micro seconds
unsigned int nVideoExposureMin; // Micro seconds
unsigned int nVideoExposureMax; // Micro seconds
unsigned int nVideoFlashTime; // Micro seconds
unsigned int nVideoFlashTimeMin; // Micro seconds
unsigned int nVideoFlashTimeMax; // Micro seconds
unsigned int nMarkerExposure; // Micro seconds
unsigned int nMarkerExposureMin; // Micro seconds
unsigned int nMarkerExposureMax; // Micro seconds
unsigned int nMarkerThreshold;
unsigned int nMarkerThresholdMin;
unsigned int nMarkerThresholdMax;
float fPositionX;
float fPositionY;
float fPositionZ;
float fPositionRotMatrix[3][3];
unsigned int nOrientation; // Degrees
unsigned int nMarkerResolutionWidth; // Sub pixels
unsigned int nMarkerResolutionHeight; // Sub pixels
unsigned int nVideoResolutionWidth; // Pixels
unsigned int nVideoResolutionHeight; // Pixels
unsigned int nMarkerFOVLeft; // Pixels
unsigned int nMarkerFOVTop; // Pixels
unsigned int nMarkerFOVRight; // Pixels
unsigned int nMarkerFOVBottom; // Pixels
unsigned int nVideoFOVLeft; // Pixels
unsigned int nVideoFOVTop; // Pixels
unsigned int nVideoFOVRight; // Pixels
unsigned int nVideoFOVBottom; // Pixels
ESyncOutFreqMode eSyncOutMode[2];
unsigned int nSyncOutValue[2];
float fSyncOutDutyCycle[2]; // Percent
bool bSyncOutNegativePolarity[3];
float fFocus;
float fAperture;
bool autoExposureEnabled;
float autoExposureCompensation;
int autoWhiteBalance;
};
struct SSettingsGeneralCameraSystem {
ECameraSystemType eType;
};
struct SSettingsGeneralExternalTimebase {
SSettingsGeneralExternalTimebase()
: bEnabled(false),
eSignalSource(SourceControlPort),
bSignalModePeriodic(false),
nFreqMultiplier(0),
nFreqDivisor(0),
nFreqTolerance(0),
fNominalFrequency(0.0f),
bNegativeEdge(false),
nSignalShutterDelay(0),
fNonPeriodicTimeout(0.0f) {}
bool bEnabled;
ESignalSource eSignalSource;
bool bSignalModePeriodic;
unsigned int nFreqMultiplier;
unsigned int nFreqDivisor;
unsigned int nFreqTolerance;
float fNominalFrequency;
bool bNegativeEdge;
unsigned int nSignalShutterDelay;
float fNonPeriodicTimeout;
};
struct SSettingsGeneral {
SSettingsGeneral()
: nCaptureFrequency(0),
fCaptureTime(0.0f),
bStartOnExternalTrigger(false),
bStartOnTrigNO(false),
bStartOnTrigNC(false),
bStartOnTrigSoftware(false),
eProcessingActions(EProcessingActions::ProcessingNone),
eRtProcessingActions(EProcessingActions::ProcessingNone),
eReprocessingActions(EProcessingActions::ProcessingNone) {
sExternalTimebase = {};
sCameraSystem = {Unknown};
}
unsigned int nCaptureFrequency;
float fCaptureTime;
bool bStartOnExternalTrigger;
bool bStartOnTrigNO;
bool bStartOnTrigNC;
bool bStartOnTrigSoftware;
SSettingsGeneralCameraSystem sCameraSystem;
SSettingsGeneralExternalTimebase sExternalTimebase;
EProcessingActions eProcessingActions; // Binary flags.
EProcessingActions eRtProcessingActions; // Binary flags.
EProcessingActions eReprocessingActions; // Binary flags.
std::vector<SSettingsGeneralCamera> vsCameras;
};
struct SSettings3DLabel {
std::string oName;
unsigned int nRGBColor;
};
struct SSettingsBone {
std::string fromName;
std::string toName;
unsigned int color;
};
struct SSettings3D {
EAxis eAxisUpwards;
char pCalibrationTime[32];
std::vector<SSettings3DLabel> s3DLabels;
std::vector<SSettingsBone> sBones;
};
struct SSettings6DOFBody {
std::string oName;
unsigned int nRGBColor;
std::vector<SPoint> vsPoints;
};
struct SSettings6DOF {
std::vector<SSettings6DOFBody> bodySettings;
std::string eulerFirst;
std::string eulerSecond;
std::string eulerThird;
};
struct SGazeVector {
std::string name;
float frequency;
};
struct SAnalogDevice {
unsigned int nDeviceID;
unsigned int nChannels;
std::string oName;
std::vector<std::string> voLabels;
unsigned int nFrequency;
std::string oUnit;
float fMinRange;
float fMaxRange;
std::vector<std::string> voUnits;
};
struct SForceChannel {
unsigned int nChannelNumber;
float fConversionFactor;
};
struct SForcePlate {
unsigned int nID;
unsigned int nAnalogDeviceID;
std::string oType;
std::string oName;
unsigned int nFrequency;
float fLength;
float fWidth;
SPoint asCorner[4];
SPoint sOrigin;
std::vector<SForceChannel> vChannels;
bool bValidCalibrationMatrix;
float afCalibrationMatrix[12][12];
unsigned int nCalibrationMatrixRows;
unsigned int nCalibrationMatrixColumns;
};
struct SSettingsForce {
std::string oUnitLength;
std::string oUnitForce;
std::vector<SForcePlate> vsForcePlates;
};
struct SImageCamera {
unsigned int nID;
bool bEnabled;
CRTPacket::EImageFormat eFormat;
unsigned int nWidth;
unsigned int nHeight;
float fCropLeft;
float fCropTop;
float fCropRight;
float fCropBottom;
};
public:
struct SSettingsSkeletonSegment : CRTPacket::SSkeletonSegment {
std::string name;
int parentId;
int parentIndex;
};
struct SSettingsSkeleton {
std::string name;
std::vector<SSettingsSkeletonSegment> segments;
};
public:
CRTProtocol();
~CRTProtocol();
bool Connect(const char* pServerAddr, unsigned short nPort,
unsigned short* pnUDPServerPort = nullptr,
int nMajorVersion = MAJOR_VERSION,
int nMinorVersion = MINOR_VERSION, bool bBigEndian = false);
unsigned short GetUdpServerPort();
void Disconnect();
bool Connected() const;
bool SetVersion(int nMajorVersion, int nMinorVersion);
bool GetVersion(unsigned int& nMajorVersion, unsigned int& nMinorVersion);
bool GetQTMVersion(char* pVersion, unsigned int nVersionLen);
bool GetByteOrder(bool& bBigEndian);
bool CheckLicense(const char* pLicenseCode);
bool DiscoverRTServer(
unsigned short nServerPort, bool bNoLocalResponses,
unsigned short nDiscoverPort = DEFAULT_AUTO_DESCOVER_PORT);
int GetNumberOfDiscoverResponses();
bool GetDiscoverResponse(unsigned int nIndex, unsigned int& nAddr,
unsigned short& nBasePort, std::string& message);
bool GetCurrentFrame(unsigned int nComponentType,
const SComponentOptions& componentOptions = {});
bool StreamFrames(EStreamRate eRate, unsigned int nRateArg,
unsigned short nUDPPort, const char* pUDPAddr,
unsigned int nComponentType,
const SComponentOptions& componentOptions = {});
bool StreamFramesStop();
bool GetState(CRTPacket::EEvent& eEvent, bool bUpdate = true,
int nTimeout = WAIT_FOR_DATA_TIMEOUT);
bool GetCapture(const char* pFileName, bool bC3D);
bool SendTrig();
bool SetQTMEvent(const char* pLabel);
bool TakeControl(const char* pPassword = nullptr);
bool ReleaseControl();
bool IsControlling();
bool NewMeasurement();
bool CloseMeasurement();
bool StartCapture();
bool StartRTOnFile();
bool StopCapture();
bool LoadCapture(const char* pFileName);
bool SaveCapture(const char* pFileName, bool bOverwrite,
char* pNewFileName = nullptr, int nSizeOfNewFileName = 0);
bool LoadProject(const char* pFileName);
bool Reprocess();
static bool GetEventString(CRTPacket::EEvent eEvent, char* pStr);
static bool ConvertRateString(const char* pRate, EStreamRate& eRate,
unsigned int& nRateArg);
static unsigned int ConvertComponentString(const char* pComponentType);
static bool GetComponentString(
char* pComponentStr, unsigned int nComponentType,
const SComponentOptions& options = SComponentOptions());
int ReceiveRTPacket(
CRTPacket::EPacketType& eType, bool bSkipEvents = true,
int nTimeout = WAIT_FOR_DATA_TIMEOUT); // nTimeout < 0 : Blocking receive
CRTPacket* GetRTPacket();
bool ReadXmlBool(CMarkup* xml, const std::string& element, bool& value) const;
bool ReadCameraSystemSettings();
bool Read3DSettings(bool& bDataAvailable);
bool Read6DOFSettings(bool& bDataAvailable);
bool ReadGazeVectorSettings(bool& bDataAvailable);
bool ReadAnalogSettings(bool& bDataAvailable);
bool ReadForceSettings(bool& bDataAvailable);
bool ReadImageSettings(bool& bDataAvailable);
bool ReadSkeletonSettings(bool& bDataAvailable,
bool skeletonGlobalData = false);
void GetSystemSettings(unsigned int& nCaptureFrequency, float& fCaptureTime,
bool& bStartOnExtTrig, bool& trigNO, bool& trigNC,
bool& trigSoftware,
EProcessingActions& eProcessingActions,
EProcessingActions& eRtProcessingActions,
EProcessingActions& eReprocessingActions) const;
void GetExtTimeBaseSettings(bool& bEnabled, ESignalSource& eSignalSource,
bool& bSignalModePeriodic,
unsigned int& nFreqMultiplier,
unsigned int& nFreqDivisor,
unsigned int& nFreqTolerance,
float& fNominalFrequency, bool& bNegativeEdge,
unsigned int& nSignalShutterDelay,
float& fNonPeriodicTimeout) const;
unsigned int GetCameraCount() const;
bool GetCameraSettings(unsigned int nCameraIndex, unsigned int& nID,
ECameraModel& eModel, bool& bUnderwater,
bool& bSupportsHwSync, unsigned int& nSerial,
ECameraMode& eMode) const;
bool GetCameraMarkerSettings(unsigned int nCameraIndex,
unsigned int& nCurrentExposure,
unsigned int& nMinExposure,
unsigned int& nMaxExposure,
unsigned int& nCurrentThreshold,
unsigned int& nMinThreshold,
unsigned int& nMaxThreshold) const;
bool GetCameraVideoSettings(
unsigned int nCameraIndex, EVideoResolution& eVideoResolution,
EVideoAspectRatio& eVideoAspectRatio, unsigned int& nVideoFrequency,
unsigned int& nCurrentExposure, unsigned int& nMinExposure,
unsigned int& nMaxExposure, unsigned int& nCurrentFlashTime,
unsigned int& nMinFlashTime, unsigned int& nMaxFlashTime) const;
bool GetCameraSyncOutSettings(unsigned int nCameraIndex,
unsigned int portNumber,
ESyncOutFreqMode& eSyncOutMode,
unsigned int& nSyncOutValue,
float& fSyncOutDutyCycle,
bool& bSyncOutNegativePolarity) const;
bool GetCameraPosition(unsigned int nCameraIndex, SPoint& sPoint,
float fvRotationMatrix[3][3]) const;
bool GetCameraOrientation(unsigned int nCameraIndex, int& nOrientation) const;
bool GetCameraResolution(unsigned int nCameraIndex,
unsigned int& nMarkerWidth,
unsigned int& nMarkerHeight,
unsigned int& nVideoWidth,
unsigned int& nVideoHeight) const;
bool GetCameraFOV(unsigned int nCameraIndex, unsigned int& nMarkerLeft,
unsigned int& nMarkerTop, unsigned int& nMarkerRight,
unsigned int& nMarkerBottom, unsigned int& nVideoLeft,
unsigned int& nVideoTop, unsigned int& nVideoRight,
unsigned int& nVideoBottom) const;
bool GetCameraLensControlSettings(const unsigned int nCameraIndex,
float* focus, float* aperture) const;
bool GetCameraAutoExposureSettings(const unsigned int nCameraIndex,
bool* autoExposureEnabled,
float* autoExposureCompensation) const;
bool GetCameraAutoWhiteBalance(const unsigned int nCameraIndex,
bool* autoWhiteBalanceEnabled) const;
EAxis Get3DUpwardAxis() const;
const char* Get3DCalibrated() const;
unsigned int Get3DLabeledMarkerCount() const;
const char* Get3DLabelName(unsigned int nMarkerIndex) const;
unsigned int Get3DLabelColor(unsigned int nMarkerIndex) const;
unsigned int Get3DBoneCount() const;
const char* Get3DBoneFromName(unsigned int boneIndex) const;
const char* Get3DBoneToName(unsigned int boneIndex) const;
void Get6DOFEulerNames(std::string& first, std::string& second,
std::string& third) const;
unsigned int Get6DOFBodyCount() const;
const char* Get6DOFBodyName(unsigned int nBodyIndex) const;
unsigned int Get6DOFBodyColor(unsigned int nBodyIndex) const;
unsigned int Get6DOFBodyPointCount(unsigned int nBodyIndex) const;
bool Get6DOFBodyPoint(unsigned int nBodyIndex, unsigned int nMarkerIndex,
SPoint& sPoint) const;
unsigned int GetGazeVectorCount() const;
const char* GetGazeVectorName(unsigned int nGazeVectorIndex) const;
float GetGazeVectorFrequency(unsigned int nGazeVectorIndex) const;
unsigned int GetAnalogDeviceCount() const;
bool GetAnalogDevice(unsigned int nDeviceIndex, unsigned int& nDeviceID,
unsigned int& nChannels, char*& pName,
unsigned int& nFrequency, char*& pUnit, float& fMinRange,
float& fMaxRange) const;
const char* GetAnalogLabel(unsigned int nDeviceIndex,
unsigned int nChannelIndex) const;
const char* GetAnalogUnit(unsigned int nDeviceIndex,
unsigned int nChannelIndex) const;
void GetForceUnits(char*& pLength, char*& pForce) const;
unsigned int GetForcePlateCount() const;
bool GetForcePlate(unsigned int nPlateIndex, unsigned int& nID,
unsigned int& nAnalogDeviceID, unsigned int& nFrequency,
char*& pType, char*& pName, float& fLength,
float& fWidth) const;
bool GetForcePlateLocation(unsigned int nPlateIndex, SPoint sCorner[4]) const;
bool GetForcePlateOrigin(unsigned int nPlateIndex, SPoint& sOrigin) const;
unsigned int GetForcePlateChannelCount(unsigned int nPlateIndex) const;
bool GetForcePlateChannel(unsigned int nPlateIndex,
unsigned int nChannelIndex,
unsigned int& nChannelNumber,
float& fConversionFactor) const;
bool GetForcePlateCalibrationMatrix(unsigned int nPlateIndex,
float fvCalMatrix[12][12],
unsigned int* rows,
unsigned int* columns) const;
unsigned int GetImageCameraCount() const;
bool GetImageCamera(unsigned int nCameraIndex, unsigned int& nCameraID,
bool& bEnabled, CRTPacket::EImageFormat& eFormat,
unsigned int& nWidth, unsigned int& nHeight,
float& fCropLeft, float& fCropTop, float& fCropRight,
float& fCropBottom) const;
unsigned int GetSkeletonCount() const;
const char* GetSkeletonName(unsigned int skeletonIndex);
unsigned int GetSkeletonSegmentCount(unsigned int skeletonIndex);
bool GetSkeleton(unsigned int skeletonIndex, SSettingsSkeleton* skeleton);
bool GetSkeletonSegment(
unsigned int skeletonIndex, unsigned int segmentIndex,
SSettingsSkeletonSegment* segment); // parentIndex = -1 => No parent.
ECameraSystemType GetCameraSystemType() const;
bool SetSystemSettings(const unsigned int* pnCaptureFrequency,
const float* pfCaptureTime,
const bool* pbStartOnExtTrig, const bool* trigNO,
const bool* trigNC, const bool* trigSoftware,
const EProcessingActions* peProcessingActions,
const EProcessingActions* peRtProcessingActions,
const EProcessingActions* peReprocessingActions);
bool SetExtTimeBaseSettings(
const bool* pbEnabled, const ESignalSource* peSignalSource,
const bool* pbSignalModePeriodic, const unsigned int* pnFreqMultiplier,
const unsigned int* pnFreqDivisor, const unsigned int* pnFreqTolerance,
const float* pfNominalFrequency, const bool* pbNegativeEdge,
const unsigned int* pnSignalShutterDelay,
const float* pfNonPeriodicTimeout);
bool SetCameraSettings(const unsigned int nCameraID,
const ECameraMode* peMode,
const float* pfMarkerExposure,
const float* pfMarkerThreshold,
const int* pnOrientation);
bool SetCameraVideoSettings(const unsigned int nCameraID,
const EVideoResolution* eVideoResolution,
const EVideoAspectRatio* eVideoAspectRatio,
const unsigned int* pnVideoFrequency,
const float* pfVideoExposure,
const float* pfVideoFlashTime);
bool SetCameraSyncOutSettings(const unsigned int nCameraID,
const unsigned int portNumber,
const ESyncOutFreqMode* peSyncOutMode,
const unsigned int* pnSyncOutValue,
const float* pfSyncOutDutyCycle,
const bool* pbSyncOutNegativePolarity);
bool SetCameraLensControlSettings(const unsigned int nCameraID,
const float focus, const float aperture);
bool SetCameraAutoExposureSettings(const unsigned int nCameraID,
const bool autoExposure,
const float compensation);
bool SetCameraAutoWhiteBalance(const unsigned int nCameraID,
const bool enable);
bool SetImageSettings(const unsigned int nCameraID, const bool* pbEnable,
const CRTPacket::EImageFormat* peFormat,
const unsigned int* pnWidth,
const unsigned int* pnHeight, const float* pfLeftCrop,
const float* pfTopCrop, const float* pfRightCrop,
const float* pfBottomCrop);
bool SetForceSettings(const unsigned int nPlateID, const SPoint* psCorner1,
const SPoint* psCorner2, const SPoint* psCorner3,
const SPoint* psCorner4);
char* GetErrorString();
private:
bool SendString(const char* pCmdStr, int nType);
bool SendCommand(const char* pCmdStr);
bool SendCommand(const char* pCmdStr, char* pCommandResponseStr,
unsigned int timeout = WAIT_FOR_DATA_TIMEOUT);
bool SendXML(const char* pCmdStr);
void AddXMLElementBool(CMarkup* oXML, const char* tTag, const bool* pbValue,
const char* tTrue = "True",
const char* tFalse = "False");
void AddXMLElementBool(CMarkup* oXML, const char* tTag, const bool bValue,
const char* tTrue = "True",
const char* tFalse = "False");
void AddXMLElementInt(CMarkup* oXML, const char* tTag, const int* pnValue);
void AddXMLElementUnsignedInt(CMarkup* oXML, const char* tTag,
const unsigned int* pnValue);
void AddXMLElementFloat(CMarkup* oXML, const char* tTag, const float* pfValue,
unsigned int pnDecimals = 6);
bool CompareNoCase(std::string tStr1, const char* tStr2) const;
private:
CNetwork* mpoNetwork;
CRTPacket* mpoRTPacket;
char* maDataBuff;
unsigned int mDataBuffSize;
CRTPacket::EEvent meLastEvent;
CRTPacket::EEvent
meState; // Same as meLastEvent but without EventCameraSettingsChanged
int mnMinorVersion;
int mnMajorVersion;
bool mbBigEndian;
bool mbIsMaster;
SSettingsGeneral msGeneralSettings;
SSettings3D ms3DSettings;
SSettings6DOF mvs6DOFSettings;
std::vector<SGazeVector> mvsGazeVectorSettings;
std::vector<SAnalogDevice> mvsAnalogDeviceSettings;
SSettingsForce msForceSettings;
std::vector<SImageCamera> mvsImageSettings;
std::vector<SSettingsSkeleton> mSkeletonSettings;
char maErrorStr[1024];
unsigned short mnBroadcastPort;
FILE* mpFileBuffer;
std::vector<SDiscoverResponse> mvsDiscoverResponseList;
};
#endif // RTPROTOCOL_H
include/sot/talos_balance/simple-admittance-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_admittance_controller_H__
#define __sot_talos_balance_admittance_controller_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(admittance_controller_EXPORTS)
#define SIMPLEADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
#else
#define SIMPLEADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define SIMPLEADMITTANCECONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SIMPLEADMITTANCECONTROLLER_EXPORT SimpleAdmittanceController
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
SimpleAdmittanceController(const std::string& name);
void init(const double& dt, const unsigned& n);
void setPosition(const dynamicgraph::Vector& position);
/* --- 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_OUT(dqRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(qRef, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
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;
}; // class AdmittanceController
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_admittance_controller_H__
include/sot/talos_balance/simple-controller-6d.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_simple_controller_6d_H__
#define __sot_talos_balance_simple_controller_6d_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(simple_controller_6d_EXPORTS)
#define SIMPLE_CONTROLLER_6D_EXPORT __declspec(dllexport)
#else
#define SIMPLE_CONTROLLER_6D_EXPORT __declspec(dllimport)
#endif
#else
#define SIMPLE_CONTROLLER_6D_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 SIMPLE_CONTROLLER_6D_EXPORT SimpleController6d
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
SimpleController6d(const std::string& name);
void init();
template <typename Derived>
Eigen::Matrix3d skew(const Eigen::MatrixBase<Derived>& v);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(x, MatrixHomogeneous);
DECLARE_SIGNAL_IN(x_des, MatrixHomogeneous);
DECLARE_SIGNAL_IN(v_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(v_ref, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
}; // class SimpleController6d
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_simple_controller_6d_H__
include/sot/talos_balance/simple-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_simple_distribute_wrench_H__
#define __sot_talos_balance_simple_distribute_wrench_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define SIMPLE_DISTRIBUTE_WRENCH_EXPORT __declspec(dllexport)
#else
#define SIMPLE_DISTRIBUTE_WRENCH_EXPORT __declspec(dllimport)
#endif
#else
#define SIMPLE_DISTRIBUTE_WRENCH_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinocchio first
#include <dynamic-graph/signal-helper.h>
#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 SIMPLE_DISTRIBUTE_WRENCH_EXPORT SimpleDistributeWrench
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
SimpleDistributeWrench(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_INNER(kinematics_computations, int);
DECLARE_SIGNAL_INNER(wrenches, 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;
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;
void distributeWrench(const Eigen::VectorXd& wrenchDes, const double rho);
void saturateWrench(const Eigen::VectorXd& wrenchDes, const int phase);
bool m_emergency_stop_triggered;
}; // class SimpleDistributeWrench
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_simple_distribute_wrench_H__