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
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 2519 additions and 1653 deletions
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,167 +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 <dynamic-graph/signal-helper.h>
#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 <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;
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__
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 @ ca7bad3e
/*
* Copyright 2015, Andrea Del Prete, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_torque_control_parameter_server_H__
#define __sot_torque_control_parameter_server_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (__sot_torque_parameter_server_H__)
# define SOTParameterServer_EXPORT __declspec(dllexport)
# else
# define SOTParameterServer_EXPORT __declspec(dllimport)
# endif
#else
# define SOTParameterServer_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
#include <map>
#include "boost/assign.hpp"
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <sot/talos_balance/robot/robot-wrapper.hh>
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/// Number of time step to transition from one ctrl mode to another
#define CTRL_MODE_TRANSITION_TIME_STEP 1000.0
class SOTParameterServer_EXPORT ParameterServer
:public::dynamicgraph::Entity
{
typedef ParameterServer EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
ParameterServer( const std::string & name);
/// Initialize
/// @param dt: control interval
/// @param urdfFile: path to the URDF model of the robot
void init(const double & dt,
const std::string & urdfFile,
const std::string & robotRef);
/* --- SIGNALS --- */
/* --- COMMANDS --- */
/// Commands related to joint name and joint id
void setNameToId(const std::string& jointName, const unsigned int & jointId);
void setJointLimitsFromId(const unsigned int &jointId, const double &lq, const double &uq);
/// Command related to ForceUtil
void setForceLimitsFromId(const unsigned int & jointId, const dynamicgraph::Vector &lq, const dynamicgraph::Vector &uq);
void setForceNameToForceId(const std::string& forceName, const unsigned int & forceId);
/// Commands related to FootUtil
void setRightFootSoleXYZ(const dynamicgraph::Vector &);
void setRightFootForceSensorXYZ(const dynamicgraph::Vector &);
void setFootFrameName(const std::string &, const std::string &);
void setImuJointName(const std::string &);
void displayRobotUtil();
/// Set the mapping between urdf and sot.
void setJoints(const dynamicgraph::Vector &);
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
protected:
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/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/round-double-to-int.hh 0 → 100644
View file @ e217f893
This diff is collapsed.
include/sot/talos_balance/saturation.hh 0 → 100644
View file @ e217f893
This diff is collapsed.
include/sot/talos_balance/simple-admittance-controller.hh 0 → 100644
View file @ e217f893
This diff is collapsed.
include/sot/talos_balance/simple-controller-6d.hh 0 → 100644
View file @ e217f893
This diff is collapsed.
include/sot/talos_balance/simple-distribute-wrench.hh 0 → 100644
View file @ e217f893
This diff is collapsed.
include/sot/talos_balance/simple-pid.hh 0 → 100644
View file @ e217f893
This diff is collapsed.
include/sot/talos_balance/simple-pidd.hh 0 → 100644
View file @ e217f893
This diff is collapsed.
include/sot/talos_balance/simple-reference-frame.hh 0 → 100644
View file @ e217f893
This diff is collapsed.
include/sot/talos_balance/simple-state-integrator.hh 0 → 100644
View file @ e217f893
This diff is collapsed.