diff --git a/CMakeLists.txt b/CMakeLists.txt
index d3e59b1dc74df3dfb22b21a084e5485d4066e0c5..2c03ab397b3774e8902a172c4b982319c98dee2a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -87,11 +87,9 @@ 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
)
@@ -101,8 +99,6 @@ 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
)
diff --git a/include/sot/talos_balance/joint-trajectory-generator.hh b/include/sot/talos_balance/joint-trajectory-generator.hh
deleted file mode 100644
index 0957407c5e523f78675e77cd1f9320685b480d04..0000000000000000000000000000000000000000
--- a/include/sot/talos_balance/joint-trajectory-generator.hh
+++ /dev/null
@@ -1,221 +0,0 @@
-/*
- * 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
index 3f8138aa68b0dfdc499894b6a55a81ddc91323e0..ce5b28f4182e09b46b56f9e91e7c86bc70abb1e8 100644
--- a/include/sot/talos_balance/nd-trajectory-generator.hh
+++ b/include/sot/talos_balance/nd-trajectory-generator.hh
@@ -50,7 +50,6 @@
#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"
diff --git a/include/sot/talos_balance/utils/common.hh b/include/sot/talos_balance/utils/common.hh
deleted file mode 100644
index 69d64a4cfd4cb31e5be18d1991f3ea206499df94..0000000000000000000000000000000000000000
--- a/include/sot/talos_balance/utils/common.hh
+++ /dev/null
@@ -1,292 +0,0 @@
-/*
- * 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
deleted file mode 100644
index 8edd00608455a24a940e29661277f40af4f6386f..0000000000000000000000000000000000000000
--- a/include/sot/talos_balance/utils/trajectory-generators.hh
+++ /dev/null
@@ -1,570 +0,0 @@
-/*
- * 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/python/sot_talos_balance/create_entities_utils.py b/python/sot_talos_balance/create_entities_utils.py
index b3d22dc13929d610d086305e837df24beb27b96c..2372170e235fe3403e4bd6775f36bceb1cf0145e 100644
--- a/python/sot_talos_balance/create_entities_utils.py
+++ b/python/sot_talos_balance/create_entities_utils.py
@@ -1,14 +1,11 @@
-from sot_talos_balance.joint_trajectory_generator import JointTrajectoryGenerator
-from sot_talos_balance.joint_position_controller import JointPositionController
+from sot_talos_balance.nd_trajectory_generator import NdTrajectoryGenerator
from dynamic_graph import plug
-def create_joint_trajectory_generator(device, dt=0.001, robot_name="robot"):
- jtg = JointTrajectoryGenerator("jtg");
- plug(device.robotState, jtg.base6d_encoders);
- jtg.init(dt, robot_name);
- return jtg;
+N_JOINTS = 32;
-def create_joint_position_controller(robot, dt=0.001, robot_name="robot"):
- jpc = JointPositionController("jpc");
- plug(robot.device.robotState, jpc.q);
- return jpc;
+def create_joint_trajectory_generator(dt):
+ jtg = NdTrajectoryGenerator("jtg");
+ jtg.initial_value.value = tuple(32*[0.0]);
+ jtg.trigger.value = 1.0;
+ jtg.init(dt, N_JOINTS);
+ return jtg;
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 5ab769c1cc239aca5af932c9d31144a068deec8a..b5bb1ede7aea180aaca710e10c65085e209ae448 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -37,7 +37,6 @@ ENDIF(UNIX)
SET(plugins
example
joint-position-controller
- joint-trajectory-generator
nd-trajectory-generator
)
diff --git a/src/joint-trajectory-generator.cpp b/src/joint-trajectory-generator.cpp
deleted file mode 100644
index 1ae205d2655bd03f9705d057c041e52ed767fbb6..0000000000000000000000000000000000000000
--- a/src/joint-trajectory-generator.cpp
+++ /dev/null
@@ -1,884 +0,0 @@
-/*
- * 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/utils/common.cpp b/src/utils/common.cpp
deleted file mode 100644
index 8591b70f029026c60cdfe8c1bbad3de57664b118..0000000000000000000000000000000000000000
--- a/src/utils/common.cpp
+++ /dev/null
@@ -1,548 +0,0 @@
-/*
- * 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
deleted file mode 100644
index 0dd248c3afd9935fa0c43e86739e401c89b62d91..0000000000000000000000000000000000000000
--- a/src/utils/trajectory-generators.cpp
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * 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/test_jointTrajGen.py b/unittest/python/test_jointTrajGen.py
index e502f369d233f05839fe18c0435f880ad6b64d6f..5ccbf1aa01b6f5f05e34a7a00da736bf17c02921 100644
--- a/unittest/python/test_jointTrajGen.py
+++ b/unittest/python/test_jointTrajGen.py
@@ -1,22 +1,20 @@
#!/usr/bin/python
# -*- coding: utf-8 -*-1
from sot_talos_balance.create_entities_utils import create_joint_trajectory_generator
-from sot_talos_balance.create_entities_utils import create_joint_position_controller
from dynamic_graph import plug
+from time import sleep
+dt = robot.timeStep;
+robot.traj_gen = create_joint_trajectory_generator(dt);
+robot.device.control.value = 32*[0.0];
+plug(robot.traj_gen.dx, robot.device.control);
-# Waiting for services
-try:
- dt = robot.timeStep;
-
- robot.traj_gen = create_joint_trajectory_generator(robot.device,dt)
- robot.joint_pos_controller =create_joint_position_controller(robot,dt);
- plug(robot.traj_gen.q, robot.joint_pos_controller.qDes);
- plug(robot.traj_gen.dq, robot.joint_pos_controller.dqDes);
- plug(robot.joint_pos_controller.dqRef, robot.device.control);
- robot.joint_pos_controller.init(tuple([1.0]*32));
-
-
- sleep(1.0)
- os.system('rosservice call /start_dynamic_graph');
+sleep(1.0);
+os.system('rosservice call /start_dynamic_graph');
+sleep(1.0);
+robot.traj_gen.move(31,-1.0,1.0);
+sleep(1.1);
+robot.traj_gen.startSinusoid(31,3.0,2.0);
+sleep(10.0);
+robot.traj_gen.stop(31);