From 67eccfcc6dcc9224c8d3888672a12f8624bfa8c4 Mon Sep 17 00:00:00 2001
From: flforget <florent.forget1@gmail.com>
Date: Thu, 6 Dec 2018 11:09:07 +0100
Subject: [PATCH] [cpp] add joint_trajectory_generator and nd_trajectory
generator entities entites are derived from sot-torque-control package
addition to previous commit
---
CMakeLists.txt | 10 +-
.../joint-trajectory-generator.hh | 221 +++++
.../talos_balance/nd-trajectory-generator.hh | 196 ++++
include/sot/talos_balance/stc-commands.hh | 64 ++
include/sot/talos_balance/utils/common.hh | 292 ++++++
.../utils/trajectory-generators.hh | 570 +++++++++++
src/CMakeLists.txt | 2 +
src/joint-trajectory-generator.cpp | 884 ++++++++++++++++++
src/nd-trajectory-generator.cpp | 643 +++++++++++++
src/utils/common.cpp | 548 +++++++++++
src/utils/trajectory-generators.cpp | 38 +
unittest/python/appli_jointTrajGen.py | 13 +
unittest/python/test_jointTrajGen.py | 58 ++
13 files changed, 3538 insertions(+), 1 deletion(-)
create mode 100644 include/sot/talos_balance/joint-trajectory-generator.hh
create mode 100644 include/sot/talos_balance/nd-trajectory-generator.hh
create mode 100644 include/sot/talos_balance/stc-commands.hh
create mode 100644 include/sot/talos_balance/utils/common.hh
create mode 100644 include/sot/talos_balance/utils/trajectory-generators.hh
create mode 100644 src/joint-trajectory-generator.cpp
create mode 100644 src/nd-trajectory-generator.cpp
create mode 100644 src/utils/common.cpp
create mode 100644 src/utils/trajectory-generators.cpp
create mode 100644 unittest/python/appli_jointTrajGen.py
create mode 100644 unittest/python/test_jointTrajGen.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5c48d61..eb46c51 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -78,16 +78,20 @@ SEARCH_FOR_EIGEN()
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("pinocchio >= 1.2")
+ADD_REQUIRED_DEPENDENCY("parametric-curves")
SET(SOTTALOSBALANCE_LIB_NAME ${PROJECT_NAME})
SET(LIBRARY_NAME ${SOTTALOSBALANCE_LIB_NAME})
SET(${LIBRARY_NAME}_HEADERS
# include/sot/talos_balance/example.hh
+ include/sot/talos_balance/stc-commands.hh
include/sot/talos_balance/utils/commands-helper.hh
+ include/sot/talos_balance/utils/common.hh
include/sot/talos_balance/utils/signal-helper.hh
include/sot/talos_balance/utils/logger.hh
include/sot/talos_balance/utils/stop-watch.hh
+ include/sot/talos_balance/utils/trajectory-generators.hh
include/sot/talos_balance/utils/vector-conversions.hh
)
@@ -97,6 +101,8 @@ SET(${LIBRARY_NAME}_HEADERS
SET(${LIBRARY_NAME}_SOURCES ${${LIBRARY_NAME}_HEADERS}
# src/example.cpp
+ src/utils/common.cpp
+ src/utils/trajectory-generators.cpp
src/utils/logger.cpp
src/utils/stop-watch.cpp
)
@@ -112,6 +118,8 @@ SET_TARGET_PROPERTIES(${LIBRARY_NAME}
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} sot-core)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} pinocchio)
+PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} parametric-curves)
+
IF(BUILD_PYTHON_INTERFACE)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph-python)
@@ -157,7 +165,7 @@ MACRO(DYNAMIC_GRAPH_CUSTOM_PYTHON_MODULE MODULENAME SUBMODULENAME LIBRARYNAME TA
list(GET extra_macro_args 1 SOURCE_PYTHON_MODULE)
endif(${num_extra_args} GREATER 1)
endif(${num_extra_args} GREATER 0)
-
+
IF(NOT DEFINED PYTHONLIBS_FOUND)
FINDPYTHON()
ELSEIF(NOT ${PYTHONLIBS_FOUND} STREQUAL "TRUE")
diff --git a/include/sot/talos_balance/joint-trajectory-generator.hh b/include/sot/talos_balance/joint-trajectory-generator.hh
new file mode 100644
index 0000000..0957407
--- /dev/null
+++ b/include/sot/talos_balance/joint-trajectory-generator.hh
@@ -0,0 +1,221 @@
+/*
+ * Copyright 2014, Oscar E. Ramos Ponce, LAAS-CNRS
+ * 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.
+ * 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-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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_talos_balance_joint_trajectory_generator_H__
+#define __sot_talos_balance_joint_trajectory_generator_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (joint_position_controller_EXPORTS)
+# define SOTJOINTTRAJECTORYGENERATOR_EXPORT __declspec(dllexport)
+# else
+# define SOTJOINTTRAJECTORYGENERATOR_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTJOINTTRAJECTORYGENERATOR_EXPORT
+#endif
+
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <sot/talos_balance/utils/common.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 <sot/talos_balance/utils/trajectory-generators.hh>
+#include <map>
+#include "boost/assign.hpp"
+
+
+namespace dynamicgraph {
+ namespace sot {
+ namespace talos_balance {
+
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+ class SOTJOINTTRAJECTORYGENERATOR_EXPORT JointTrajectoryGenerator
+ :public::dynamicgraph::Entity
+ {
+ typedef JointTrajectoryGenerator EntityClassName;
+ DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+ /* --- CONSTRUCTOR ---- */
+ JointTrajectoryGenerator( const std::string & name );
+
+ void init(const double& dt, const std::string &robotRef);
+
+ /* --- SIGNALS --- */
+ DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
+ DECLARE_SIGNAL_OUT(q, dynamicgraph::Vector);
+ DECLARE_SIGNAL_OUT(dq, dynamicgraph::Vector);
+ DECLARE_SIGNAL_OUT(ddq, dynamicgraph::Vector);
+ DECLARE_SIGNAL(fRightFoot, OUT, dynamicgraph::Vector);
+ DECLARE_SIGNAL(fLeftFoot, OUT, dynamicgraph::Vector);
+ DECLARE_SIGNAL(fRightHand, OUT, dynamicgraph::Vector);
+ DECLARE_SIGNAL(fLeftHand, OUT, dynamicgraph::Vector);
+
+ protected:
+ DECLARE_SIGNAL_OUT_FUNCTION(fRightFoot, dynamicgraph::Vector);
+ DECLARE_SIGNAL_OUT_FUNCTION(fLeftFoot, dynamicgraph::Vector);
+ DECLARE_SIGNAL_OUT_FUNCTION(fRightHand, dynamicgraph::Vector);
+ DECLARE_SIGNAL_OUT_FUNCTION(fLeftHand, dynamicgraph::Vector);
+
+ public:
+
+ /* --- COMMANDS --- */
+
+ void playTrajectoryFile(const std::string& fileName);
+
+ /** Print the current angle of the specified joint. */
+ void getJoint(const std::string& jointName);
+
+ /** Returns whether all given trajectories have ended **/
+ bool isTrajectoryEnded();
+
+ /** Move a joint to a position with a minimum-jerk trajectory.
+ * @param jointName The short name of the joint.
+ * @param qFinal The desired final position of the joint [rad].
+ * @param time The time to go from the current position to qFinal [sec].
+ */
+ void moveJoint(const std::string& jointName, const double& qFinal, const double& time);
+ void moveForce(const std::string& forceName, const int& axis, const double& fFinal, const double& time);
+
+ /** Start an infinite sinusoidal trajectory.
+ * @param jointName The short name of the joint.
+ * @param qFinal The position of the joint corresponding to the max amplitude of the sinusoid [rad].
+ * @param time The time to go from the current position to qFinal [sec].
+ */
+ void startSinusoid(const std::string& jointName, const double& qFinal, const double& time);
+
+ /** Start an infinite triangle trajectory.
+ * @param jointName The short name of the joint.
+ * @param qFinal The position of the joint corresponding to the max amplitude of the trajectory [rad].
+ * @param time The time to go from the current position to qFinal [sec].
+ */
+ void startTriangle(const std::string& jointName, const double& qFinal, const double& time, const double& Tacc);
+
+ /** Start an infinite trajectory with piece-wise constant acceleration.
+ * @param jointName The short name of the joint.
+ * @param qFinal The position of the joint corresponding to the max amplitude of the trajectory [rad].
+ * @param time The time to go from the current position to qFinal [sec].
+ * @param Tacc The time during witch acceleration is keept constant [sec].
+ */
+ void startConstAcc(const std::string& jointName, const double& qFinal, const double& time);
+
+ /** Start an infinite sinusoidal trajectory for the specified force signal.
+ * @param forceName The short name of the force signal (rh, lh, rf, lf).
+ * @param fFinal The 6d force corresponding to the max amplitude of the sinusoid [N/Nm].
+ * @param time The time to go from 0 to fFinal [sec].
+ */
+// void startForceSinusoid(const std::string& forceName, const dynamicgraph::Vector& fFinal, const double& time);
+ void startForceSinusoid(const std::string& forceName, const int& axis, const double& fFinal, const double& time);
+
+ /** Start a linear-chirp trajectory, that is a sinusoidal trajectory with frequency
+ * being a linear function of time.
+ * @param jointName The short name of the joint.
+ * @param qFinal The position of the joint 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 std::string& jointName, const double& qFinal, const double& f0, const double& f1, const double& time);
+
+ void startForceLinearChirp(const std::string& forceName, const int& axis, const double& fFinal, const double& f0, const double& f1, const double& time);
+
+ /** Stop the motion of the specified joint. If jointName is "all"
+ * it stops the motion of all joints.
+ * @param jointName A string identifying the joint to stop.
+ * */
+ void stop(const std::string& jointName);
+
+ /** Stop the trajectory of the specified force. If forceName is "all"
+ * it stops all forces.
+ * @param forceName A string identifying the force to stop.
+ * */
+ void stopForce(const std::string& forceName);
+
+ /* --- 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("[JointTrajectoryGenerator-"+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
+ };
+
+ 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 period
+
+ RobotUtil *m_robot_util;
+
+ std::vector<int> m_iterForceSignals;
+
+ std::vector<JTG_Status> m_status; /// status of the joints
+ std::vector<AbstractTrajectoryGenerator*> m_currentTrajGen;
+ std::vector<NoTrajectoryGenerator*> m_noTrajGen;
+ std::vector<MinimumJerkTrajectoryGenerator*> m_minJerkTrajGen;
+ std::vector<SinusoidTrajectoryGenerator*> m_sinTrajGen;
+ std::vector<LinearChirpTrajectoryGenerator*> m_linChirpTrajGen;
+ std::vector<TriangleTrajectoryGenerator*> m_triangleTrajGen;
+ std::vector<ConstantAccelerationTrajectoryGenerator*> m_constAccTrajGen;
+ TextFileTrajectoryGenerator* m_textFileTrajGen;
+
+ std::vector<JTG_Status> m_status_force; /// status of the forces
+ std::vector<AbstractTrajectoryGenerator*> m_currentTrajGen_force;
+ std::vector<NoTrajectoryGenerator*> m_noTrajGen_force;
+ std::vector<SinusoidTrajectoryGenerator*> m_sinTrajGen_force;
+ std::vector<MinimumJerkTrajectoryGenerator*> m_minJerkTrajGen_force;
+ std::vector<LinearChirpTrajectoryGenerator*> m_linChirpTrajGen_force;
+
+ bool generateReferenceForceSignal(const std::string& forceName, int fid, dynamicgraph::Vector& s, int iter);
+
+ bool convertJointNameToJointId(const std::string& name, unsigned int& id);
+ bool convertForceNameToForceId(const std::string& name, unsigned int& id);
+ bool isJointInRange(unsigned int id, double q);
+ bool isForceInRange(unsigned int id, const Eigen::VectorXd& f);
+ bool isForceInRange(unsigned int id, int axis, double f);
+
+ }; // class JointTrajectoryGenerator
+
+ } // namespace talos_balance
+ } // namespace sot
+} // namespace dynamicgraph
+
+
+
+#endif // #ifndef __sot_talos_balance_joint_position_controller_H__
diff --git a/include/sot/talos_balance/nd-trajectory-generator.hh b/include/sot/talos_balance/nd-trajectory-generator.hh
new file mode 100644
index 0000000..3f8138a
--- /dev/null
+++ b/include/sot/talos_balance/nd-trajectory-generator.hh
@@ -0,0 +1,196 @@
+/*
+ * Copyright 2017,Thomas Flayols, LAAS-CNRS
+ * 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.
+ * 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-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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_talos_balance_nd_trajectory_generator_H__
+#define __sot_talos_balance_nd_trajectory_generator_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (nd_position_controller_EXPORTS)
+# define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllexport)
+# else
+# define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTNDTRAJECTORYGENERATOR_EXPORT
+#endif
+
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+
+#include <parametric-curves/spline.hpp>
+#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 <sot/talos_balance/utils/signal-helper.hh>
+#include <sot/talos_balance/utils/vector-conversions.hh>
+#include <sot/talos_balance/utils/logger.hh>
+//#include <sot/talos_balance/utils/trajectory-generators.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__
diff --git a/include/sot/talos_balance/stc-commands.hh b/include/sot/talos_balance/stc-commands.hh
new file mode 100644
index 0000000..5b6f638
--- /dev/null
+++ b/include/sot/talos_balance/stc-commands.hh
@@ -0,0 +1,64 @@
+/*
+ * Copyright 2010-2017,
+ * Florent Lamiraux, Rohan Budhiraja
+ *
+ * CNRS/AIST
+ * 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.
+ * 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-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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef STC_COMMAND_HH
+#define STC_COMMAND_HH
+
+#include <fstream>
+#include <boost/assign/list_of.hpp>
+
+#include <dynamic-graph/command.h>
+#include <dynamic-graph/command-setter.h>
+#include <dynamic-graph/command-getter.h>
+#include <sot/talos_balance/joint-trajectory-generator.hh>
+
+namespace dynamicgraph { namespace sot {
+ namespace command {
+ using ::dynamicgraph::command::Command;
+ using ::dynamicgraph::command::Value;
+ using ::dynamicgraph::sot::talos_balance::JointTrajectoryGenerator;
+ // Command DisplayModel
+ class IsTrajectoryEnded : public Command
+ {
+ public:
+ virtual ~IsTrajectoryEnded() { sotDEBUGIN(15);
+ sotDEBUGOUT(15);}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ IsTrajectoryEnded(JointTrajectoryGenerator& entity, const std::string& docstring) :
+ Command(entity, std::vector<Value::Type>(),
+ docstring)
+ {
+ }
+ virtual Value doExecute()
+ {
+ JointTrajectoryGenerator& jtg = static_cast<JointTrajectoryGenerator&>(owner());
+ bool output = jtg.isTrajectoryEnded();
+ return Value(output);
+ }
+ }; // class IsTrajectoryEnded
+
+ } // namespace command
+ } /* namespace sot */
+} /* namespace dynamicgraph */
+
+#endif //STC_COMMAND_HH
diff --git a/include/sot/talos_balance/utils/common.hh b/include/sot/talos_balance/utils/common.hh
new file mode 100644
index 0000000..69d64a4
--- /dev/null
+++ b/include/sot/talos_balance/utils/common.hh
@@ -0,0 +1,292 @@
+/*
+ * Copyright 2017, A. Del Prete, T. Flayols, O. Stasse, LAAS-CNRS
+ * 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.
+ * 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-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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#ifndef __sot_talos_balance_common_H__
+#define __sot_talos_balance_common_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (hrp2_common_EXPORTS)
+# define HRP2COMMON_EXPORT __declspec(dllexport)
+# else
+# define HRP2COMMON_EXPORT __declspec(dllimport)
+# endif
+#else
+# define HRP2COMMON_EXPORT
+#endif
+
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+#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 talos_balance
+ } // namespace sot
+} // namespace dynamicgraph
+
+#endif // sot_talos_balance_common_h_
diff --git a/include/sot/talos_balance/utils/trajectory-generators.hh b/include/sot/talos_balance/utils/trajectory-generators.hh
new file mode 100644
index 0000000..8edd006
--- /dev/null
+++ b/include/sot/talos_balance/utils/trajectory-generators.hh
@@ -0,0 +1,570 @@
+/*
+ * Copyright 2015, Andrea Del Prete, LAAS-CNRS
+ * 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.
+ * 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-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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_talos_balance_trajectory_generator_H__
+#define __sot_talos_balance_trajectory_generator_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (trajectory_generator_EXPORTS)
+# define TRAJECTORYGENERATOR_EXPORT __declspec(dllexport)
+# else
+# define TRAJECTORYGENERATOR_EXPORT __declspec(dllimport)
+# endif
+#else
+# define TALOSCOMMON_EXPORT
+#endif
+
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+#include <iostream>
+#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 <fstream> /// to read text file
+#include "boost/assign.hpp"
+
+
+namespace dynamicgraph {
+ namespace sot {
+ namespace talos_balance {
+
+ #define MAXBUFSIZE ((int) 1000000)
+
+ Eigen::MatrixXd readMatrixFromFile(const char *filename)
+ {
+ int cols = 0, rows = 0;
+ double buff[MAXBUFSIZE];
+
+ // Read numbers from file into buffer.
+ std::ifstream infile;
+ infile.open(filename);
+ while (! infile.eof())
+ {
+ std::string line;
+ getline(infile, line);
+// std::cout<<"Read line "<<rows<<"\n";
+
+ int temp_cols = 0;
+ std::stringstream stream(line);
+ while(! stream.eof())
+ stream >> buff[cols*rows+temp_cols++];
+
+ if (temp_cols == 0)
+ continue;
+
+ if (cols == 0)
+ cols = temp_cols;
+ else if(temp_cols!=cols && !infile.eof())
+ {
+ std::cout<<"Error while reading matrix from file, line "<<rows<<" has "<<temp_cols<<" columnds, while preceding lines had "<<cols<<" columnds\n";
+ std::cout<<line<<"\n";
+ break;
+ }
+
+ rows++;
+ if((rows+1)*cols>=MAXBUFSIZE)
+ {
+ std::cout<<"Max buffer size exceeded ("<<rows<<" rows, "<<cols<<" cols)\n";
+ break;
+ }
+ }
+ infile.close();
+ rows--;
+
+ // Populate matrix with numbers.
+ Eigen::MatrixXd result(rows,cols);
+ for (int i = 0; i < rows; i++)
+ for (int j = 0; j < cols; j++)
+ result(i,j) = buff[ cols*i+j ];
+
+ return result;
+ }
+
+ class AbstractTrajectoryGenerator
+ {
+ protected:
+ Eigen::VectorXd m_x; /// current position
+ Eigen::VectorXd m_dx; /// current velocity
+ Eigen::VectorXd m_ddx; /// current acceleration
+ Eigen::VectorXd m_x_init; /// initial position
+ Eigen::VectorXd m_x_final; /// final position
+
+ double m_traj_time; /// time to go from x_init to x_final (sec)
+ double m_dt; /// control dt (sampling period of the trajectory)
+ double m_t; /// current time
+ Eigen::VectorXd::Index m_size;
+
+ virtual void resizeAllData(Eigen::VectorXd::Index size)
+ {
+ m_size = size;
+ m_x.setZero(size);
+ m_dx.setZero(size);
+ m_ddx.setZero(size);
+ m_x_init.setZero(size);
+ m_x_final.setZero(size);
+ }
+
+ void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
+ {
+ getLogger().sendMsg("[AbstrTrajGen] "+msg, t, file, line);
+ }
+
+ public:
+
+ AbstractTrajectoryGenerator(double dt, double traj_time,
+ Eigen::VectorXd::Index size)
+ {
+ m_t = 0.0;
+ m_dt = dt;
+ m_traj_time = traj_time;
+ resizeAllData(size);
+ }
+
+ AbstractTrajectoryGenerator(double dt, double traj_time, const Eigen::VectorXd &x_init, const Eigen::VectorXd &x_final)
+ {
+ assert(x_init.size()==x_final.size() && "Initial and final state must have the same size");
+ m_dt = dt;
+ m_traj_time = traj_time;
+ resizeAllData(x_init.size());
+ set_initial_point(x_init);
+ set_final_point(x_final);
+ }
+
+ virtual bool set_initial_point(const Eigen::VectorXd& x_init)
+ {
+ if(x_init.size()!=m_x_init.size())
+ return false;
+ m_x_init = x_init;
+ m_x = x_init;
+ m_dx.setZero();
+ m_t = 0.0;
+ return true;
+ }
+
+ virtual bool set_initial_point(const double& x_init)
+ {
+ if(1!=m_x_init.size())
+ return false;
+ m_x_init(0) = x_init;
+ m_x(0) = x_init;
+ m_dx(0) = 0.0;
+ m_t = 0.0;
+ return true;
+ }
+
+ virtual bool set_final_point(const Eigen::VectorXd& x_final)
+ {
+ if(x_final.size()!=m_x_final.size())
+ return false;
+ m_x_final = x_final;
+ return true;
+ }
+
+ virtual bool set_final_point(const double& x_final)
+ {
+ if(1!=m_x_final.size())
+ return false;
+ m_x_final(0) = x_final;
+ return true;
+ }
+
+ virtual bool set_trajectory_time(double traj_time)
+ {
+ if(traj_time<=0.0)
+ return false;
+ m_traj_time = traj_time;
+ return true;
+ }
+
+ virtual const Eigen::VectorXd& compute_next_point() = 0;
+
+ virtual const Eigen::VectorXd& getPos(){ return m_x; }
+ virtual const Eigen::VectorXd& getVel(){ return m_dx; }
+ virtual const Eigen::VectorXd& getAcc(){ return m_ddx; }
+ virtual const Eigen::VectorXd& get_initial_point(){ return m_x_init; }
+ virtual const Eigen::VectorXd& get_final_point(){ return m_x_final; }
+
+ virtual bool isTrajectoryEnded(){ return m_t>= m_traj_time; }
+
+ };
+
+ /** Fake trajectory generator that actually generates no trajectory
+ * at all.
+ * */
+ class NoTrajectoryGenerator: public AbstractTrajectoryGenerator
+ {
+ public:
+ NoTrajectoryGenerator(int size):
+ AbstractTrajectoryGenerator(0.001, 1.0, size)
+ {}
+
+ virtual const Eigen::VectorXd& compute_next_point()
+ {
+ m_t += m_dt;
+ return m_x;
+ }
+
+ virtual bool isTrajectoryEnded(){ return false; }
+ };
+
+
+ /** Trajectory generator that reads the trajectory and its derivatives from a text file.
+ * */
+ class TextFileTrajectoryGenerator: public AbstractTrajectoryGenerator
+ {
+ protected:
+ Eigen::MatrixXd m_posTraj;
+ Eigen::MatrixXd m_velTraj;
+ Eigen::MatrixXd m_accTraj;
+
+ public:
+ TextFileTrajectoryGenerator(double dt, Eigen::VectorXd::Index size):
+ AbstractTrajectoryGenerator(dt, 1.0, size)
+ {}
+
+ virtual bool loadTextFile(const std::string& fileName)
+ {
+ Eigen::MatrixXd data = readMatrixFromFile(fileName.c_str());
+// std::cout<<"Read matrix with "<<data.rows()<<" rows and "<<data.cols()<<" cols from text file\n";
+ if(data.cols()!=3*m_size)
+ {
+ std::cout<<"Unexpected number of columns (expected "<<3*m_size<<", found "<<data.cols()<<")\n";
+ return false;
+ }
+
+ m_traj_time = m_dt*(double)data.rows();
+ m_t = 0.0;
+
+ m_posTraj = data.leftCols(m_size);
+ m_velTraj = data.middleCols(m_size,m_size);
+ m_accTraj = data.rightCols(m_size);
+
+ m_x_init = m_posTraj.row(0);
+
+ return true;
+ }
+
+ virtual const Eigen::VectorXd& compute_next_point()
+ {
+ Eigen::VectorXd::Index i = (Eigen::VectorXd::Index)std::floor(m_t/m_dt);
+ if(i<m_posTraj.rows())
+ {
+ m_x = m_posTraj.row(i);
+ m_dx = m_velTraj.row(i);
+ m_ddx = m_accTraj.row(i);
+ }
+ m_t += m_dt;
+ return m_x;
+ }
+ };
+
+
+ /** Point-to-point minimum-jerk trajectory generator. */
+ class MinimumJerkTrajectoryGenerator: public AbstractTrajectoryGenerator
+ {
+ public:
+ MinimumJerkTrajectoryGenerator(double dt, double traj_time, int size):
+ AbstractTrajectoryGenerator(dt, traj_time, size)
+ {}
+
+ virtual const Eigen::VectorXd& compute_next_point()
+ {
+ if(m_t <= m_traj_time)
+ {
+ double td = m_t/m_traj_time;
+ double td2 = td*td;
+ double td3 = td2*td;
+ double td4 = td3*td;
+ double td5 = td4*td;
+ double p = 10*td3 - 15*td4 + 6*td5;
+ double dp = (30*td2 - 60*td3 + 30*td4)/m_traj_time;
+ double ddp = (60*td - 180*td2 + 120*td3)/(m_traj_time*m_traj_time);
+ m_x = m_x_init + (m_x_final-m_x_init)*p;
+ m_dx = (m_x_final-m_x_init)*dp;
+ m_ddx = (m_x_final-m_x_init)*ddp;
+ }
+ m_t += m_dt;
+ return m_x;
+ }
+
+ };
+
+ /** Endless sinusoidal trajectory generator.
+ * The sinusoid is actually a cosine so that it starts with zero velocity.
+ */
+ class SinusoidTrajectoryGenerator: public AbstractTrajectoryGenerator
+ {
+ public:
+
+ SinusoidTrajectoryGenerator(double dt, double traj_time, int size):
+ AbstractTrajectoryGenerator(dt, traj_time, size)
+ {}
+
+
+ const Eigen::VectorXd& compute_next_point()
+ {
+ double f = 1.0/(2.0*m_traj_time);
+ double two_pi_f = 2*M_PI*f;
+ double two_pi_f_t = two_pi_f*m_t;
+ double p = 0.5*(1.0-cos(two_pi_f_t));
+ double dp = 0.5*two_pi_f*sin(two_pi_f_t);
+ double ddp = 0.5*two_pi_f*two_pi_f*cos(two_pi_f_t);
+ m_x = m_x_init + (m_x_final-m_x_init)*p;
+ m_dx = (m_x_final-m_x_init)*dp;
+ m_ddx = (m_x_final-m_x_init)*ddp;
+
+ m_t += m_dt;
+ return m_x;
+ }
+
+ virtual bool isTrajectoryEnded(){ return false; }
+ };
+
+ /** Endless triangular trajectory generator.
+ */
+ class TriangleTrajectoryGenerator: public AbstractTrajectoryGenerator
+ {
+ protected:
+ bool m_comingBack;
+ double m_Tacc;
+
+ public:
+
+ TriangleTrajectoryGenerator(double dt, double traj_time, int size):
+ AbstractTrajectoryGenerator(dt, traj_time, size),
+ m_comingBack(false)
+ {
+ m_Tacc = 1.0;
+ }
+
+ bool set_acceleration_time(const double Tacc)
+ {
+ if(Tacc<0.0 || Tacc> 0.5*m_traj_time)
+ return false;
+ m_Tacc = Tacc;
+ return true;
+ }
+
+
+ const Eigen::VectorXd& compute_next_point()
+ {
+ int way;
+ double t=m_t;
+ Eigen::VectorXd max_vel = (m_x_final-m_x_init)/(m_traj_time-m_Tacc);
+ if (m_t > m_traj_time)
+ {
+ way = -1;
+ t=t-m_traj_time;
+ }
+ else
+ {
+ way = 1;
+ }
+ if (t < m_Tacc)
+ {
+ m_ddx = way*max_vel / m_Tacc;
+ m_dx = t/m_Tacc *way*max_vel;
+ }
+ else if (t > m_traj_time-m_Tacc)
+ {
+ m_ddx = -way*max_vel / m_Tacc;
+ m_dx = (m_traj_time-t)/m_Tacc *way*max_vel;
+ }
+ else
+ {
+ m_ddx = 0.0 * max_vel;
+ m_dx = way*max_vel;
+ }
+ m_x += m_dt*m_dx;
+ m_t += m_dt;
+ if (m_t>=2*m_traj_time) m_t =m_t-2*m_traj_time;
+ return m_x;
+ }
+ virtual bool isTrajectoryEnded(){ return false; }
+ };
+
+ /** Endless piece-wise constant acceleration trajectory generator.
+ */
+ class ConstantAccelerationTrajectoryGenerator: public AbstractTrajectoryGenerator
+ {
+ protected:
+ bool m_is_accelerating; // if true then apply ddx0, otherwise apply -ddx0
+ int m_counter; // counter to decide when to switch from ddx0 to -ddx0
+ int m_counter_max; // value of the counter at which to switch from ddx0 to -ddx0
+ Eigen::VectorXd m_ddx0; // acceleration to go halfway from x_init to x_final in half traj_time
+
+ public:
+
+ ConstantAccelerationTrajectoryGenerator(double dt, double traj_time, int size):
+ AbstractTrajectoryGenerator(dt, traj_time, size),
+ m_is_accelerating(true)
+ {}
+
+ virtual bool set_trajectory_time(double traj_time)
+ {
+ bool res = AbstractTrajectoryGenerator::set_trajectory_time(traj_time);
+ if(!res)
+ return false;
+ m_counter_max = int(m_traj_time/m_dt);
+ m_counter = int(0.5*m_counter_max);
+ m_is_accelerating = true;
+ return true;
+ }
+
+ const Eigen::VectorXd& compute_next_point()
+ {
+ m_ddx0 = 4.0*(m_x_final-m_x_init)/(m_traj_time*m_traj_time);
+
+ if(m_counter==m_counter_max)
+ {
+ m_counter = 0;
+ m_is_accelerating = ! m_is_accelerating;
+ }
+ m_counter += 1;
+
+ m_ddx = m_ddx0;
+ if(m_is_accelerating == false)
+ m_ddx *= -1.0;
+
+ m_x += m_dt*m_dx + 0.5*m_dt*m_dt*m_ddx;
+ m_dx += m_dt*m_ddx;
+
+ m_t += m_dt;
+ return m_x;
+ }
+
+ virtual bool isTrajectoryEnded(){ return false; }
+ };
+
+ /** Linear chirp trajectory generator.
+ * A linear chirp is a sinusoid whose frequency is a linear function of time.
+ * In particular the frequency starts from a value f0 and it increases linearly
+ * up to a value f1. Then it goes back to f0 and the trajectory is ended.
+ */
+ class LinearChirpTrajectoryGenerator: public AbstractTrajectoryGenerator
+ {
+ protected:
+ Eigen::VectorXd m_f0; /// initial frequency
+ Eigen::VectorXd m_f1; /// final frequency
+
+ /// Variables for temporary results
+ Eigen::VectorXd m_k; /// frequency first derivative
+ Eigen::VectorXd m_f; /// current frequency (i.e. time derivative of the phase over 2*pi)
+ Eigen::VectorXd m_phi; /// current phase
+ Eigen::VectorXd m_phi_0; /// phase shift for second half of trajectory
+ Eigen::VectorXd m_p;
+ Eigen::VectorXd m_dp;
+ Eigen::VectorXd m_ddp;
+
+ public:
+
+ LinearChirpTrajectoryGenerator(double dt, double traj_time, int size):
+ AbstractTrajectoryGenerator(dt, traj_time, size)
+ {
+ m_f0.setZero(size);
+ m_f1.setZero(size);
+ m_k.setZero(size);
+ m_f.setZero(size);
+ m_phi.setZero(size);
+ m_phi_0.setZero(size);
+ m_p.setZero(size);
+ m_dp.setZero(size);
+ m_ddp.setZero(size);
+ }
+
+ virtual bool set_initial_frequency(const Eigen::VectorXd& f0)
+ {
+ if(f0.size()!=m_f0.size())
+ return false;
+ m_f0 = f0;
+ return true;
+ }
+
+ virtual bool set_initial_frequency(const double& f0)
+ {
+ if(1!=m_f0.size())
+ return false;
+ m_f0[0] = f0;
+ return true;
+ }
+
+ virtual bool set_final_frequency(const Eigen::VectorXd& f1)
+ {
+ if(f1.size()!=m_f1.size())
+ return false;
+ m_f1 = f1;
+ return true;
+ }
+
+
+ virtual bool set_final_frequency(const double& f1)
+ {
+ if(1!=m_f1.size())
+ return false;
+ m_f1[0] = f1;
+ return true;
+ }
+
+
+ const Eigen::VectorXd& compute_next_point()
+ {
+ if(m_t==0.0)
+ {
+ m_k = 2.0*(m_f1-m_f0)/m_traj_time;
+ m_phi_0 = M_PI*m_traj_time*(m_f0-m_f1);
+ }
+
+ if(m_t<0.5*m_traj_time)
+ {
+ m_f = m_f0 + m_k*m_t;
+ m_phi = 2*M_PI*m_t*(m_f0 + 0.5*m_k*m_t);
+ }
+ else
+ {
+ m_f = m_f1 + m_k*(0.5*m_traj_time - m_t);
+ m_phi = m_phi_0 + 2*M_PI*m_t*(m_f1 + 0.5*m_k*(m_traj_time - m_t));
+ }
+ m_p = 0.5*(1.0-m_phi.array().cos());
+ m_dp = M_PI*m_f.array() * m_phi.array().sin();
+ m_ddp = 2.0*M_PI*M_PI * m_f.array() * m_f.array() * m_phi.array().cos();
+
+ m_x = m_x_init.array() + (m_x_final.array()-m_x_init.array())*m_p.array();
+ m_dx = (m_x_final-m_x_init).array()*m_dp.array();
+ m_ddx = (m_x_final-m_x_init).array()*m_ddp.array();
+
+ m_t += m_dt;
+ return m_x;
+ }
+ };
+
+
+
+ } // namespace talos_balance
+ } // namespace sot
+} // namespace dynamicgraph
+
+
+
+#endif // #ifndef __sot_talos_balance_trajectory_generators_H__
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 61a6362..5ab769c 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -37,6 +37,8 @@ ENDIF(UNIX)
SET(plugins
example
joint-position-controller
+ joint-trajectory-generator
+ nd-trajectory-generator
)
#set(ADDITIONAL_feature-task_LIBS feature-generic task)
diff --git a/src/joint-trajectory-generator.cpp b/src/joint-trajectory-generator.cpp
new file mode 100644
index 0000000..1ae205d
--- /dev/null
+++ b/src/joint-trajectory-generator.cpp
@@ -0,0 +1,884 @@
+/*
+ * Copyright 2014, Oscar E. Ramos Ponce, LAAS-CNRS
+ * 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.
+ * 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-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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <sot/talos_balance/joint-trajectory-generator.hh>
+#include <sot/core/debug.hh>
+#include <dynamic-graph/factory.h>
+
+#include <sot/talos_balance/utils/commands-helper.hh>
+#include <sot/talos_balance/utils/stop-watch.hh>
+
+#include "../include/sot/talos_balance/stc-commands.hh"
+
+namespace dynamicgraph
+{
+ namespace sot
+ {
+ namespace talos_balance
+ {
+ namespace dynamicgraph = ::dynamicgraph;
+ using namespace dynamicgraph;
+ using namespace dynamicgraph::command;
+ using namespace std;
+ using namespace Eigen;
+
+//Size to be aligned "-------------------------------------------------------"
+#define PROFILE_POSITION_DESIRED_COMPUTATION "TrajGen: reference joint traj computation "
+#define PROFILE_FORCE_DESIRED_COMPUTATION "TrajGen: reference force computation "
+
+ /// Define EntityClassName here rather than in the header file
+ /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
+ typedef JointTrajectoryGenerator EntityClassName;
+
+ /* --- DG FACTORY ---------------------------------------------------- */
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(JointTrajectoryGenerator,
+ "JointTrajectoryGenerator");
+
+ /* ------------------------------------------------------------------- */
+ /* --- CONSTRUCTION -------------------------------------------------- */
+ /* ------------------------------------------------------------------- */
+ JointTrajectoryGenerator::
+ JointTrajectoryGenerator(const std::string& name)
+ : Entity(name)
+ ,CONSTRUCT_SIGNAL_IN(base6d_encoders,dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL_OUT(q, dynamicgraph::Vector, m_base6d_encodersSIN)
+ ,CONSTRUCT_SIGNAL_OUT(dq, dynamicgraph::Vector, m_qSOUT)
+ ,CONSTRUCT_SIGNAL_OUT(ddq, dynamicgraph::Vector, m_qSOUT)
+ ,CONSTRUCT_SIGNAL(fRightFoot, OUT, dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL(fLeftFoot, OUT, dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL(fRightHand, OUT, dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL(fLeftHand, OUT, dynamicgraph::Vector)
+ ,m_firstIter(true)
+ ,m_initSucceeded(false)
+ ,m_robot_util(RefVoidRobotUtil())
+ {
+ BIND_SIGNAL_TO_FUNCTION(fRightFoot, OUT, dynamicgraph::Vector);
+ BIND_SIGNAL_TO_FUNCTION(fLeftFoot, OUT, dynamicgraph::Vector);
+ BIND_SIGNAL_TO_FUNCTION(fRightHand, OUT, dynamicgraph::Vector);
+ BIND_SIGNAL_TO_FUNCTION(fLeftHand, OUT, dynamicgraph::Vector);
+
+
+ m_status_force.resize(4,JTG_STOP);
+ m_minJerkTrajGen_force.resize(4);
+ m_sinTrajGen_force.resize(4);
+ m_linChirpTrajGen_force.resize(4);
+ m_currentTrajGen_force.resize(4);
+ m_noTrajGen_force.resize(4);
+
+ m_iterForceSignals.resize(4, 0);
+
+ Entity::signalRegistration( m_qSOUT << m_dqSOUT << m_ddqSOUT << m_base6d_encodersSIN
+ << m_fRightFootSOUT << m_fLeftFootSOUT
+ << m_fRightHandSOUT << m_fLeftHandSOUT);
+
+ /* Commands. */
+ addCommand("init",
+ makeCommandVoid2(*this, &JointTrajectoryGenerator::init,
+ docCommandVoid2("Initialize the entity.",
+ "Time period in seconds (double)",
+ "robotRef (string)")));
+
+ addCommand("getJoint",
+ makeCommandVoid1(*this, &JointTrajectoryGenerator::getJoint,
+ docCommandVoid1("Get the current angle of the specified joint.",
+ "Joint name (string)")));
+
+ //const std::string& docstring = ;
+ addCommand("isTrajectoryEnded",
+ new command::IsTrajectoryEnded(*this, "Return whether all joint trajectories have ended"));
+
+ addCommand("playTrajectoryFile",
+ makeCommandVoid1(*this, &JointTrajectoryGenerator::playTrajectoryFile,
+ docCommandVoid1("Play the trajectory read from the specified text file.",
+ "(string) File name, path included")));
+
+ addCommand("startSinusoid",
+ makeCommandVoid3(*this, &JointTrajectoryGenerator::startSinusoid,
+ docCommandVoid3("Start an infinite sinusoid motion.",
+ "(string) joint name",
+ "(double) final angle in radians",
+ "(double) time to reach the final angle in sec")));
+
+ addCommand("startTriangle",
+ makeCommandVoid4(*this, &JointTrajectoryGenerator::startTriangle,
+ docCommandVoid4("Start an infinite triangle wave.",
+ "(string) joint name",
+ "(double) final angle in radians",
+ "(double) time to reach the final angle in sec",
+ "(double) time to accelerate in sec")));
+
+ addCommand("startConstAcc",
+ makeCommandVoid3(*this, &JointTrajectoryGenerator::startConstAcc,
+ docCommandVoid3("Start an infinite trajectory with piece-wise constant acceleration.",
+ "(string) joint name",
+ "(double) final angle in radians",
+ "(double) time to reach the final angle in sec")));
+
+ addCommand("startForceSinusoid",
+ makeCommandVoid4(*this, &JointTrajectoryGenerator::startForceSinusoid,
+ docCommandVoid4("Start an infinite sinusoid force.",
+ "(string) force name",
+ "(int) force axis in [0, 5]",
+ "(double) final 1d force in N or Nm",
+ "(double) time to reach the final force in sec")));
+
+// addCommand("startForceSinusoid",
+// makeCommandVoid3(*this, &JointTrajectoryGenerator::startForceSinusoid,
+// docCommandVoid3("Start an infinite sinusoid force.",
+// "(string) force name",
+// "(Vector) final 6d force in N/Nm",
+// "(double) time to reach the final force in sec")));
+
+ addCommand("startLinChirp",
+ makeCommandVoid5(*this, &JointTrajectoryGenerator::startLinearChirp,
+ docCommandVoid5("Start a linear-chirp motion.",
+ "(string) joint name",
+ "(double) final angle in radians",
+ "(double) initial frequency [Hz]",
+ "(double) final frequency [Hz]",
+ "(double) trajectory time [sec]")));
+
+ addCommand("startForceLinChirp",
+ makeCommandVoid6(*this, &JointTrajectoryGenerator::startForceLinearChirp,
+ docCommandVoid6("Start a linear-chirp force traj.",
+ "(string) force name",
+ "(int) force axis",
+ "(double) final force in N/Nm",
+ "(double) initial frequency [Hz]",
+ "(double) final frequency [Hz]",
+ "(double) trajectory time [sec]")));
+
+ addCommand("moveJoint",
+ makeCommandVoid3(*this, &JointTrajectoryGenerator::moveJoint,
+ docCommandVoid3("Move the joint to the specified angle with a minimum jerk trajectory.",
+ "(string) joint name",
+ "(double) final angle in radians",
+ "(double) time to reach the final angle in sec")));
+
+ addCommand("moveForce",
+ makeCommandVoid4(*this, &JointTrajectoryGenerator::moveForce,
+ docCommandVoid4("Move the force to the specified value with a minimum jerk trajectory.",
+ "(string) force name",
+ "(int) force axis",
+ "(double) final force in N/Nm",
+ "(double) time to reach the final force in sec")));
+
+ addCommand("stop",
+ makeCommandVoid1(*this, &JointTrajectoryGenerator::stop,
+ docCommandVoid1("Stop the motion of the specified joint, or of all joints if no joint name is specified.",
+ "(string) joint name")));
+
+ addCommand("stopForce",
+ makeCommandVoid1(*this, &JointTrajectoryGenerator::stopForce,
+ docCommandVoid1("Stop the specified force trajectort",
+ "(string) force name (rh,lh,lf,rf)")));
+ }
+
+ void JointTrajectoryGenerator::init(const double& dt,
+ const std::string& robotRef)
+ {
+ /* Retrieve m_robot_util informations */
+ std::string localName(robotRef);
+ if (isNameInRobotUtil(localName))
+ m_robot_util = getRobotUtil(localName);
+ else
+ {
+ SEND_MSG("You should have an entity controller manager initialized before",MSG_TYPE_ERROR);
+ return;
+ }
+
+ if(dt<=0.0)
+ return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
+ m_firstIter = true;
+ m_dt = dt;
+
+ m_status.resize(m_robot_util->m_nbJoints,JTG_STOP);
+ m_minJerkTrajGen.resize(m_robot_util->m_nbJoints);
+ m_sinTrajGen.resize(m_robot_util->m_nbJoints);
+ m_triangleTrajGen.resize(m_robot_util->m_nbJoints);
+ m_constAccTrajGen.resize(m_robot_util->m_nbJoints);
+ m_linChirpTrajGen.resize(m_robot_util->m_nbJoints);
+ m_currentTrajGen.resize(m_robot_util->m_nbJoints);
+ m_noTrajGen.resize(m_robot_util->m_nbJoints);
+
+ for(int i=0; i<m_robot_util->m_nbJoints; i++)
+ {
+ m_minJerkTrajGen[i] = new MinimumJerkTrajectoryGenerator(dt,5.0,1);
+ m_sinTrajGen[i] = new SinusoidTrajectoryGenerator(dt,5.0,1);
+ m_triangleTrajGen[i] = new TriangleTrajectoryGenerator(dt,5.0,1);
+ m_constAccTrajGen[i] = new ConstantAccelerationTrajectoryGenerator(dt,5.0,1);
+ m_linChirpTrajGen[i] = new LinearChirpTrajectoryGenerator(dt,5.0,1);
+ m_noTrajGen[i] = new NoTrajectoryGenerator(1);
+ m_currentTrajGen[i] = m_noTrajGen[i];
+ }
+ m_textFileTrajGen = new TextFileTrajectoryGenerator(dt, m_robot_util->m_nbJoints);
+
+ for(int i=0; i<4; i++)
+ {
+ m_minJerkTrajGen_force[i] = new MinimumJerkTrajectoryGenerator(dt,5.0,6);
+ m_sinTrajGen_force[i] = new SinusoidTrajectoryGenerator(dt,5.0,6);
+ m_linChirpTrajGen_force[i] = new LinearChirpTrajectoryGenerator(dt,5.0,6);
+ m_noTrajGen_force[i] = new NoTrajectoryGenerator(6);
+ m_currentTrajGen_force[i] = m_noTrajGen_force[i];
+ }
+ m_initSucceeded = true;
+ }
+
+
+ /* ------------------------------------------------------------------- */
+ /* --- SIGNALS ------------------------------------------------------- */
+ /* ------------------------------------------------------------------- */
+
+ DEFINE_SIGNAL_OUT_FUNCTION(q, dynamicgraph::Vector)
+ {
+ if(!m_initSucceeded)
+ {
+ SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
+ return s;
+ }
+
+
+ getProfiler().start(PROFILE_POSITION_DESIRED_COMPUTATION);
+ {
+ // at first iteration store current joints positions
+ if(m_firstIter)
+ {
+ const dynamicgraph::Vector& base6d_encoders = m_base6d_encodersSIN(iter);
+ if(base6d_encoders.size()!=m_robot_util->m_nbJoints+6)
+ {
+ SEND_ERROR_STREAM_MSG("Unexpected size of signal base6d_encoder " +
+ toString(base6d_encoders.size()) + " " +
+ toString(m_robot_util->m_nbJoints+6)
+ );
+ return s;
+ }
+ for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
+ m_noTrajGen[i]->set_initial_point(base6d_encoders(6+i));
+ m_firstIter = false;
+ }
+
+ if(s.size()!=m_robot_util->m_nbJoints)
+ s.resize(m_robot_util->m_nbJoints);
+
+ if(m_status[0]==JTG_TEXT_FILE)
+ {
+ const VectorXd& qRef = m_textFileTrajGen->compute_next_point();
+ for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
+ {
+ s(i) = qRef[i];
+ if(m_textFileTrajGen->isTrajectoryEnded())
+ {
+ m_noTrajGen[i]->set_initial_point(s(i));
+ m_status[i] = JTG_STOP;
+ }
+ }
+ if(m_textFileTrajGen->isTrajectoryEnded())
+ SEND_MSG("Text file trajectory ended.", MSG_TYPE_INFO);
+ }
+ else
+ {
+ for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
+ {
+ s(i) = m_currentTrajGen[i]->compute_next_point()(0);
+ if(m_currentTrajGen[i]->isTrajectoryEnded())
+ {
+ m_currentTrajGen[i] = m_noTrajGen[i];
+ m_noTrajGen[i]->set_initial_point(s(i));
+ m_status[i] = JTG_STOP;
+ SEND_MSG("Trajectory of joint "+
+ m_robot_util->get_name_from_id(i)+
+ " ended.", MSG_TYPE_INFO);
+ }
+ }
+ }
+
+ }
+ getProfiler().stop(PROFILE_POSITION_DESIRED_COMPUTATION);
+
+ return s;
+ }
+
+ DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)
+ {
+ if(!m_initSucceeded)
+ {
+ SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
+ return s;
+ }
+
+ const dynamicgraph::Vector& q = m_qSOUT(iter);
+
+ if(s.size()!=m_robot_util->m_nbJoints)
+ s.resize(m_robot_util->m_nbJoints);
+ if(m_status[0]==JTG_TEXT_FILE)
+ {
+ for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
+ s(i) = m_textFileTrajGen->getVel()[i];
+ }
+ else
+ for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
+ s(i) = m_currentTrajGen[i]->getVel()(0);
+
+ return s;
+ }
+
+ DEFINE_SIGNAL_OUT_FUNCTION(ddq, dynamicgraph::Vector)
+ {
+ if(!m_initSucceeded)
+ {
+ SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
+ return s;
+ }
+
+ const dynamicgraph::Vector& q = m_qSOUT(iter);
+
+ if(s.size()!=m_robot_util->m_nbJoints)
+ s.resize(m_robot_util->m_nbJoints);
+
+ if(m_status[0]==JTG_TEXT_FILE)
+ {
+ for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
+ s(i) = m_textFileTrajGen->getAcc()[i];
+ }
+ else
+ for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
+ s(i) = m_currentTrajGen[i]->getAcc()(0);
+
+ return s;
+ }
+
+ DEFINE_SIGNAL_OUT_FUNCTION(fRightFoot, dynamicgraph::Vector)
+ {
+ // SEND_MSG("Compute force right foot iter "+toString(iter), MSG_TYPE_DEBUG);
+ generateReferenceForceSignal("fRightFoot",
+ m_robot_util->m_force_util.get_force_id_right_foot(),
+ s, iter);
+ return s;
+ }
+
+ DEFINE_SIGNAL_OUT_FUNCTION(fLeftFoot, dynamicgraph::Vector)
+ {
+ generateReferenceForceSignal("fLeftFoot",
+ m_robot_util->m_force_util.get_force_id_left_foot(),
+ s, iter);
+ return s;
+ }
+
+ DEFINE_SIGNAL_OUT_FUNCTION(fRightHand, dynamicgraph::Vector)
+ {
+ generateReferenceForceSignal("fRightHand",
+ m_robot_util->
+ m_force_util.get_force_id_right_hand(),
+ s, iter);
+ return s;
+ }
+
+ DEFINE_SIGNAL_OUT_FUNCTION(fLeftHand, dynamicgraph::Vector)
+ {
+ generateReferenceForceSignal("fLeftHand",
+ m_robot_util->
+ m_force_util.get_force_id_left_hand(),
+ s, iter);
+ return s;
+ }
+
+ bool JointTrajectoryGenerator::generateReferenceForceSignal(const std::string& forceName, int fid, dynamicgraph::Vector& s, int iter)
+ {
+ if(!m_initSucceeded)
+ {
+ SEND_WARNING_STREAM_MSG("Cannot compute signal "+
+ forceName+" before initialization!");
+ return false;
+ }
+
+ getProfiler().start(PROFILE_FORCE_DESIRED_COMPUTATION);
+ {
+ if(s.size()!=6)
+ s.resize(6);
+
+ if(iter>m_iterForceSignals[fid])
+ {
+ m_currentTrajGen_force[fid]->compute_next_point();
+// SEND_MSG("Compute force "+forceName+" with id "+toString(fid)+": "+toString(fr.transpose()), MSG_TYPE_DEBUG);
+ m_iterForceSignals[fid]++;
+ }
+
+ const Eigen::VectorXd& fr = m_currentTrajGen_force[fid]->getPos();
+ for(unsigned int i=0; i<6; i++)
+ s(i) = fr(i);
+
+ if(m_currentTrajGen_force[fid]->isTrajectoryEnded())
+ {
+ m_noTrajGen_force[fid]->set_initial_point(m_currentTrajGen_force[fid]->getPos());
+ m_currentTrajGen_force[fid] = m_noTrajGen_force[fid];
+ m_status_force[fid] = JTG_STOP;
+ SEND_MSG("Trajectory of force "+forceName+" ended.", MSG_TYPE_INFO);
+ }
+ }
+ getProfiler().stop(PROFILE_FORCE_DESIRED_COMPUTATION);
+
+ return true;
+ }
+
+ /* ------------------------------------------------------------------- */
+ /* --- COMMANDS ------------------------------------------------------ */
+ /* ------------------------------------------------------------------- */
+
+ void JointTrajectoryGenerator::getJoint(const std::string& jointName)
+ {
+ unsigned int i;
+ if(convertJointNameToJointId(jointName,i)==false)
+ return;
+ const dynamicgraph::Vector& base6d_encoders = m_base6d_encodersSIN.accessCopy();
+ SEND_MSG("Current angle of joint "+jointName+" is "+toString(base6d_encoders(6+i)), MSG_TYPE_INFO);
+ }
+
+ bool JointTrajectoryGenerator::isTrajectoryEnded()
+ {
+ bool output=true;
+ if(m_status[0]==JTG_TEXT_FILE)
+ {
+ for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
+ {
+ if(!m_textFileTrajGen->isTrajectoryEnded())
+ {
+ output=false;
+ SEND_MSG("Text file trajectory not ended.", MSG_TYPE_INFO);
+ return output;
+ }
+ }
+ }
+ else
+ {
+ for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
+ {
+ if(!m_currentTrajGen[i]->isTrajectoryEnded())
+ {
+ output=false;
+ SEND_MSG("Trajectory of joint "+
+ m_robot_util->get_name_from_id(i)+
+ "not ended.", MSG_TYPE_INFO);
+ return output;
+ }
+ }
+ }
+ return output;
+ }
+
+ void JointTrajectoryGenerator::playTrajectoryFile(const std::string& fileName)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot start sinusoid before initialization!",MSG_TYPE_ERROR);
+
+ for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
+ if(m_status[i]!=JTG_STOP)
+ return SEND_MSG("You cannot joint "+m_robot_util->get_name_from_id(i)+" because it is already controlled.", MSG_TYPE_ERROR);
+
+ if(!m_textFileTrajGen->loadTextFile(fileName))
+ return SEND_MSG("Error while loading text file "+fileName, MSG_TYPE_ERROR);
+
+ // check current configuration is not too far from initial trajectory configuration
+ bool needToMoveToInitConf = false;
+ const VectorXd& qInit = m_textFileTrajGen->get_initial_point();
+ for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
+ if(fabs(qInit[i] - m_currentTrajGen[i]->getPos()(0)) > 0.001)
+ {
+ needToMoveToInitConf = true;
+ SEND_MSG("Joint "+m_robot_util->get_name_from_id(i)+" is too far from initial configuration so first i will move it there.", MSG_TYPE_WARNING);
+ }
+
+ // if necessary move joints to initial configuration
+ if(needToMoveToInitConf)
+ {
+ for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
+ {
+ if(!isJointInRange(i, qInit[i]))
+ return;
+
+ m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
+ m_minJerkTrajGen[i]->set_final_point(qInit[i]);
+ m_minJerkTrajGen[i]->set_trajectory_time(4.0);
+ m_status[i] = JTG_MIN_JERK;
+ m_currentTrajGen[i] = m_minJerkTrajGen[i];
+ }
+ return;
+ }
+
+ for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
+ {
+ m_status[i] = JTG_TEXT_FILE;
+ }
+ }
+
+ void JointTrajectoryGenerator::startSinusoid(const std::string& jointName, const double& qFinal, const double& time)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot start sinusoid before initialization!",MSG_TYPE_ERROR);
+
+ unsigned int i;
+ if(convertJointNameToJointId(jointName,i)==false)
+ return;
+ if(time<=0.0)
+ return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
+ if(m_status[i]!=JTG_STOP)
+ return SEND_MSG("You cannot move the specified joint because it is already controlled.", MSG_TYPE_ERROR);
+ if(!isJointInRange(i, qFinal))
+ return;
+
+ m_sinTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
+ SEND_MSG("Set initial point of sinusoid to "+toString(m_sinTrajGen[i]->getPos()),MSG_TYPE_DEBUG);
+ m_sinTrajGen[i]->set_final_point(qFinal);
+ m_sinTrajGen[i]->set_trajectory_time(time);
+ m_status[i] = JTG_SINUSOID;
+ m_currentTrajGen[i] = m_sinTrajGen[i];
+ }
+
+ void JointTrajectoryGenerator::startTriangle(const std::string& jointName, const double& qFinal, const double& time, const double& Tacc)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot start triangle before initialization!",MSG_TYPE_ERROR);
+
+ unsigned int i;
+ if(convertJointNameToJointId(jointName,i)==false)
+ return;
+ if(m_status[i]!=JTG_STOP)
+ return SEND_MSG("You cannot move the specified joint because it is already controlled.", MSG_TYPE_ERROR);
+ if(!isJointInRange(i, qFinal))
+ return;
+
+ m_triangleTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
+ SEND_MSG("Set initial point of triangular trajectory to "+toString(m_triangleTrajGen[i]->getPos()),MSG_TYPE_DEBUG);
+ m_triangleTrajGen[i]->set_final_point(qFinal);
+
+ if(!m_triangleTrajGen[i]->set_trajectory_time(time))
+ return SEND_MSG("Trajectory time cannot be negative.", MSG_TYPE_ERROR);
+
+ if(!m_triangleTrajGen[i]->set_acceleration_time(Tacc))
+ return SEND_MSG("Acceleration time cannot be negative or larger than half the trajectory time.", MSG_TYPE_ERROR);
+
+ m_status[i] = JTG_TRIANGLE;
+ m_currentTrajGen[i] = m_triangleTrajGen[i];
+ }
+
+ void JointTrajectoryGenerator::startConstAcc(const std::string& jointName, const double& qFinal, const double& time)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot start constant-acceleration trajectory before initialization!",MSG_TYPE_ERROR);
+
+ unsigned int i;
+ if(convertJointNameToJointId(jointName,i)==false)
+ return;
+ if(time<=0.0)
+ return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
+ if(m_status[i]!=JTG_STOP)
+ return SEND_MSG("You cannot move the specified joint because it is already controlled.", MSG_TYPE_ERROR);
+ if(!isJointInRange(i, qFinal))
+ return;
+
+ m_constAccTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
+ SEND_MSG("Set initial point of const-acc trajectory to "+toString(m_constAccTrajGen[i]->getPos()),MSG_TYPE_DEBUG);
+ m_constAccTrajGen[i]->set_final_point(qFinal);
+ m_constAccTrajGen[i]->set_trajectory_time(time);
+ m_status[i] = JTG_CONST_ACC;
+ m_currentTrajGen[i] = m_constAccTrajGen[i];
+ }
+
+ void JointTrajectoryGenerator::startForceSinusoid(const std::string& forceName, const int& axis, const double& fFinal, const double& time)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot start force sinusoid before initialization!",MSG_TYPE_ERROR);
+
+ unsigned int i;
+ if(convertForceNameToForceId(forceName,i)==false)
+ return;
+ if(time<=0.0)
+ return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
+ if(axis<0 || axis>5)
+ return SEND_MSG("Axis must be between 0 and 5", MSG_TYPE_ERROR);
+ if(m_status_force[i]!=JTG_STOP)
+ return SEND_MSG("You cannot move the specified force because it is already controlled.", MSG_TYPE_ERROR);
+// EIGEN_CONST_VECTOR_FROM_SIGNAL(fFinal_eig, fFinal);
+ if(!isForceInRange(i, axis, fFinal))
+ return;
+
+ VectorXd currentF = m_noTrajGen_force[i]->getPos();
+ m_sinTrajGen_force[i]->set_initial_point(currentF);
+ SEND_MSG("Set initial point of sinusoid to "+toString(m_sinTrajGen_force[i]->getPos()),MSG_TYPE_DEBUG);
+ currentF[axis] = fFinal;
+ m_sinTrajGen_force[i]->set_final_point(currentF);
+ m_sinTrajGen_force[i]->set_trajectory_time(time);
+ m_status_force[i] = JTG_SINUSOID;
+ m_currentTrajGen_force[i] = m_sinTrajGen_force[i];
+ }
+
+ void JointTrajectoryGenerator::startLinearChirp(const string& jointName, const double& qFinal, const double& f0, const double& f1, const double& time)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot start linear chirp before initialization!",MSG_TYPE_ERROR);
+
+ unsigned int i;
+ if(convertJointNameToJointId(jointName,i)==false)
+ return;
+ if(time<=0.0)
+ return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
+ if(m_status[i]!=JTG_STOP)
+ return SEND_MSG("You cannot move the specified joint because it is already controlled.", MSG_TYPE_ERROR);
+ if(!isJointInRange(i, qFinal))
+ return;
+ if(f0>f1)
+ return SEND_MSG("f0 "+toString(f0)+" cannot to be more than f1 "+toString(f1),MSG_TYPE_ERROR);
+ if(f0<=0.0)
+ return SEND_MSG("Frequency has to be positive "+toString(f0),MSG_TYPE_ERROR);
+
+ if(!m_linChirpTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos()))
+ return SEND_MSG("Error while setting initial point "+toString(m_noTrajGen[i]->getPos()), MSG_TYPE_ERROR);
+ if(!m_linChirpTrajGen[i]->set_final_point(qFinal))
+ return SEND_MSG("Error while setting final point "+toString(qFinal), MSG_TYPE_ERROR);
+ if(!m_linChirpTrajGen[i]->set_trajectory_time(time))
+ return SEND_MSG("Error while setting trajectory time "+toString(time), MSG_TYPE_ERROR);
+ if(!m_linChirpTrajGen[i]->set_initial_frequency(f0))
+ return SEND_MSG("Error while setting initial frequency "+toString(f0), MSG_TYPE_ERROR);
+ if(!m_linChirpTrajGen[i]->set_final_frequency(f1))
+ return SEND_MSG("Error while setting final frequency "+toString(f1), MSG_TYPE_ERROR);
+ m_status[i] = JTG_LIN_CHIRP;
+ m_currentTrajGen[i] = m_linChirpTrajGen[i];
+ }
+
+ void JointTrajectoryGenerator::startForceLinearChirp(const string& forceName, const int& axis, const double& fFinal, const double& f0, const double& f1, const double& time)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot start force linear chirp before initialization!",MSG_TYPE_ERROR);
+
+ unsigned int i;
+ if(convertForceNameToForceId(forceName,i)==false)
+ return;
+ if(time<=0.0)
+ return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
+ if(m_status_force[i]!=JTG_STOP)
+ return SEND_MSG("You cannot move the specified force because it is already controlled.", MSG_TYPE_ERROR);
+ if(!isForceInRange(i, axis, fFinal))
+ return;
+ if(f0>f1)
+ return SEND_MSG("f0 "+toString(f0)+" cannot to be more than f1 "+toString(f1),MSG_TYPE_ERROR);
+ if(f0<=0.0)
+ return SEND_MSG("Frequency has to be positive "+toString(f0),MSG_TYPE_ERROR);
+
+ VectorXd currentF = m_noTrajGen_force[i]->getPos();
+ if(!m_linChirpTrajGen_force[i]->set_initial_point(currentF))
+ return SEND_MSG("Error while setting initial point "+toString(m_noTrajGen_force[i]->getPos()), MSG_TYPE_ERROR);
+ currentF[axis] = fFinal;
+ if(!m_linChirpTrajGen_force[i]->set_final_point(currentF))
+ return SEND_MSG("Error while setting final point "+toString(fFinal), MSG_TYPE_ERROR);
+ if(!m_linChirpTrajGen_force[i]->set_trajectory_time(time))
+ return SEND_MSG("Error while setting trajectory time "+toString(time), MSG_TYPE_ERROR);
+ if(!m_linChirpTrajGen_force[i]->set_initial_frequency(Vector6d::Constant(f0)))
+ return SEND_MSG("Error while setting initial frequency "+toString(f0), MSG_TYPE_ERROR);
+ if(!m_linChirpTrajGen_force[i]->set_final_frequency(Vector6d::Constant(f1)))
+ return SEND_MSG("Error while setting final frequency "+toString(f1), MSG_TYPE_ERROR);
+ m_status_force[i] = JTG_LIN_CHIRP;
+ m_currentTrajGen_force[i] = m_linChirpTrajGen_force[i];
+ }
+
+ void JointTrajectoryGenerator::moveJoint(const string& jointName, const double& qFinal, const double& time)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot move joint before initialization!",MSG_TYPE_ERROR);
+
+ unsigned int i;
+ if(convertJointNameToJointId(jointName,i)==false)
+ return;
+ if(time<=0.0)
+ return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
+ if(m_status[i]!=JTG_STOP)
+ return SEND_MSG("You cannot move the specified joint because it is already controlled.", MSG_TYPE_ERROR);
+ if(!isJointInRange(i, qFinal))
+ return;
+
+ m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
+ m_minJerkTrajGen[i]->set_final_point(qFinal);
+ m_minJerkTrajGen[i]->set_trajectory_time(time);
+ m_status[i] = JTG_MIN_JERK;
+ m_currentTrajGen[i] = m_minJerkTrajGen[i];
+ }
+
+ void JointTrajectoryGenerator::moveForce(const string& forceName, const int& axis, const double& fFinal, const double& time)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot move force before initialization!",MSG_TYPE_ERROR);
+
+ unsigned int i;
+ if(convertForceNameToForceId(forceName,i)==false)
+ return;
+ if(time<=0.0)
+ return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
+ if(m_status_force[i]!=JTG_STOP)
+ return SEND_MSG("You cannot move the specified force because it is already controlled.", MSG_TYPE_ERROR);
+ if(!isForceInRange(i, axis, fFinal))
+ return;
+
+ VectorXd currentF = m_noTrajGen_force[i]->getPos();
+ m_minJerkTrajGen_force[i]->set_initial_point(currentF);
+ currentF[axis] = fFinal;
+ m_minJerkTrajGen_force[i]->set_final_point(currentF);
+ m_minJerkTrajGen_force[i]->set_trajectory_time(time);
+ m_status_force[i] = JTG_MIN_JERK;
+ m_currentTrajGen_force[i] = m_minJerkTrajGen_force[i];
+ }
+
+ void JointTrajectoryGenerator::stop(const std::string& jointName)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot stop joint before initialization!",MSG_TYPE_ERROR);
+
+ const dynamicgraph::Vector& base6d_encoders = m_base6d_encodersSIN.accessCopy();
+ if(jointName=="all")
+ {
+ for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
+ {
+ m_status[i] = JTG_STOP;
+ // update the initial position
+ m_noTrajGen[i]->set_initial_point(base6d_encoders(6+i));
+ m_currentTrajGen[i] = m_noTrajGen[i];
+ }
+ return;
+ }
+
+ unsigned int i;
+ if(convertJointNameToJointId(jointName,i)==false)
+ return;
+ m_noTrajGen[i]->set_initial_point(base6d_encoders(6+i)); // update the initial position
+ m_status[i] = JTG_STOP;
+ m_currentTrajGen[i] = m_noTrajGen[i];
+ }
+
+ void JointTrajectoryGenerator::stopForce(const std::string& forceName)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot stop force before initialization!",MSG_TYPE_ERROR);
+
+ unsigned int i;
+ if(convertForceNameToForceId(forceName,i)==false)
+ return;
+ m_noTrajGen_force[i]->set_initial_point(m_currentTrajGen_force[i]->getPos()); // update the initial position
+ m_status_force[i] = JTG_STOP;
+ m_currentTrajGen_force[i] = m_noTrajGen_force[i];
+ }
+
+
+ /* ------------------------------------------------------------------- */
+ // ************************ PROTECTED MEMBER METHODS ********************
+ /* ------------------------------------------------------------------- */
+
+
+ bool JointTrajectoryGenerator::
+ convertJointNameToJointId(const std::string& name, unsigned int& id)
+ {
+ // Check if the joint name exists
+ int jid = m_robot_util->get_id_from_name(name);
+ if (jid<0)
+ {
+ SEND_MSG("The specified joint name does not exist", MSG_TYPE_ERROR);
+ std::stringstream ss;
+ for(map<string, Index>::const_iterator it =
+ m_robot_util->m_name_to_id.begin();
+ it != m_robot_util->m_name_to_id.end(); it++)
+ ss<<it->first<<", ";
+ SEND_MSG("Possible joint names are: "+ss.str(), MSG_TYPE_INFO);
+ return false;
+ }
+ id = jid;
+ return true;
+ }
+
+ bool JointTrajectoryGenerator::
+ convertForceNameToForceId(const std::string& name, unsigned int& id)
+ {
+ // Check if the joint name exists
+ Index fid = m_robot_util->m_force_util.get_id_from_name(name);
+ if (fid<0)
+ {
+ SEND_MSG("The specified force name does not exist", MSG_TYPE_ERROR);
+ std::stringstream ss;
+ for(map<string, Index>::const_iterator
+ it = m_robot_util->m_force_util.m_name_to_force_id.begin();
+ it != m_robot_util->m_force_util.m_name_to_force_id.end(); it++)
+ ss<<it->first<<", ";
+ SEND_MSG("Possible force names are: "+ss.str(), MSG_TYPE_INFO);
+ return false;
+ }
+ id = (unsigned int)fid;
+ return true;
+ }
+
+ bool JointTrajectoryGenerator::isJointInRange(unsigned int id, double q)
+ {
+ JointLimits jl = m_robot_util->get_joint_limits_from_id(id);
+ if(q<jl.lower)
+ {
+ SEND_MSG("Joint "+m_robot_util->get_name_from_id(id)+", desired angle "+toString(q)+" is smaller than lower limit "+toString(jl.lower),MSG_TYPE_ERROR);
+ return false;
+ }
+ if(q>jl.upper)
+ {
+ SEND_MSG("Joint "+m_robot_util->get_name_from_id(id)+", desired angle "+toString(q)+" is larger than upper limit "+toString(jl.upper),MSG_TYPE_ERROR);
+ return false;
+ }
+ return true;
+ }
+
+ bool JointTrajectoryGenerator::isForceInRange(unsigned int id, const Eigen::VectorXd& f)
+ {
+ ForceLimits fl = m_robot_util->m_force_util.get_limits_from_id(id);
+ for(unsigned int i=0; i<6; i++)
+ if(f[i]<fl.lower[i] || f[i]>fl.upper[i])
+ {
+ SEND_MSG("Desired force "+toString(i)+" is out of range: "+toString(fl.lower[i])+" < "+
+ toString(f[i])+" < "+toString(fl.upper[i]),MSG_TYPE_ERROR);
+ return false;
+ }
+ return true;
+ }
+
+ bool JointTrajectoryGenerator::isForceInRange(unsigned int id, int axis, double f)
+ {
+ ForceLimits fl = m_robot_util->m_force_util.get_limits_from_id(id);
+ if(f<fl.lower[axis] || f>fl.upper[axis])
+ {
+ SEND_MSG("Desired force "+toString(axis)+" is out of range: "+toString(fl.lower[axis])+" < "+
+ toString(f)+" < "+toString(fl.upper[axis]),MSG_TYPE_ERROR);
+ return false;
+ }
+ return true;
+ }
+
+
+ /* ------------------------------------------------------------------- */
+ /* --- ENTITY -------------------------------------------------------- */
+ /* ------------------------------------------------------------------- */
+
+ void JointTrajectoryGenerator::display(std::ostream& os) const
+ {
+ os << "JointTrajectoryGenerator "<<getName();
+ try
+ {
+ getProfiler().report_all(3, os);
+ }
+ catch (ExceptionSignal e) {}
+ }
+ } // namespace talos_balance
+ } // namespace sot
+} // namespace dynamicgraph
diff --git a/src/nd-trajectory-generator.cpp b/src/nd-trajectory-generator.cpp
new file mode 100644
index 0000000..3b84bff
--- /dev/null
+++ b/src/nd-trajectory-generator.cpp
@@ -0,0 +1,643 @@
+/*
+ * Copyright 2017,Thomas Flayols, LAAS-CNRS
+ * 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.
+ * 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-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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <sot/talos_balance/nd-trajectory-generator.hh>
+#include <sot/core/debug.hh>
+#include <dynamic-graph/factory.h>
+
+#include <sot/talos_balance/utils/commands-helper.hh>
+#include <sot/talos_balance/utils/stop-watch.hh>
+
+namespace dynamicgraph
+{
+ namespace sot
+ {
+ namespace talos_balance
+ {
+ namespace dynamicgraph = ::dynamicgraph;
+ using namespace dynamicgraph;
+ using namespace dynamicgraph::command;
+ using namespace std;
+ using namespace Eigen;
+
+#define PROFILE_ND_POSITION_DESIRED_COMPUTATION "NdTrajGen: traj computation"
+#define DOUBLE_INF std::numeric_limits<double>::max()
+ /// Define EntityClassName here rather than in the header file
+ /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
+ typedef NdTrajectoryGenerator EntityClassName;
+
+ /* --- DG FACTORY ---------------------------------------------------- */
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(NdTrajectoryGenerator,
+ "NdTrajectoryGenerator");
+
+ /* ------------------------------------------------------------------- */
+ /* --- CONSTRUCTION -------------------------------------------------- */
+ /* ------------------------------------------------------------------- */
+ NdTrajectoryGenerator::
+ NdTrajectoryGenerator(const std::string& name)
+ : Entity(name)
+ ,CONSTRUCT_SIGNAL_IN(initial_value,dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL_IN(trigger,bool)
+ ,CONSTRUCT_SIGNAL(x, OUT, dynamicgraph::Vector)
+ ,CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_xSOUT)
+ ,CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_xSOUT)
+ ,m_firstIter(true)
+ ,m_splineReady(false)
+ ,m_initSucceeded(false)
+ ,m_n(1)
+ ,m_t(0)
+ ,m_iterLast(0)
+ {
+ BIND_SIGNAL_TO_FUNCTION(x, OUT, dynamicgraph::Vector);
+
+ Entity::signalRegistration( m_xSOUT << m_dxSOUT << m_ddxSOUT << m_initial_valueSIN
+ <<m_triggerSIN);
+
+ /* Commands. */
+ addCommand("init",
+ makeCommandVoid2(*this, &NdTrajectoryGenerator::init,
+ docCommandVoid2("Initialize the entity.",
+ "Time period in seconds (double)",
+ "size of output vector signal (int)")));
+
+ addCommand("getValue",
+ makeCommandVoid1(*this, &NdTrajectoryGenerator::getValue,
+ docCommandVoid1("Get the current value of the specified index.",
+ "index (int)")));
+
+ addCommand("playTrajectoryFile",
+ makeCommandVoid1(*this, &NdTrajectoryGenerator::playTrajectoryFile,
+ docCommandVoid1("Play the trajectory read from the specified text file.",
+ "(string) File name, path included")));
+
+ addCommand("startSinusoid",
+ makeCommandVoid3(*this, &NdTrajectoryGenerator::startSinusoid,
+ docCommandVoid3("Start an infinite sinusoid motion.",
+ "(int) index",
+ "(double) final value",
+ "(double) time to reach the final value in sec")));
+
+ addCommand("setSpline",
+ makeCommandVoid2(*this, &NdTrajectoryGenerator::setSpline,
+ docCommandVoid2("Load serialized spline from file",
+ "(string) filename",
+ "(double) time to initial conf")));
+
+ /* addCommand("startTriangle",
+ makeCommandVoid4(*this, &NdTrajectoryGenerator::startTriangle,
+ docCommandVoid4("Start an infinite triangle wave.",
+ "(int) index",
+ "(double) final values",
+ "(double) time to reach the final value in sec",
+ "(double) time to accelerate in sec")));
+ */
+ addCommand("startConstAcc",
+ makeCommandVoid3(*this, &NdTrajectoryGenerator::startConstAcc,
+ docCommandVoid3("Start an infinite trajectory with piece-wise constant acceleration.",
+ "(int) index",
+ "(double) final values",
+ "(double) time to reach the final value in sec")));
+
+ addCommand("startLinChirp",
+ makeCommandVoid5(*this, &NdTrajectoryGenerator::startLinearChirp,
+ docCommandVoid5("Start a linear-chirp motion.",
+ "(int) index",
+ "(double) final values",
+ "(double) initial frequency [Hz]",
+ "(double) final frequency [Hz]",
+ "(double) trajectory time [sec]")));
+ addCommand("move",
+ makeCommandVoid3(*this, &NdTrajectoryGenerator::move,
+ docCommandVoid3("Move component corresponding to index to the specified value with a minimum jerk trajectory.",
+ "(int) index",
+ "(double) final values",
+ "(double) time to reach the final value in sec")));
+ addCommand("stop",
+ makeCommandVoid1(*this, &NdTrajectoryGenerator::stop,
+ docCommandVoid1("Stop the motion of the specified index, or of all components of the vector if index is equal to -1.",
+ "(int) index")));
+
+ }
+
+ void NdTrajectoryGenerator::init(const double& dt, const unsigned int& n)
+ {
+ if(dt<=0.0)
+ return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
+ if(n<1)
+ return SEND_MSG("n must be at least 1", MSG_TYPE_ERROR);
+ m_firstIter = true;
+ m_dt = dt;
+ m_n = n;
+ m_status.resize(m_n,JTG_STOP);
+ m_minJerkTrajGen.resize(m_n);
+ m_sinTrajGen.resize(m_n);
+ //m_triangleTrajGen.resize(m_n);
+ m_constAccTrajGen.resize(m_n);
+ m_linChirpTrajGen.resize(m_n);
+ m_currentTrajGen.resize(m_n);
+ m_noTrajGen.resize(m_n);
+ for(int i=0; i<m_n; i++)
+ {
+ m_minJerkTrajGen[i] = new parametriccurves::MinimumJerk<double,1>(5.0);
+ m_sinTrajGen[i] = new parametriccurves::InfiniteSinusoid<double,1>(5.0);
+ //m_triangleTrajGen[i] = new parametriccurves::InfiniteTriangle<double,1>(5.0);
+ m_constAccTrajGen[i] = new parametriccurves::InfiniteConstAcc<double,1>(5.0);
+ m_linChirpTrajGen[i] = new parametriccurves::LinearChirp<double,1>(5.0);
+ m_noTrajGen[i] = new parametriccurves::Constant<double,1>(5.0);
+ m_currentTrajGen[i] = m_noTrajGen[i];
+
+ //Set infinite time for infinite trajectories
+ m_noTrajGen[i]->setTimePeriod(DOUBLE_INF);
+ m_constAccTrajGen[i]->setTimePeriod(DOUBLE_INF);
+ m_sinTrajGen[i]->setTimePeriod(DOUBLE_INF);
+ }
+ m_splineTrajGen = new parametriccurves::Spline<double,Eigen::Dynamic>();
+ m_textFileTrajGen = new parametriccurves::TextFile<double, Eigen::Dynamic>(dt, n);
+ m_initSucceeded = true;
+ }
+
+ /* ------------------------------------------------------------------- */
+ /* --- SIGNALS ------------------------------------------------------- */
+ /* ------------------------------------------------------------------- */
+
+ DEFINE_SIGNAL_OUT_FUNCTION(x, dynamicgraph::Vector)
+ {
+ if(!m_initSucceeded)
+ {
+ SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
+ return s;
+ }
+
+ getProfiler().start(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
+ {
+ if(s.size()!=m_n)
+ s.resize(m_n);
+
+ // at first iteration store initial values
+ if(m_firstIter)
+ {
+ const dynamicgraph::Vector& initial_value = m_initial_valueSIN(iter);
+ if(initial_value.size()!=m_n)
+ {
+ SEND_MSG("Unexpected size of input signal initial_value: "+toString(initial_value.size()),MSG_TYPE_ERROR);
+ getProfiler().stop(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
+ return s;
+ }
+ for(unsigned int i=0; i<m_n; i++)
+ m_currentTrajGen[i]->setInitialPoint(initial_value(i));
+ m_firstIter = false;
+ }
+ else if(iter == m_iterLast)
+ {
+ if (m_triggerSIN(iter)==true && m_splineReady) startSpline();
+ if(m_status[0]==JTG_TEXT_FILE)
+ {
+ s = (*m_textFileTrajGen)(m_t);
+ }
+ else if(m_status[0]==JTG_SPLINE)
+ {
+ s = (*m_splineTrajGen)(m_t);
+ }
+ else
+ for(unsigned int i=0; i<m_n; i++)
+ s(i) = (*m_currentTrajGen[i])(m_t)[0];
+ getProfiler().stop(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
+ return s;
+ }
+ m_iterLast = iter;
+ m_t += m_dt;
+
+ if (m_triggerSIN(iter)==true && m_splineReady) startSpline();
+ if(m_status[0]==JTG_TEXT_FILE)
+ {
+ if(!m_textFileTrajGen->checkRange(m_t))
+ {
+ s = (*m_textFileTrajGen)(m_textFileTrajGen->tmax());
+ for(unsigned int i=0; i<m_n; i++)
+ {
+ m_noTrajGen[i]->setInitialPoint(s(i));
+ m_status[i] = JTG_STOP;
+ }
+ SEND_MSG("Text file trajectory ended.", MSG_TYPE_INFO);
+ m_t =0;
+ }
+ else
+ s = (*m_textFileTrajGen)(m_t);
+ }
+ else if(m_status[0]==JTG_SPLINE)
+ {
+ if(!m_splineTrajGen->checkRange(m_t))
+ {
+ s = (*m_splineTrajGen)(m_splineTrajGen->tmax());
+ for(unsigned int i=0; i<m_n; i++)
+ {
+ m_noTrajGen[i]->setInitialPoint(s(i));
+ m_status[i] = JTG_STOP;
+ }
+ m_splineReady =false;
+ SEND_MSG("Spline trajectory ended. Remember to turn off the trigger.", MSG_TYPE_INFO);
+ m_t =0;
+ }
+ else
+ s = (*m_splineTrajGen)(m_t);
+ }
+ else
+ {
+ for(unsigned int i=0; i<m_n; i++)
+ {
+ if(!m_currentTrajGen[i]->checkRange(m_t))
+ {
+ s(i) = (*m_currentTrajGen[i])(m_currentTrajGen[i]->tmax())[0];
+ m_currentTrajGen[i] = m_noTrajGen[i];
+ m_noTrajGen[i]->setInitialPoint(s(i));
+ m_status[i] = JTG_STOP;
+ SEND_MSG("Trajectory of index "+toString(i)+" ended.", MSG_TYPE_INFO);
+ }
+ else
+ s(i) = (*m_currentTrajGen[i])(m_t)[0];
+ }
+ }
+ }
+ getProfiler().stop(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
+
+ return s;
+ }
+
+ DEFINE_SIGNAL_OUT_FUNCTION(dx, dynamicgraph::Vector)
+ {
+ if(!m_initSucceeded)
+ {
+ SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
+ return s;
+ }
+
+ const dynamicgraph::Vector& x = m_xSOUT(iter);
+
+ if(s.size()!=m_n)
+ s.resize(m_n);
+ if(m_status[0]==JTG_TEXT_FILE)
+ {
+ s = m_textFileTrajGen->derivate(m_t, 1);
+ }
+ else if(m_status[0]==JTG_SPLINE)
+ s = m_splineTrajGen->derivate(m_t, 1);
+ else
+ for(unsigned int i=0; i<m_n; i++)
+ s(i) = m_currentTrajGen[i]->derivate(m_t, 1)[0];
+
+ return s;
+ }
+
+ DEFINE_SIGNAL_OUT_FUNCTION(ddx, dynamicgraph::Vector)
+ {
+ if(!m_initSucceeded)
+ {
+ SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
+ return s;
+ }
+
+ const dynamicgraph::Vector& x = m_xSOUT(iter);
+
+ if(s.size()!=m_n)
+ s.resize(m_n);
+
+ if(m_status[0]==JTG_TEXT_FILE)
+ {
+ s = m_textFileTrajGen->derivate(m_t, 2);
+ }
+ else if(m_status[0]==JTG_SPLINE)
+ s = m_splineTrajGen->derivate(m_t, 2);
+ else
+ for(unsigned int i=0; i<m_n; i++)
+ s(i) = m_currentTrajGen[i]->derivate(m_t, 2)[0];
+
+ return s;
+ }
+
+ /* ------------------------------------------------------------------- */
+ /* --- COMMANDS ------------------------------------------------------ */
+ /* ------------------------------------------------------------------- */
+
+ void NdTrajectoryGenerator::getValue(const int& id)
+ {
+ if(id<0 || id>=m_n)
+ return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
+
+ SEND_MSG("Current value of component "+toString(id)+" is "+toString( (*m_currentTrajGen[id])(m_t)[0]) , MSG_TYPE_INFO);
+ }
+
+ void NdTrajectoryGenerator::playTrajectoryFile(const std::string& fileName)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot start sinusoid before initialization!",MSG_TYPE_ERROR);
+
+ for(unsigned int i=0; i<m_n; i++)
+ if(m_status[i]!=JTG_STOP)
+ return SEND_MSG("You cannot control component "+toString(i)+" because it is already controlled.", MSG_TYPE_ERROR);
+
+ if(!m_textFileTrajGen->loadTextFile(fileName))
+ return SEND_MSG("Error while loading text file "+fileName, MSG_TYPE_ERROR);
+
+ // check current configuration is not too far from initial trajectory configuration
+ bool needToMoveToInitConf = false;
+ const VectorXd& xInit = m_textFileTrajGen->getInitialPoint();
+ for(unsigned int i=0; i<m_n; i++)
+ if(fabs(xInit[i] - (*m_currentTrajGen[i])(m_t)[0]) > 0.001)
+ {
+ needToMoveToInitConf = true;
+ SEND_MSG("Component "+ toString(i) +" is too far from initial configuration so first i will move it there.", MSG_TYPE_WARNING);
+ }
+ // if necessary move joints to initial configuration
+ if(needToMoveToInitConf)
+ {
+ for(unsigned int i=0; i<m_n; i++)
+ {
+// if(!isJointInRange(i, xInit[i]))
+// return;
+
+ m_minJerkTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
+ m_minJerkTrajGen[i]->setFinalPoint(xInit[i]);
+ m_minJerkTrajGen[i]->setTimePeriod(4.0);
+ m_status[i] = JTG_MIN_JERK;
+ m_currentTrajGen[i] = m_minJerkTrajGen[i];
+ }
+ m_t = 0.0;
+ return;
+ }
+
+ m_t = 0.0;
+ for(unsigned int i=0; i<m_n; i++)
+ {
+ m_status[i] = JTG_TEXT_FILE;
+ }
+ }
+
+ void NdTrajectoryGenerator::setSpline(const std::string& filename, const double& timeToInitConf)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot start spline before initialization!",MSG_TYPE_ERROR);
+
+ for(unsigned int i=0; i<m_n; i++)
+ if(m_status[i]!=JTG_STOP)
+ return SEND_MSG("You cannot control component " +toString(i)+" because it is already controlled.",MSG_TYPE_ERROR);
+
+ if(!m_splineTrajGen->loadFromFile(filename))
+ return SEND_MSG("Error while loading spline"+filename, MSG_TYPE_ERROR);
+
+ SEND_MSG("Spline set to "+filename+". Now checking initial position", MSG_TYPE_INFO);
+
+ bool needToMoveToInitConf = false;
+ if(timeToInitConf < 0)
+ {
+ m_splineReady = true;
+ SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
+ return;
+ }
+
+ // check current configuration is not too far from initial configuration
+ const VectorXd& xInit = (*m_splineTrajGen)(0.0);
+ assert(xInit.size() == m_n);
+ for(unsigned int i=0; i<m_n; i++)
+ if(fabs(xInit[i] - (*m_currentTrajGen[i])(m_t)[0]) > 0.001)
+ {
+ needToMoveToInitConf = true;
+ SEND_MSG("Component "+ toString(i) +" is too far from initial configuration so first i will move it there.", MSG_TYPE_WARNING);
+ }
+ // if necessary move joints to initial configuration
+ if(needToMoveToInitConf)
+ {
+ for(unsigned int i=0; i<m_n; i++)
+ {
+ m_minJerkTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
+ m_minJerkTrajGen[i]->setFinalPoint(xInit[i]);
+ m_minJerkTrajGen[i]->setTimePeriod(timeToInitConf);
+ m_status[i] = JTG_MIN_JERK;
+ m_currentTrajGen[i] = m_minJerkTrajGen[i];
+// SEND_MSG("MinimumJerk trajectory for index "+ toString(i) +" to go to final position" + toString(xInit[i]), MSG_TYPE_WARNING);
+ }
+ m_t = 0.0;
+ m_splineReady = true;
+ return;
+ }
+ m_splineReady = true;
+ SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
+ }
+
+ void NdTrajectoryGenerator::startSpline()
+ {
+ if(m_status[0]==JTG_SPLINE) return;
+ m_t = 0.0;
+ for(unsigned int i=0; i<m_n; i++)
+ {
+ m_status[i] = JTG_SPLINE;
+ }
+ }
+
+ void NdTrajectoryGenerator::startSinusoid(const int& id, const double& xFinal, const double& time)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot start sinusoid before initialization!",MSG_TYPE_ERROR);
+
+ if(id<0 || id>=m_n)
+ return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
+ unsigned int i = id;
+ if(time<=0.0)
+ return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
+ if(m_status[i]!=JTG_STOP)
+ return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
+// if(!isJointInRange(i, xFinal))
+// return;
+
+ m_sinTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
+ m_sinTrajGen[i]->setFinalPoint(xFinal);
+ m_sinTrajGen[i]->setTrajectoryTime(time);
+ SEND_MSG("Set initial point of sinusoid to "+toString((*m_noTrajGen[i])(m_t)[0]),MSG_TYPE_DEBUG);
+ m_status[i] = JTG_SINUSOID;
+ m_currentTrajGen[i] = m_sinTrajGen[i];
+ m_t = 0.0;
+ }
+ /*
+ void NdTrajectoryGenerator::startTriangle(const int& id, const double& xFinal, const double& time, const double& Tacc)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot start triangle before initialization!",MSG_TYPE_ERROR);
+ if(id<0 || id>=m_n)
+ return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
+ unsigned int i = id;
+ if(m_status[i]!=JTG_STOP)
+ return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
+// if(!isJointInRange(i, xFinal))
+// return;
+
+ m_triangleTrajGen[i]->setInitialPoint(m_noTrajGen[i]->getPos());
+ SEND_MSG("Set initial point of triangular trajectory to "+toString(m_triangleTrajGen[i]->getPos()),MSG_TYPE_DEBUG);
+ m_triangleTrajGen[i]->setFinalPoint(xFinal);
+
+ if(!m_triangleTrajGen[i]->setTimePeriod(time))
+ return SEND_MSG("Trajectory time cannot be negative.", MSG_TYPE_ERROR);
+
+ if(!m_triangleTrajGen[i]->set_acceleration_time(Tacc))
+ return SEND_MSG("Acceleration time cannot be negative or larger than half the trajectory time.", MSG_TYPE_ERROR);
+
+ m_status[i] = JTG_TRIANGLE;
+ m_currentTrajGen[i] = m_triangleTrajGen[i];
+ m_t = 0.0;
+ }
+ */
+ void NdTrajectoryGenerator::startConstAcc(const int& id, const double& xFinal, const double& time)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot start constant-acceleration trajectory before initialization!",MSG_TYPE_ERROR);
+ if(id<0 || id>=m_n)
+ return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
+ unsigned int i = id;
+ if(time<=0.0)
+ return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
+ if(m_status[i]!=JTG_STOP)
+ return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
+// if(!isJointInRange(i, xFinal))
+// return;
+
+ m_constAccTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
+ SEND_MSG("Set initial point of const-acc trajectory to "+toString((*m_noTrajGen[i])(m_t)[0]),MSG_TYPE_DEBUG);
+ m_constAccTrajGen[i]->setFinalPoint(xFinal);
+ m_constAccTrajGen[i]->setTrajectoryTime(time);
+ m_status[i] = JTG_CONST_ACC;
+ m_currentTrajGen[i] = m_constAccTrajGen[i];
+ m_t = 0.0;
+ }
+ void NdTrajectoryGenerator::startLinearChirp(const int& id, const double& xFinal, const double& f0, const double& f1, const double& time)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot start linear chirp before initialization!",MSG_TYPE_ERROR);
+ if(id<0 || id>=m_n)
+ return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
+ unsigned int i = id;
+ if(time<=0.0)
+ return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
+ if(m_status[i]!=JTG_STOP)
+ return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
+// if(!isJointInRange(i, xFinal))
+// return;
+ if(f0>f1)
+ return SEND_MSG("f0 "+toString(f0)+" cannot to be more than f1 "+toString(f1),MSG_TYPE_ERROR);
+ if(f0<=0.0)
+ return SEND_MSG("Frequency has to be positive "+toString(f0),MSG_TYPE_ERROR);
+
+ if(!m_linChirpTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]))
+ return SEND_MSG("Error while setting initial point "+toString((*m_noTrajGen[i])(m_t)[0]), MSG_TYPE_ERROR);
+ if(!m_linChirpTrajGen[i]->setFinalPoint(xFinal))
+ return SEND_MSG("Error while setting final point "+toString(xFinal), MSG_TYPE_ERROR);
+ if(!m_linChirpTrajGen[i]->setTimePeriod(time))
+ return SEND_MSG("Error while setting trajectory time "+toString(time), MSG_TYPE_ERROR);
+ if(!m_linChirpTrajGen[i]->setInitialFrequency(f0))
+ return SEND_MSG("Error while setting initial frequency "+toString(f0), MSG_TYPE_ERROR);
+ if(!m_linChirpTrajGen[i]->setFinalFrequency(f1))
+ return SEND_MSG("Error while setting final frequency "+toString(f1), MSG_TYPE_ERROR);
+ m_status[i] = JTG_LIN_CHIRP;
+ m_currentTrajGen[i] = m_linChirpTrajGen[i];
+ m_t = 0.0;
+ }
+
+ void NdTrajectoryGenerator::move(const int& id, const double& xFinal, const double& time)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot move value before initialization!",MSG_TYPE_ERROR);
+ unsigned int i = id;
+ if(id<0 || id>=m_n)
+ return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
+ if(time<=0.0)
+ return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
+ if(m_status[i]!=JTG_STOP)
+ return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
+// if(!isJointInRange(i, xFinal))
+// return;
+
+ m_minJerkTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
+ m_minJerkTrajGen[i]->setFinalPoint(xFinal);
+ m_minJerkTrajGen[i]->setTimePeriod(time);
+ m_status[i] = JTG_MIN_JERK;
+ m_currentTrajGen[i] = m_minJerkTrajGen[i];
+ m_t = 0.0;
+ }
+
+
+ void NdTrajectoryGenerator::stop(const int& id)
+ {
+ if(!m_initSucceeded)
+ return SEND_MSG("Cannot stop value before initialization!",MSG_TYPE_ERROR);
+
+ if(id==-1) //Stop entire vector
+ {
+ for(unsigned int i=0; i<m_n; i++)
+ {
+ m_status[i] = JTG_STOP;
+ // update the initial value
+ m_noTrajGen[i]->setInitialPoint((*m_currentTrajGen[i])(m_t)[0]);
+ m_currentTrajGen[i] = m_noTrajGen[i];
+ }
+ m_t = 0.0;
+ return;
+ }
+ if(id<0 || id>=m_n)
+ return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
+ unsigned int i = id;
+ m_noTrajGen[i]->setInitialPoint((*m_currentTrajGen[i])(m_t)[0]);
+ m_status[i] = JTG_STOP;
+ m_splineReady = false;
+ m_currentTrajGen[i] = m_noTrajGen[i];
+ m_t = 0.0;
+ }
+
+ /* ------------------------------------------------------------------- */
+ // ************************ PROTECTED MEMBER METHODS ********************
+ /* ------------------------------------------------------------------- */
+
+
+// bool NdTrajectoryGenerator::isJointInRange(const int& id, double x)
+// {
+// JointLimits jl = JointUtil::get_limits_from_id(id);
+// if(x<jl.lower)
+// {
+// SEND_MSG("Desired joint angle is smaller than lower limit: "+toString(jl.lower),MSG_TYPE_ERROR);
+// return false;
+// }
+// if(x>jl.upper)
+// {
+// SEND_MSG("Desired joint angle is larger than upper limit: "+toString(jl.upper),MSG_TYPE_ERROR);
+// return false;
+// }
+// return true;
+// }
+
+ /* ------------------------------------------------------------------- */
+ /* --- ENTITY -------------------------------------------------------- */
+ /* ------------------------------------------------------------------- */
+
+ void NdTrajectoryGenerator::display(std::ostream& os) const
+ {
+ os << "NdTrajectoryGenerator "<<getName();
+ try
+ {
+ getProfiler().report_all(3, os);
+ }
+ catch (ExceptionSignal e) {}
+ }
+ } // namespace torquecontrol
+ } // namespace sot
+} // namespace dynamicgraph
diff --git a/src/utils/common.cpp b/src/utils/common.cpp
new file mode 100644
index 0000000..8591b70
--- /dev/null
+++ b/src/utils/common.cpp
@@ -0,0 +1,548 @@
+/*
+ * Copyright 2017, A. Del Prete, T. Flayols, O. Stasse, LAAS-CNRS
+ * 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.
+ * 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-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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <iostream>
+
+#include <sot/talos_balance/utils/common.hh>
+#include <sot/core/debug.hh>
+#include <dynamic-graph/factory.h>
+
+namespace dynamicgraph
+{
+ namespace sot
+ {
+ namespace talos_balance
+ {
+ namespace dg = ::dynamicgraph;
+ using namespace dg;
+ using namespace dg::command;
+
+ RobotUtil VoidRobotUtil;
+
+ RobotUtil * RefVoidRobotUtil()
+ {
+ return & VoidRobotUtil;
+ }
+
+ void ForceLimits::display(std::ostream &os) const
+ {
+ os << "Lower limits:" << std::endl;
+ os << lower << std::endl;
+ os << "Upper Limits:" << std::endl;
+ os << upper << std::endl;
+ }
+
+ /******************** FootUtil ***************************/
+
+ void FootUtil::display(std::ostream &os) const
+ {
+ os << "Right Foot Sole XYZ " << std::endl;
+ os << m_Right_Foot_Sole_XYZ << std::endl;
+ os << "Left Foot Frame Name:" << m_Left_Foot_Frame_Name << std::endl;
+ os << "Right Foot Frame Name:" << m_Right_Foot_Frame_Name << std::endl;
+ }
+
+ /******************** ForceUtil ***************************/
+
+ void ForceUtil::set_name_to_force_id(const std::string &name,
+ const Index &force_id)
+ {
+ m_name_to_force_id[name] = (Index) force_id;
+ create_force_id_to_name_map();
+ if (name=="rf")
+ set_force_id_right_foot(m_name_to_force_id[name]);
+ else if (name=="lf")
+ set_force_id_left_foot(m_name_to_force_id[name]);
+ else if (name=="lh")
+ set_force_id_left_hand(m_name_to_force_id[name]);
+ else if (name=="rh")
+ set_force_id_right_hand(m_name_to_force_id[name]);
+ }
+
+
+ void ForceUtil::set_force_id_to_limits(const Index &force_id,
+ const dg::Vector &lf,
+ const dg::Vector &uf)
+ {
+ m_force_id_to_limits[(Index)force_id] =
+ ForceLimits(lf,uf); // Potential memory leak
+ }
+
+ Index ForceUtil::get_id_from_name(const std::string &name)
+ {
+ std::map<std::string, Index>::const_iterator it;
+ it = m_name_to_force_id.find(name);
+ if (it!=m_name_to_force_id.end())
+ return it->second;
+ return -1;
+ }
+
+ const std::string & ForceUtil::get_name_from_id(Index idx)
+ {
+ std::string default_rtn("Force name not found");
+ std::map<Index,std::string>::const_iterator it;
+ it = m_force_id_to_name.find(idx);
+ if (it!=m_force_id_to_name.end())
+ return it->second;
+ return default_rtn;
+ }
+
+ std::string ForceUtil::cp_get_name_from_id(Index idx)
+ {
+ const std::string & default_rtn = get_name_from_id(idx);
+ return default_rtn;
+ }
+ void ForceUtil::create_force_id_to_name_map()
+ {
+ std::map<std::string, Index>::const_iterator it;
+ for(it = m_name_to_force_id.begin();
+ it != m_name_to_force_id.end(); it++)
+ m_force_id_to_name[it->second] = it->first;
+ }
+
+ const ForceLimits & ForceUtil::get_limits_from_id(Index force_id)
+ {
+ std::map<Index,ForceLimits>::const_iterator iter =
+ m_force_id_to_limits.find(force_id);
+ if(iter==m_force_id_to_limits.end())
+ return ForceLimits(); // Potential memory leak
+ return iter->second;
+ }
+
+ ForceLimits ForceUtil::cp_get_limits_from_id(Index force_id)
+ {
+ std::map<Index,ForceLimits>::const_iterator iter =
+ m_force_id_to_limits.find(force_id);
+ if(iter==m_force_id_to_limits.end())
+ return ForceLimits(); // Potential memory leak
+ return iter->second;
+ }
+
+ void ForceUtil::display(std::ostream &os) const
+ {
+ os << "Force Id to limits "<< std::endl;
+ for( std::map<Index,ForceLimits>::const_iterator
+ it = m_force_id_to_limits.begin();
+ it != m_force_id_to_limits.end();
+ ++it)
+ {
+ it->second.display(os);
+ }
+
+ os << "Name to force id:" << std::endl;
+ for( std::map<std::string,Index>::const_iterator
+ it = m_name_to_force_id.begin();
+ it != m_name_to_force_id.end();
+ ++it)
+ {
+ os << "(" << it->first << "," << it->second << ") ";
+ }
+ os << std::endl;
+
+ os << "Force id to Name:" << std::endl;
+ for( std::map<Index,std::string>::const_iterator
+ it = m_force_id_to_name.begin();
+ it != m_force_id_to_name.end();
+ ++it)
+ {
+ os << "(" << it->first << "," << it->second << ") ";
+ }
+ os << std::endl;
+
+ os << "Index for force sensors:" << std::endl;
+ os << "Left Hand (" <<m_Force_Id_Left_Hand << ") ,"
+ << "Right Hand (" << m_Force_Id_Right_Hand << ") ,"
+ << "Left Foot (" <<m_Force_Id_Left_Foot << ") ,"
+ << "Right Foot (" << m_Force_Id_Right_Foot << ") "
+ << std::endl;
+ }
+
+ /**************** FromURDFToSot *************************/
+ RobotUtil::RobotUtil()
+ {}
+
+ void RobotUtil::
+ set_joint_limits_for_id( const Index &idx,
+ const double &lq,
+ const double &uq)
+ {
+ m_limits_map[(Index)idx] = JointLimits(lq,uq);
+ }
+
+ const JointLimits & RobotUtil::
+ get_joint_limits_from_id(Index id)
+ {
+ std::map<Index,JointLimits>::const_iterator
+ iter = m_limits_map.find(id);
+ if(iter==m_limits_map.end())
+ return JointLimits(0.0,0.0);
+ return iter->second;
+ }
+ JointLimits RobotUtil::
+ cp_get_joint_limits_from_id(Index id)
+ {
+ const JointLimits &rtn = get_joint_limits_from_id(id);
+ return rtn;
+ }
+
+ void RobotUtil::
+ set_name_to_id(const std::string &jointName,
+ const Index &jointId)
+ {
+ m_name_to_id[jointName] = (Index)jointId;
+ create_id_to_name_map();
+ }
+
+ void RobotUtil::
+ create_id_to_name_map()
+ {
+ std::map<std::string, Index>::const_iterator it;
+ for(it = m_name_to_id.begin(); it != m_name_to_id.end(); it++)
+ m_id_to_name[it->second] = it->first;
+ }
+
+ const Index RobotUtil::
+ get_id_from_name(const std::string &name)
+ {
+ std::map<std::string,Index>::const_iterator it =
+ m_name_to_id.find(name);
+ if (it==m_name_to_id.end())
+ return -1;
+ return it->second;
+ }
+
+ const std::string & RobotUtil::
+ get_name_from_id(Index id)
+ {
+ std::map<Index,std::string>::const_iterator iter =
+ m_id_to_name.find(id);
+ if(iter==m_id_to_name.end())
+ return "Joint name not found";
+ return iter->second;
+ }
+
+ void RobotUtil::
+ set_urdf_to_sot(const std::vector<Index> &urdf_to_sot)
+ {
+ m_nbJoints = urdf_to_sot.size();
+ m_urdf_to_sot.resize(urdf_to_sot.size());
+ m_dgv_urdf_to_sot.resize(urdf_to_sot.size());
+ for(std::size_t idx=0;
+ idx<urdf_to_sot.size();idx++)
+ {
+ m_urdf_to_sot[(Index)idx] = urdf_to_sot[(Index)idx];
+ m_dgv_urdf_to_sot[(Index)idx] = urdf_to_sot[(Index)idx];
+ }
+ }
+
+ void RobotUtil::
+ set_urdf_to_sot(const dg::Vector &urdf_to_sot)
+ {
+ m_nbJoints = urdf_to_sot.size();
+ m_urdf_to_sot.resize(urdf_to_sot.size());
+ for(unsigned int idx=0;idx<urdf_to_sot.size();idx++)
+ {
+ m_urdf_to_sot[idx] = (unsigned int)urdf_to_sot[idx];
+ }
+ m_dgv_urdf_to_sot = urdf_to_sot;
+ }
+
+ bool RobotUtil::
+ joints_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot)
+ {
+ if (m_nbJoints==0)
+ {
+ SEND_MSG("set_urdf_to_sot should be called", MSG_TYPE_ERROR);
+ return false;
+ }
+ assert(q_urdf.size()==m_nbJoints);
+ assert(q_sot.size()==m_nbJoints);
+
+ for(unsigned int idx=0;idx<m_nbJoints;idx++)
+ q_sot[m_urdf_to_sot[idx]]=q_urdf[idx];
+ return true;
+ }
+
+ bool RobotUtil::
+ joints_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf)
+ {
+ assert(q_urdf.size()==static_cast<Eigen::VectorXd::Index>(m_nbJoints));
+ assert(q_sot.size()==static_cast<Eigen::VectorXd::Index>(m_nbJoints));
+
+ if (m_nbJoints==0)
+ {
+ SEND_MSG("set_urdf_to_sot should be called", MSG_TYPE_ERROR);
+ return false;
+ }
+
+ for(unsigned int idx=0;idx<m_nbJoints;idx++)
+ q_urdf[idx]=q_sot[m_urdf_to_sot[idx]];
+ return true;
+ }
+
+ bool RobotUtil::
+ velocity_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::ConstRefVector v_urdf,
+ Eigen::RefVector v_sot)
+ {
+ assert(q_urdf.size()==m_nbJoints+7);
+ assert(v_urdf.size()==m_nbJoints+6);
+ assert(v_sot.size()==m_nbJoints+6);
+
+ if (m_nbJoints==0)
+ {
+ SEND_MSG("velocity_urdf_to_sot should be called", MSG_TYPE_ERROR);
+ return false;
+ }
+ const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
+ Eigen::Matrix3d oRb = q.toRotationMatrix();
+ v_sot.head<3>() = oRb*v_urdf.head<3>();
+ v_sot.segment<3>(3) = oRb*v_urdf.segment<3>(3);
+// v_sot.head<6>() = v_urdf.head<6>();
+ joints_urdf_to_sot(v_urdf.tail(m_nbJoints),
+ v_sot.tail(m_nbJoints));
+ return true;
+ }
+
+ bool RobotUtil::
+ velocity_sot_to_urdf(Eigen::ConstRefVector q_urdf, Eigen::ConstRefVector v_sot,
+ Eigen::RefVector v_urdf)
+ {
+ assert(q_urdf.size()==m_nbJoints+7);
+ assert(v_urdf.size()==m_nbJoints+6);
+ assert(v_sot.size()==m_nbJoints+6);
+
+ if (m_nbJoints==0)
+ {
+ SEND_MSG("velocity_sot_to_urdf should be called", MSG_TYPE_ERROR);
+ return false;
+ }
+ // compute rotation from world to base frame
+ const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
+ Eigen::Matrix3d oRb = q.toRotationMatrix();
+ v_urdf.head<3>() = oRb.transpose()*v_sot.head<3>();
+ v_urdf.segment<3>(3) = oRb.transpose()*v_sot.segment<3>(3);
+// v_urdf.head<6>() = v_sot.head<6>();
+ joints_sot_to_urdf(v_sot.tail(m_nbJoints),
+ v_urdf.tail(m_nbJoints));
+ return true;
+ }
+
+ bool RobotUtil::
+ base_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot)
+ {
+ assert(q_urdf.size()==7);
+ assert(q_sot.size()==6);
+
+ // ********* Quat to RPY *********
+ const double W = q_urdf[6];
+ const double X = q_urdf[3];
+ const double Y = q_urdf[4];
+ const double Z = q_urdf[5];
+ const Eigen::Matrix3d R = Eigen::Quaterniond(W, X, Y, Z).toRotationMatrix();
+ return base_se3_to_sot(q_urdf.head<3>(), R, q_sot);
+
+ }
+
+ bool RobotUtil::
+ base_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf)
+ {
+ assert(q_urdf.size()==7);
+ assert(q_sot.size()==6);
+ // ********* RPY to Quat *********
+ const double r = q_sot[3];
+ const double p = q_sot[4];
+ const double y = q_sot[5];
+ const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
+ const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
+ const Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());
+ const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;
+
+ q_urdf[0 ]=q_sot[0 ]; //BASE
+ q_urdf[1 ]=q_sot[1 ];
+ q_urdf[2 ]=q_sot[2 ];
+ q_urdf[3 ]=quat.x();
+ q_urdf[4 ]=quat.y();
+ q_urdf[5 ]=quat.z();
+ q_urdf[6 ]=quat.w();
+
+ return true;
+ }
+
+ bool RobotUtil::
+ config_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot)
+ {
+ assert(q_urdf.size()==m_nbJoints+7);
+ assert(q_sot.size()==m_nbJoints+6);
+
+ base_urdf_to_sot(q_urdf.head<7>(), q_sot.head<6>());
+ joints_urdf_to_sot(q_urdf.tail(m_nbJoints), q_sot.tail(m_nbJoints));
+
+ return true;
+ }
+
+ bool RobotUtil::
+ config_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf)
+ {
+ assert(q_urdf.size()==m_nbJoints+7);
+ assert(q_sot.size()==m_nbJoints+6);
+ base_sot_to_urdf(q_sot.head<6>(), q_urdf.head<7>());
+ joints_sot_to_urdf(q_sot.tail(m_nbJoints),
+ q_urdf.tail(m_nbJoints));
+
+
+ }
+ void RobotUtil::
+ display(std::ostream &os) const
+ {
+ m_force_util.display(os);
+ m_foot_util.display(os);
+ os << "Nb of joints: " << m_nbJoints <<std::endl;
+ os << "Urdf file name: " << m_urdf_filename << std::endl;
+
+ // Display m_urdf_to_sot
+ os << "Map from urdf index to the Sot Index " << std::endl;
+ for(unsigned int i=0;i<m_urdf_to_sot.size();i++)
+ os << "(" << i << " : " << m_urdf_to_sot[i] << ") ";
+ os << std::endl;
+
+ os << "Joint name to joint id:" << std::endl;
+ for( std::map<std::string,Index>::const_iterator
+ it = m_name_to_id.begin();
+ it != m_name_to_id.end();
+ ++it)
+ {
+ os << "(" << it->first << "," << it->second << ") ";
+ }
+ os << std::endl;
+
+ os << "Joint id to joint Name:" << std::endl;
+ for( std::map<Index,std::string>::const_iterator
+ it = m_id_to_name.begin();
+ it != m_id_to_name.end();
+ ++it)
+ {
+ os << "(" << it->first << "," << it->second << ") ";
+ }
+ os << std::endl;
+
+ }
+ bool base_se3_to_sot(Eigen::ConstRefVector pos,
+ Eigen::ConstRefMatrix R,
+ Eigen::RefVector q_sot)
+ {
+ assert(q_sot.size()==6);
+ assert(pos.size()==3);
+ assert(R.rows()==3);
+ assert(R.cols()==3);
+ // ********* Quat to RPY *********
+ double r,p,y,m;
+ m = sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2));
+ p = atan2(-R(2, 0), m);
+ if (fabs(fabs(p) - M_PI / 2) < 0.001 )
+ {
+ r = 0.0;
+ y = -atan2(R(0, 1), R(1, 1));
+ }
+ else
+ {
+ y = atan2(R(1, 0), R(0, 0)) ;
+ r = atan2(R(2, 1), R(2, 2)) ;
+ }
+ // *********************************
+ q_sot[0 ]=pos[0 ];
+ q_sot[1 ]=pos[1 ];
+ q_sot[2 ]=pos[2 ];
+ q_sot[3 ]=r;
+ q_sot[4 ]=p;
+ q_sot[5 ]=y;
+ return true;
+ }
+
+ bool base_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot)
+ {
+ assert(q_urdf.size()==7);
+ assert(q_sot.size()==6);
+ // ********* Quat to RPY *********
+ const double W = q_urdf[6];
+ const double X = q_urdf[3];
+ const double Y = q_urdf[4];
+ const double Z = q_urdf[5];
+ const Eigen::Matrix3d R = Eigen::Quaterniond(W, X, Y, Z).toRotationMatrix();
+ return base_se3_to_sot(q_urdf.head<3>(), R, q_sot);
+ }
+
+ bool base_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf)
+ {
+ assert(q_urdf.size()==7);
+ assert(q_sot.size()==6);
+ // ********* RPY to Quat *********
+ const double r = q_sot[3];
+ const double p = q_sot[4];
+ const double y = q_sot[5];
+ const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
+ const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
+ const Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());
+ const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;
+
+ q_urdf[0 ]=q_sot[0 ]; //BASE
+ q_urdf[1 ]=q_sot[1 ];
+ q_urdf[2 ]=q_sot[2 ];
+ q_urdf[3 ]=quat.x();
+ q_urdf[4 ]=quat.y();
+ q_urdf[5 ]=quat.z();
+ q_urdf[6 ]=quat.w();
+
+ return true;
+ }
+
+ std::map<std::string,RobotUtil *> sgl_map_name_to_robot_util;
+
+ RobotUtil * getRobotUtil(std::string &robotName)
+ {
+ std::map<std::string,RobotUtil *>::iterator it =
+ sgl_map_name_to_robot_util.find(robotName);
+ if (it!=sgl_map_name_to_robot_util.end())
+ return it->second;
+ return RefVoidRobotUtil();
+ }
+
+ bool isNameInRobotUtil(std::string &robotName)
+ {
+ std::map<std::string,RobotUtil *>::iterator it =
+ sgl_map_name_to_robot_util.find(robotName);
+ if (it!=sgl_map_name_to_robot_util.end())
+ return true;
+ return false;
+ }
+ RobotUtil * createRobotUtil(std::string &robotName)
+ {
+ std::map<std::string,RobotUtil *>::iterator it =
+ sgl_map_name_to_robot_util.find(robotName);
+ if (it==sgl_map_name_to_robot_util.end())
+ {
+ sgl_map_name_to_robot_util[robotName]= new RobotUtil;
+ it = sgl_map_name_to_robot_util.find(robotName);
+ return it->second;
+ }
+ std::cout << "Another robot is already in the map for " << robotName
+ << std::endl;
+ return RefVoidRobotUtil();
+ }
+
+ } // talos_balance
+ } // sot
+} // dynamic_graph
diff --git a/src/utils/trajectory-generators.cpp b/src/utils/trajectory-generators.cpp
new file mode 100644
index 0000000..0dd248c
--- /dev/null
+++ b/src/utils/trajectory-generators.cpp
@@ -0,0 +1,38 @@
+/*
+ * Copyright 2015, Andrea Del Prete, LAAS-CNRS
+ * 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.
+ * 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-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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <sot/talos_balance/utils/trajectory-generators.hh>
+#include <sot/core/debug.hh>
+#include <dynamic-graph/factory.h>
+
+
+namespace dynamicgraph
+{
+ namespace sot
+ {
+ namespace talos_balance
+ {
+ namespace dynamicgraph = ::dynamicgraph;
+ using namespace dynamicgraph;
+ using namespace dynamicgraph::command;
+
+
+
+ } // namespace torquecontrol
+ } // namespace sot
+} // namespace dynamicgraph
diff --git a/unittest/python/appli_jointTrajGen.py b/unittest/python/appli_jointTrajGen.py
new file mode 100644
index 0000000..6d18fc5
--- /dev/null
+++ b/unittest/python/appli_jointTrajGen.py
@@ -0,0 +1,13 @@
+from sot_talos_balance.joint_trajectory_generator import JointTrajectoryGenerator
+from dynamic_graph import plug
+
+dt = robot.timeStep;
+
+robot.traj_gen = JointTrajectoryGenerator("jtg");
+plug(robot.device.robotState, robot.traj_gen.base6d_encoders);
+robot.traj_gen.init(dt, "robot");
+plug(robot.traj_gen.dq, robot.device.control);
+
+
+
+robot.device.control.recompute(0)
diff --git a/unittest/python/test_jointTrajGen.py b/unittest/python/test_jointTrajGen.py
new file mode 100644
index 0000000..b8c2021
--- /dev/null
+++ b/unittest/python/test_jointTrajGen.py
@@ -0,0 +1,58 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-1
+
+import sys
+import rospy
+from dynamic_graph import plug
+from std_srvs.srv import *
+from dynamic_graph_bridge.srv import *
+from dynamic_graph_bridge_msgs.srv import *
+
+def launchScript(code,title,description = ""):
+ raw_input(title+': '+description)
+ rospy.loginfo(title)
+ rospy.loginfo(code)
+ for line in code:
+ if line != '' and line[0] != '#':
+ print line
+ answer = runCommandClient(str(line))
+ rospy.logdebug(answer)
+ print answer
+ rospy.loginfo("...done with "+title)
+
+
+
+# Waiting for services
+try:
+ rospy.loginfo("Waiting for run_command")
+ rospy.wait_for_service('/run_command')
+ rospy.loginfo("...ok")
+
+ rospy.loginfo("Waiting for start_dynamic_graph")
+ rospy.wait_for_service('/start_dynamic_graph')
+ rospy.loginfo("...ok")
+
+ runCommandClient = rospy.ServiceProxy('run_command', RunCommand)
+ runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty)
+
+ initCode = open( "appli_jointTrajGen.py", "r").read().split("\n")
+
+ rospy.loginfo("Stack of Tasks launched")
+
+ # runCommandClient("from dynamic_graph import plug")
+ # runCommandClient("from dynamic_graph.sot.core import SOT")
+ # runCommandClient("sot = SOT('sot')")
+ # runCommandClient("sot.setSize(robot.dynamic.getDimension())")
+ # runCommandClient("plug(sot.control,robot.device.control)")
+
+ launchScript(initCode,'initialize SoT')
+ raw_input("Wait before starting the dynamic graph")
+ runCommandStartDynamicGraph()
+ #raw_input("Wait before moving the hand")
+ #runCommandClient("target = (0.5,-0.2,1.0)")
+ #runCommandClient("gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))")
+ #runCommandClient("sot.push(taskRH.task.name)")
+
+except rospy.ServiceException, e:
+ rospy.logerr("Service call failed: %s" % e)
+
--
GitLab