Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • lscherrer/sot-talos-balance
  • imaroger/sot-talos-balance
  • pfernbac/sot-talos-balance
  • ostasse/sot-talos-balance
  • gsaurel/sot-talos-balance
  • loco-3d/sot-talos-balance
6 results
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 1509 additions and 963 deletions
include/sot/talos_balance/boolean-identity.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-talos-balance is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_boolean_identity_H__
#define __sot_talos_balance_boolean_identity_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define BOOLEAN_IDENTITY_EXPORT __declspec(dllexport)
#else
#define BOOLEAN_IDENTITY_EXPORT __declspec(dllimport)
#endif
#else
#define BOOLEAN_IDENTITY_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class BOOLEAN_IDENTITY_EXPORT BooleanIdentity : public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
BooleanIdentity(const std::string& name);
void init() {}
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(sin, bool);
DECLARE_SIGNAL_OUT(sout, bool);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
}; // class BooleanIdentity
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_boolean_identity_H__
include/sot/talos_balance/com-admittance-controller.hh
View file @ e217f893
......@@ -21,84 +21,77 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (com_admittance_controller_EXPORTS)
# define COMADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
# else
# define COMADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(com_admittance_controller_EXPORTS)
#define COMADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
#else
# define COMADMITTANCECONTROLLER_EXPORT
#define COMADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define COMADMITTANCECONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include "utils/signal-helper.hh"
#include "utils/logger.hh"
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class COMADMITTANCECONTROLLER_EXPORT ComAdmittanceController
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
ComAdmittanceController( const std::string & name );
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
void init(const double & dt);
class COMADMITTANCECONTROLLER_EXPORT ComAdmittanceController
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
void setPosition(const dynamicgraph::Vector &);
void setVelocity(const dynamicgraph::Vector &);
void setState(const dynamicgraph::Vector &, const dynamicgraph::Vector &);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmpDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ddcomDes, dynamicgraph::Vector);
/* --- CONSTRUCTOR ---- */
ComAdmittanceController(const std::string &name);
DECLARE_SIGNAL_OUT(ddcomRef, dynamicgraph::Vector);
void init(const double &dt);
DECLARE_SIGNAL_INNER(stateRef, dynamicgraph::Vector);
void setPosition(const dynamicgraph::Vector &);
void setVelocity(const dynamicgraph::Vector &);
void setState(const dynamicgraph::Vector &, const dynamicgraph::Vector &);
DECLARE_SIGNAL_OUT(comRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dcomRef, dynamicgraph::Vector);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmpDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ddcomDes, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
DECLARE_SIGNAL_OUT(ddcomRef, dynamicgraph::Vector);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("[ComAdmittanceController-"+name+"] "+msg, t, file, line);
}
DECLARE_SIGNAL_INNER(stateRef, dynamicgraph::Vector);
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_state; // internal state
double m_dt;
DECLARE_SIGNAL_OUT(comRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dcomRef, dynamicgraph::Vector);
}; // class ComAdmittanceController
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_state; // internal state
double m_dt;
}; // class ComAdmittanceController
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_com_admittance_controller_H__
#endif // #ifndef __sot_talos_balance_com_admittance_controller_H__
include/sot/talos_balance/common.hh deleted 100644 → 0
View file @ f4144c44
/*
* Copyright 2017, A. Del Prete, T. Flayols, O. Stasse, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_torque_control_common_H__
#define __sot_torque_control_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 <iostream>
#include <dynamic-graph/linear-algebra.h>
#include <sot/talos_balance/utils/signal-helper.hh>
#include <sot/talos_balance/utils/vector-conversions.hh>
#include <map>
#include "boost/assign.hpp"
#include <sot/talos_balance/utils/logger.hh>
namespace dg = ::dynamicgraph;
using namespace dg;
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
struct JointLimits
{
double upper;
double lower;
JointLimits():
upper(0.0),
lower(0.0)
{}
JointLimits(double l, double u):
upper(u),
lower(l)
{}
};
typedef Eigen::VectorXd::Index Index;
struct ForceLimits
{
Eigen::VectorXd upper;
Eigen::VectorXd lower;
ForceLimits():
upper(Eigen::Vector6d::Zero()),
lower(Eigen::Vector6d::Zero())
{}
ForceLimits(const Eigen::VectorXd& l, const Eigen::VectorXd& u):
upper(u),
lower(l)
{}
void display(std::ostream &os) const;
};
struct ForceUtil
{
std::map<Index,ForceLimits> m_force_id_to_limits;
std::map<std::string,Index> m_name_to_force_id;
std::map<Index,std::string> m_force_id_to_name;
Index m_Force_Id_Left_Hand, m_Force_Id_Right_Hand,
m_Force_Id_Left_Foot, m_Force_Id_Right_Foot;
void set_name_to_force_id(const std::string & name,
const Index &force_id);
void set_force_id_to_limits(const Index &force_id,
const dg::Vector &lf,
const dg::Vector &uf);
void create_force_id_to_name_map();
Index get_id_from_name(const std::string &name);
const std::string & get_name_from_id(Index idx);
std::string cp_get_name_from_id(Index idx);
const ForceLimits & get_limits_from_id(Index force_id);
ForceLimits cp_get_limits_from_id(Index force_id);
Index get_force_id_left_hand()
{
return m_Force_Id_Left_Hand;
}
void set_force_id_left_hand(Index anId)
{
m_Force_Id_Left_Hand = anId;
}
Index get_force_id_right_hand()
{
return m_Force_Id_Right_Hand;
}
void set_force_id_right_hand( Index anId)
{
m_Force_Id_Right_Hand = anId;
}
Index get_force_id_left_foot()
{
return m_Force_Id_Left_Foot;
}
void set_force_id_left_foot(Index anId)
{
m_Force_Id_Left_Foot = anId;
}
Index get_force_id_right_foot()
{
return m_Force_Id_Right_Foot;
}
void set_force_id_right_foot( Index anId)
{
m_Force_Id_Right_Foot = anId;
}
void display(std::ostream & out) const;
}; // struct ForceUtil
struct FootUtil
{
/// Position of the foot soles w.r.t. the frame of the foot
dynamicgraph::Vector m_Right_Foot_Sole_XYZ;
/// Position of the force/torque sensors w.r.t. the frame of the hosting link
dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ;
std::string m_Left_Foot_Frame_Name;
std::string m_Right_Foot_Frame_Name;
void display(std::ostream & os) const;
};
struct RobotUtil
{
public:
RobotUtil();
/// Forces data
ForceUtil m_force_util;
/// Foot information
FootUtil m_foot_util;
/// Map from the urdf index to the SoT index.
std::vector<Index> m_urdf_to_sot;
/// Nb of Dofs for the robot.
long unsigned int m_nbJoints;
/// Map from the name to the id.
std::map<std::string,Index> m_name_to_id;
/// The map between id and name
std::map<Index,std::string> m_id_to_name;
/// The joint limits map.
std::map<Index,JointLimits> m_limits_map;
/// The name of the joint IMU is attached to
std::string m_imu_joint_name;
/// This method creates the map between id and name.
/// It is called each time a new link between id and name is inserted
/// (i.e. when set_name_to_id is called).
void create_id_to_name_map();
/// URDF file path
std::string m_urdf_filename;
dynamicgraph::Vector m_dgv_urdf_to_sot;
/** Given a joint name it finds the associated joint id.
* If the specified joint name is not found it returns -1;
* @param name Name of the joint to find.
* @return The id of the specified joint, -1 if not found. */
const Index get_id_from_name(const std::string &name);
/** Given a joint id it finds the associated joint name.
* If the specified joint is not found it returns "Joint name not found";
* @param id Id of the joint to find.
* @return The name of the specified joint, "Joint name not found" if not found. */
/// Get the joint name from its index
const std::string & get_name_from_id(Index id);
/// Set relation between the name and the SoT id
void set_name_to_id(const std::string &jointName,
const Index & jointId);
/// Set the map between urdf index and sot index
void set_urdf_to_sot(const std::vector<Index> &urdf_to_sot);
void set_urdf_to_sot(const dg::Vector &urdf_to_sot);
/// Set the limits (lq,uq) for joint idx
void set_joint_limits_for_id(const Index &idx,
const double &lq,
const double &uq);
bool joints_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot);
bool joints_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
bool velocity_urdf_to_sot(Eigen::ConstRefVector q_urdf,
Eigen::ConstRefVector v_urdf, Eigen::RefVector v_sot);
bool velocity_sot_to_urdf(Eigen::ConstRefVector q_urdf,
Eigen::ConstRefVector v_sot, Eigen::RefVector v_urdf);
bool config_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot);
bool config_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
bool base_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot);
bool base_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
/** Given a joint id it finds the associated joint limits.
* If the specified joint is not found it returns JointLimits(0,0).
* @param id Id of the joint to find.
* @return The limits of the specified joint, JointLimits(0,0) if not found. */
const JointLimits & get_joint_limits_from_id(Index id);
JointLimits cp_get_joint_limits_from_id(Index id);
void sendMsg(const std::string& msg,
MsgType t=MSG_TYPE_INFO,
const char* file="", int line=0)
{
getLogger().sendMsg("[FromURDFToSoT] "+msg, t, file, line);
}
void display(std::ostream &os) const;
}; // struct RobotUtil
RobotUtil * RefVoidRobotUtil();
RobotUtil * getRobotUtil(std::string &robotName);
bool isNameInRobotUtil(std::string &robotName);
RobotUtil * createRobotUtil(std::string &robotName);
bool base_se3_to_sot(Eigen::ConstRefVector pos,
Eigen::ConstRefMatrix R,
Eigen::RefVector q_sot);
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // sot_torque_control_common_h_
include/sot/talos_balance/control-manager.hh deleted 100644 → 0
View file @ f4144c44
/*
* Copyright 2015, Andrea Del Prete, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_torque_control_control_manager_H__
#define __sot_torque_control_control_manager_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (__sot_torque_control_control_manager_H__)
# define SOTCONTROLMANAGER_EXPORT __declspec(dllexport)
# else
# define SOTCONTROLMANAGER_EXPORT __declspec(dllimport)
# endif
#else
# define SOTCONTROLMANAGER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <sot/talos_balance/utils/signal-helper.hh>
#include <sot/talos_balance/utils/vector-conversions.hh>
#include <sot/talos_balance/utils/logger.hh>
#include <sot/talos_balance/common.hh>
#include <map>
#include "boost/assign.hpp"
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <sot/talos_balance/robot/robot-wrapper.hh>
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/// Number of time step to transition from one ctrl mode to another
#define CTRL_MODE_TRANSITION_TIME_STEP 1000.0
class CtrlMode
{
public:
int id;
std::string name;
CtrlMode(): id(-1), name("None"){}
CtrlMode(int id, const std::string& name):
id(id), name(name) {}
};
std::ostream& operator<<( std::ostream& os, const CtrlMode& s )
{
os<<s.id<<"_"<<s.name;
return os;
}
class SOTCONTROLMANAGER_EXPORT ControlManager
:public::dynamicgraph::Entity
{
typedef ControlManager EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
ControlManager( const std::string & name);
/// Initialize
/// @param dt: control interval
/// @param urdfFile: path to the URDF model of the robot
void init(const double & dt,
const std::string & urdfFile,
const std::string & robotRef);
/* --- SIGNALS --- */
std::vector<dynamicgraph::SignalPtr<dynamicgraph::Vector,int>*> m_ctrlInputsSIN;
std::vector< dynamicgraph::SignalPtr<bool,int>* > m_emergencyStopSIN; /// emergency stop inputs. If one is true, control is set to zero forever
std::vector<dynamicgraph::Signal<dynamicgraph::Vector,int>*> m_jointsCtrlModesSOUT;
DECLARE_SIGNAL_IN(i_measured, dynamicgraph::Vector); /// motor currents
DECLARE_SIGNAL_IN(tau, dynamicgraph::Vector); /// estimated joint torques (using dynamic robot model + F/T sensors)
DECLARE_SIGNAL_IN(tau_predicted, dynamicgraph::Vector); /// predicted joint torques (using motor model)
DECLARE_SIGNAL_IN(i_max, dynamicgraph::Vector); /// max current allowed before stopping the controller (in Ampers)
DECLARE_SIGNAL_IN(u_max, dynamicgraph::Vector); /// max desired current allowed before stopping the controller (in Ampers)
DECLARE_SIGNAL_IN(tau_max, dynamicgraph::Vector); /// max torque allowed before stopping the controller
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(u_safe, dynamicgraph::Vector); /// same as u when everything is fine, 0 otherwise
/* --- COMMANDS --- */
/// Commands related to the control mode.
void addCtrlMode(const std::string& name);
void ctrlModes();
void getCtrlMode(const std::string& jointName);
void setCtrlMode(const std::string& jointName, const std::string& ctrlMode);
void setCtrlMode(const int jid, const CtrlMode& cm);
void resetProfiler();
/// Commands related to joint name and joint id
void setNameToId(const std::string& jointName, const double & jointId);
void setJointLimitsFromId(const double &jointId,
const double &lq, const double &uq);
/// Command related to ForceUtil
void setForceLimitsFromId(const double &jointId,
const dynamicgraph::Vector &lq,
const dynamicgraph::Vector &uq);
void setForceNameToForceId(const std::string& forceName,
const double & forceId);
/// Commands related to FootUtil
void setRightFootSoleXYZ(const dynamicgraph::Vector &);
void setRightFootForceSensorXYZ(const dynamicgraph::Vector &);
void setFootFrameName(const std::string &, const std::string &);
void setImuJointName(const std::string &);
void displayRobotUtil();
/// Set the mapping between urdf and sot.
void setJoints(const dynamicgraph::Vector &);
void setStreamPrintPeriod(const double & s);
void setSleepTime(const double &seconds);
void addEmergencyStopSIN(const std::string& name);
/* --- 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("[ControlManager-"+name+"] "+msg, t, file, line);
}
protected:
RobotUtil * m_robot_util;
dynamicgraph::sot::talos_balance::robots::RobotWrapper * m_robot;
bool m_initSucceeded; /// true if the entity has been successfully initialized
double m_dt; /// control loop time period
bool m_emergency_stop_triggered; /// true if an emergency condition as been triggered either by an other entity, or by control limit violation
bool m_is_first_iter; /// true at the first iteration, false otherwise
int m_iter;
double m_sleep_time; /// time to sleep at every iteration (to slow down simulation)
std::vector<std::string> m_ctrlModes; /// existing control modes
std::vector<CtrlMode> m_jointCtrlModes_current; /// control mode of the joints
std::vector<CtrlMode> m_jointCtrlModes_previous; /// previous control mode of the joints
std::vector<int> m_jointCtrlModesCountDown; /// counters used for the transition between two ctrl modes
bool convertStringToCtrlMode(const std::string& name, CtrlMode& cm);
bool convertJointNameToJointId(const std::string& name, unsigned int& id);
bool isJointInRange(unsigned int id, double q);
void updateJointCtrlModesOutputSignal();
}; // class ControlManager
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_control_manager_H__
include/sot/talos_balance/coupled-admittance-controller.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-talos-balance is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_admittance_controller_H__
#define __sot_talos_balance_admittance_controller_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(admittance_controller_EXPORTS)
#define COUPLEDADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
#else
#define COUPLEDADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define COUPLEDADMITTANCECONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class COUPLEDADMITTANCECONTROLLER_EXPORT CoupledAdmittanceController
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
CoupledAdmittanceController(const std::string& name);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(kSum, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kDiff, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(tauL, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(tauR, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(tauDesL, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(tauDesR, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(tauSum, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(tauDiff, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(tauDesSum, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(tauDesDiff, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(dqRefSum, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(dqRefDiff, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dqRefL, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dqRefR, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
}; // class AdmittanceController
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_admittance_controller_H__
include/sot/talos_balance/dcm-com-controller.hh
View file @ e217f893
......@@ -21,84 +21,76 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (dcm_com_controller_EXPORTS)
# define DCMCOMCONTROLLER_EXPORT __declspec(dllexport)
# else
# define DCMCOMCONTROLLER_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(dcm_com_controller_EXPORTS)
#define DCMCOMCONTROLLER_EXPORT __declspec(dllexport)
#else
# define DCMCOMCONTROLLER_EXPORT
#define DCMCOMCONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define DCMCOMCONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include "utils/signal-helper.hh"
#include "utils/logger.hh"
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class DCMCOMCONTROLLER_EXPORT DcmComController
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
DcmComController( const std::string & name );
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
void init(const double & dt);
class DCMCOMCONTROLLER_EXPORT DcmComController : public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
void resetDcmIntegralError();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(decayFactor, double);
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(mass, double);
DECLARE_SIGNAL_IN(dcm, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dcmDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(comDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ddcomDes, dynamicgraph::Vector);
/* --- CONSTRUCTOR ---- */
DcmComController(const std::string& name);
DECLARE_SIGNAL_OUT(ddcomRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmpRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector);
void init(const double& dt);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
void resetDcmIntegralError();
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("[DcmComController-"+name+"] "+msg, t, file, line);
}
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(decayFactor, double);
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(mass, double);
DECLARE_SIGNAL_IN(dcm, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dcmDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(comDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ddcomDes, dynamicgraph::Vector);
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_dcmIntegralError; // internal state
double m_dt;
DECLARE_SIGNAL_OUT(ddcomRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmpRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector);
}; // class DcmComController
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_dcmIntegralError; // internal state
double m_dt;
}; // class DcmComController
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_dcm_com_controller_H__
#endif // #ifndef __sot_talos_balance_dcm_com_controller_H__
include/sot/talos_balance/dcm-controller.hh
View file @ e217f893
......@@ -21,83 +21,77 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (dcm_controller_EXPORTS)
# define DCMCONTROLLER_EXPORT __declspec(dllexport)
# else
# define DCMCONTROLLER_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(dcm_controller_EXPORTS)
#define DCMCONTROLLER_EXPORT __declspec(dllexport)
#else
# define DCMCONTROLLER_EXPORT
#define DCMCONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define DCMCONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include "utils/signal-helper.hh"
#include "utils/logger.hh"
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class DCMCONTROLLER_EXPORT DcmController
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
DcmController( const std::string & name );
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
void init(const double & dt);
class DCMCONTROLLER_EXPORT DcmController : public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
void resetDcmIntegralError();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(decayFactor, double);
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(mass, double);
DECLARE_SIGNAL_IN(com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dcm, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dcmDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmpDes, dynamicgraph::Vector);
/* --- CONSTRUCTOR ---- */
DcmController(const std::string& name);
DECLARE_SIGNAL_OUT(zmpRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector);
void init(const double& dt);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
void resetDcmIntegralError();
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("[DcmController-"+name+"] "+msg, t, file, line);
}
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(Kz, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(decayFactor, double);
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(mass, double);
DECLARE_SIGNAL_IN(com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dcm, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dcmDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmpDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmp, dynamicgraph::Vector);
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_dcmIntegralError; // internal state
double m_dt;
DECLARE_SIGNAL_OUT(zmpRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector);
}; // class DcmController
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_dcmIntegralError; // internal state
double m_dt;
}; // class DcmController
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_dcm_controller_H__
#endif // #ifndef __sot_talos_balance_dcm_controller_H__
include/sot/talos_balance/dcm-estimator.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2019,
* LAAS-CNRS,
* Gepetto team
*
* This file is part of sot-talos-balance.
* See license file
*/
#ifndef __sot_talos_balance_dcm_estimator_H__
#define __sot_talos_balance_dcm_estimator_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(dcm_estimator_EXPORTS)
#define DCMESTIMATOR_EXPORT __declspec(dllexport)
#else
#define DCMESTIMATOR_EXPORT __declspec(dllimport)
#endif
#else
#define DCMESTIMATOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinocchio first
#include <dynamic-graph/signal-helper.h>
#include <boost/math/distributions/normal.hpp> // for normal_distribution
#include <map>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
#include "boost/assign.hpp"
/* Pinocchio */
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class DCMESTIMATOR_EXPORT DcmEstimator : public ::dynamicgraph::Entity {
typedef pinocchio::SE3 SE3;
typedef Eigen::Vector2d Vector2;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Vector4d Vector4;
typedef Vector6d Vector6;
typedef Vector7d Vector7;
typedef Eigen::Matrix3d Matrix3;
typedef boost::math::normal normal;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
DcmEstimator(const std::string& name);
void init(const double& dt, const std::string& urdfFile);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(c, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dc, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
RobotUtilShrPtr m_robot_util;
pinocchio::Data m_data; /// Pinocchio robot data
Eigen::VectorXd
m_q_pin; /// robot configuration according to pinocchio convention
Eigen::VectorXd
m_v_pin; /// robot velocities according to pinocchio convention
double m_dt; /// sampling time step
pinocchio::Model m_model; /// Pinocchio robot model
}; // class DCMEstimator
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_dcm_estimator_H__
include/sot/talos_balance/delay.hh 0 → 100644
View file @ e217f893
// Copyright 2018 CNRS - Airbus SAS
// Author: Joseph Mirabel
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef __sot_talos_balance_delay_H__
#define __sot_talos_balance_delay_H__
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal.h>
#if defined(WIN32)
#if defined(delay_EXPORTS)
#define DELAY_EXPORT __declspec(dllexport)
#else
#define DELAY_EXPORT __declspec(dllimport)
#endif
#else
#define DELAY_EXPORT
#endif
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/// Delay
template <typename Value, typename Time = int>
class DELAY_EXPORT Delay : public dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
Delay(const std::string& name)
: Entity(name),
sin(NULL, "Delay(" + name + ")::input(unspecified)::sin"),
currentOUT("Delay(" + name + ")::output(unspecified)::current"),
previousOUT("Delay(" + name + ")::output(unspecified)::previous") {
currentOUT.setFunction(boost::bind(&Delay::current, this, _1, _2));
previousOUT.setFunction(boost::bind(&Delay::previous, this, _1, _2));
signalRegistration(sin << currentOUT << previousOUT);
using command::makeCommandVoid1;
std::string docstring =
"\n"
" Set current value in memory\n";
addCommand("setMemory",
makeCommandVoid1(*this, &Delay::setMemory, docstring));
}
~Delay() {}
/// Header documentation of the python class
virtual std::string getDocString() const {
return "Enable delayed signal in SoT.\n"
"Signal previous at time t+1 will return the value of signal "
"current "
" at time <= t (the last time signal current was called).";
}
void setMemory(const Value& val) { mem = val; }
private:
Value& current(Value& res, const Time& time) {
previousOUT.access(time);
res = sin.access(time);
mem = res;
return res;
}
Value& previous(Value& res, const Time&) {
res = mem;
return res;
}
Value mem;
SignalPtr<Value, Time> sin;
Signal<Value, Time> currentOUT, previousOUT;
};
typedef Delay<Vector, int> DelayVector;
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_simple_pid_H__
include/sot/talos_balance/distribute-wrench.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-talos-balance is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_distribute_wrench_H__
#define __sot_talos_balance_distribute_wrench_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define DISTRIBUTE_WRENCH_EXPORT __declspec(dllexport)
#else
#define DISTRIBUTE_WRENCH_EXPORT __declspec(dllimport)
#endif
#else
#define DISTRIBUTE_WRENCH_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinochcio first
#include <dynamic-graph/signal-helper.h>
#include <eiquadprog/eiquadprog-fast.hpp>
#include <map>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/multibody/model.hpp>
#include <sot/core/robot-utils.hh>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class DISTRIBUTE_WRENCH_EXPORT DistributeWrench
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
DistributeWrench(const std::string& name);
void init(const std::string& robotName);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(wrenchDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(rho, double);
DECLARE_SIGNAL_IN(phase, int);
DECLARE_SIGNAL_IN(frictionCoefficient, double);
DECLARE_SIGNAL_IN(wSum, double);
DECLARE_SIGNAL_IN(wNorm, double);
DECLARE_SIGNAL_IN(wRatio, double);
DECLARE_SIGNAL_IN(wAnkle, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(kinematics_computations, int);
DECLARE_SIGNAL_INNER(qp_computations, int);
DECLARE_SIGNAL_OUT(wrenchLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ankleWrenchLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(surfaceWrenchLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(copLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ankleWrenchRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(surfaceWrenchRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(copRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmpRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(emergencyStop, bool);
public:
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
Eigen::Vector3d computeCoP(const dynamicgraph::Vector& wrench,
const pinocchio::SE3& pose) const;
void set_right_foot_sizes(const dynamicgraph::Vector& s);
void set_left_foot_sizes(const dynamicgraph::Vector& s);
double m_eps;
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
pinocchio::Model m_model; /// Pinocchio robot model
pinocchio::Data m_data; /// Pinocchio robot data
RobotUtilShrPtr m_robot_util;
// pinocchio::SE3 m_ankle_M_ftSens; /// ankle to F/T sensor
// transformation
pinocchio::SE3 m_ankle_M_sole; /// ankle to sole transformation
pinocchio::FrameIndex m_left_foot_id;
pinocchio::FrameIndex m_right_foot_id;
pinocchio::SE3 m_contactLeft;
pinocchio::SE3 m_contactRight;
Eigen::Matrix<double, 6, 1> m_wrenchLeft;
Eigen::Matrix<double, 6, 1> m_wrenchRight;
Eigen::Vector4d m_left_foot_sizes; /// sizes of the left foot (pos x, neg x,
/// pos y, neg y)
Eigen::Vector4d m_right_foot_sizes; /// sizes of the left foot (pos x, neg x,
/// pos y, neg y)
void computeWrenchFaceMatrix(const double mu);
Eigen::Matrix<double, 16, 6> m_wrenchFaceMatrix; // for modelling contact
eiquadprog::solvers::EiquadprogFast m_qp1; // saturate wrench
eiquadprog::solvers::EiquadprogFast m_qp2; // distribute wrench
// QP problem matrices -- SSP
Eigen::MatrixXd m_Q1;
Eigen::VectorXd m_C1;
Eigen::MatrixXd m_Aeq1;
Eigen::VectorXd m_Beq1;
Eigen::MatrixXd m_Aineq1;
Eigen::VectorXd m_Bineq1;
Eigen::VectorXd m_result1;
// QP problem matrices -- DSP
Eigen::MatrixXd m_Q2;
Eigen::VectorXd m_C2;
Eigen::MatrixXd m_Aeq2;
Eigen::VectorXd m_Beq2;
Eigen::MatrixXd m_Aineq2;
Eigen::VectorXd m_Bineq2;
Eigen::VectorXd m_result2;
double m_wSum;
double m_wNorm;
double m_wRatio;
Eigen::VectorXd m_wAnkle;
void distributeWrench(const Eigen::VectorXd& wrenchDes, const double rho,
const double mu);
void saturateWrench(const Eigen::VectorXd& wrenchDes, const int phase,
const double mu);
bool m_emergency_stop_triggered;
}; // class DistributeWrench
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_distribute_wrench_H__
include/sot/talos_balance/dummy-dcm-estimator.hh
View file @ e217f893
......@@ -21,73 +21,66 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (dummy_dcm_estimator_EXPORTS)
# define DUMMYDCMESTIMATOR_EXPORT __declspec(dllexport)
# else
# define DUMMYDCMESTIMATOR_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(dummy_dcm_estimator_EXPORTS)
#define DUMMYDCMESTIMATOR_EXPORT __declspec(dllexport)
#else
# define DUMMYDCMESTIMATOR_EXPORT
#define DUMMYDCMESTIMATOR_EXPORT __declspec(dllimport)
#endif
#else
#define DUMMYDCMESTIMATOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include "utils/signal-helper.hh"
#include "utils/logger.hh"
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class DUMMYDCMESTIMATOR_EXPORT DummyDcmEstimator
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
DummyDcmEstimator( const std::string & name );
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
void init();
class DUMMYDCMESTIMATOR_EXPORT DummyDcmEstimator
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(mass, double);
DECLARE_SIGNAL_IN(com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(momenta, dynamicgraph::Vector);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
DECLARE_SIGNAL_OUT(dcm, dynamicgraph::Vector);
/* --- CONSTRUCTOR ---- */
DummyDcmEstimator(const std::string& name);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
void init();
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("[DummyDcmEstimator-"+name+"] "+msg, t, file, line);
}
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(mass, double);
DECLARE_SIGNAL_IN(com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(momenta, dynamicgraph::Vector);
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
DECLARE_SIGNAL_OUT(dcm, dynamicgraph::Vector);
}; // class DummyDcmEstimator
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
}; // class DummyDcmEstimator
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_dummy_dcm_estimator_H__
#endif // #ifndef __sot_talos_balance_dummy_dcm_estimator_H__
include/sot/talos_balance/dummy-walking-pattern-generator.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-talos-balance is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_dummy_walking_pattern_generator_H__
#define __sot_talos_balance_dummy_walking_pattern_generator_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(dummy_walking_pattern_generator_EXPORTS)
#define DUMMYWALKINGPATTERNGENERATOR_EXPORT __declspec(dllexport)
#else
#define DUMMYWALKINGPATTERNGENERATOR_EXPORT __declspec(dllimport)
#endif
#else
#define DUMMYWALKINGPATTERNGENERATOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinocchio first
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-helper.h>
#include <map>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class DUMMYWALKINGPATTERNGENERATOR_EXPORT DummyWalkingPatternGenerator
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
DummyWalkingPatternGenerator(const std::string& name);
void init();
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(rho, double);
DECLARE_SIGNAL_IN(phase, int);
DECLARE_SIGNAL_IN(footLeft, MatrixHomogeneous);
DECLARE_SIGNAL_IN(footRight, MatrixHomogeneous);
DECLARE_SIGNAL_IN(waist, MatrixHomogeneous);
DECLARE_SIGNAL_IN(com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(vcom, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(acom, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(referenceFrame, MatrixHomogeneous);
DECLARE_SIGNAL_INNER(rf, MatrixHomogeneous);
DECLARE_SIGNAL_OUT(comDes, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(vcomDes, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(acomDes, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dcmDes, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmpDes, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(footLeftDes, MatrixHomogeneous);
DECLARE_SIGNAL_OUT(footRightDes, MatrixHomogeneous);
DECLARE_SIGNAL_OUT(waistDes, MatrixHomogeneous);
DECLARE_SIGNAL_OUT(omegaDes, double);
DECLARE_SIGNAL_OUT(rhoDes, double);
DECLARE_SIGNAL_OUT(phaseDes, int);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector actInv(MatrixHomogeneous m, dynamicgraph::Vector v);
MatrixHomogeneous actInv(MatrixHomogeneous m1, MatrixHomogeneous m2);
}; // class DummyWalkingPatternGenerator
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_dummy_walking_pattern_generator_H__
include/sot/talos_balance/euler-to-quat.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-talos-balance is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_euler_to_quat_H__
#define __sot_talos_balance_euler_to_quat_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define EULERTOQUAT_EXPORT __declspec(dllexport)
#else
#define EULERTOQUAT_EXPORT __declspec(dllimport)
#endif
#else
#define EULERTOQUAT_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-helper.h>
#include <map>
#include <sot/core/matrix-geometry.hh>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class EULERTOQUAT_EXPORT EulerToQuat : public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
EulerToQuat(const std::string& name);
void init() {}
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(euler, ::dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(quaternion, ::dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
}; // class EulerToQuat
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_euler_to_quat_H__
include/sot/talos_balance/example.hh
View file @ e217f893
......@@ -21,71 +21,69 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (position_controller_EXPORTS)
# define EXAMPLE_EXPORT __declspec(dllexport)
# else
# define EXAMPLE_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define EXAMPLE_EXPORT __declspec(dllexport)
#else
# define EXAMPLE_EXPORT
#define EXAMPLE_EXPORT __declspec(dllimport)
#endif
#else
#define EXAMPLE_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
#include "utils/signal-helper.hh"
#include "utils/logger.hh"
#include <map>
#include "boost/assign.hpp"
// include pinocchio first
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
#include <dynamic-graph/signal-helper.h>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <map>
#include <sot/core/robot-utils.hh>
class EXAMPLE_EXPORT Example
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
#include "boost/assign.hpp"
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --- CONSTRUCTOR ---- */
Example( const std::string & name );
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
void init();
class EXAMPLE_EXPORT Example : public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(firstAddend, double);
DECLARE_SIGNAL_IN(secondAddend, double);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
DECLARE_SIGNAL_OUT(sum, double);
/* --- CONSTRUCTOR ---- */
Example(const std::string& name);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
void init(const std::string& robotName);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("[Example-"+name+"] "+msg, t, file, line);
}
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(firstAddend, double);
DECLARE_SIGNAL_IN(secondAddend, double);
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
DECLARE_SIGNAL_OUT(sum, double);
DECLARE_SIGNAL_OUT(nbJoints, int);
}; // class Example
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
RobotUtilShrPtr m_robot_util;
}; // class Example
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_example_H__
#endif // #ifndef __sot_talos_balance_example_H__
include/sot/talos_balance/filter-differentiator.hh deleted 100644 → 0
View file @ f4144c44
/*
* Copyright 2017-, Rohan Budhirja, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_torque_control_FilterDifferentiator_H__
#define __sot_torque_control_FilterDifferentiator_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (low_pass_filter_EXPORTS)
# define SOTFILTERDIFFERENTIATOR_EXPORT __declspec(dllexport)
# else
# define SOTFILTERDIFFERENTIATOR_EXPORT __declspec(dllimport)
# endif
#else
# define SOTFILTERDIFFERENTIATOR_EXPORT
#endif
//#define VP_DEBUG 1 /// enable debug output
//#define VP_DEBUG_MODE 20
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* HELPER */
#include <sot/talos_balance/utils/signal-helper.hh>
#include <tsid/utils/stop-watch.hpp>
#include <sot/talos_balance/utils/causal-filter.hh>
#include <sot/talos_balance/utils/logger.hh>
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/**
* This Entity takes as inputs a signal and applies a low pass filter
and computes finite difference derivative.
*/
class SOTFILTERDIFFERENTIATOR_EXPORT FilterDifferentiator
:public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public: /* --- SIGNALS --- */
DECLARE_SIGNAL_IN(x, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(x_filtered, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
/// The following inner signals are used because this entity has some output signals
/// whose related quantities are computed at the same time by the same algorithm
/// To avoid the risk of recomputing the same things twice, we create an inner signal that groups together
/// all the quantities that are computed together. Then the single output signals will depend
/// on this inner signal, which is the one triggering the computations.
/// Inner signals are not exposed, so that nobody can access them.
/// This signal contains the estimated positions, velocities and accelerations.
DECLARE_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector);
protected:
double m_dt; /// sampling timestep of the input signal
int m_x_size;
/// polynomial-fitting filters
CausalFilter* m_filter;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** --- CONSTRUCTOR ---- */
FilterDifferentiator( const std::string & name );
/** Initialize the FilterDifferentiator.
* @param timestep Period (in seconds) after which the sensors' data are updated.
* @param sigSize Size of the input signal.
* @param delay Delay (in seconds) introduced by the estimation.
* This should be a multiple of timestep.
* @note The estimationDelay is half of the length of the window used for the
* polynomial fitting. The larger the delay, the smoother the estimations.
*/
void init(const double &timestep,
const int& xSize,
const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator);
void switch_filter(const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator);
protected:
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("["+name+"] "+msg, t, file, line);
}
public: /* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
}; // class FilterDifferentiator
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_FilterDifferentiator_H__
include/sot/talos_balance/foot-force-difference-controller.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-talos-balance is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_foot_force_difference_controller_H__
#define __sot_talos_balance_foot_force_difference_controller_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(foot_force_difference_controller_EXPORTS)
#define FOOT_FORCE_DIFFERENCE_CONTROLLER_EXPORT __declspec(dllexport)
#else
#define FOOT_FORCE_DIFFERENCE_CONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define FOOT_FORCE_DIFFERENCE_CONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-helper.h>
#include <map>
#include <sot/core/matrix-geometry.hh>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class FOOT_FORCE_DIFFERENCE_CONTROLLER_EXPORT FootForceDifferenceController
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
FootForceDifferenceController(const std::string& name);
void init();
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(phase, int);
DECLARE_SIGNAL_IN(gainSwing, double);
DECLARE_SIGNAL_IN(gainStance, double);
DECLARE_SIGNAL_IN(gainDouble, double);
DECLARE_SIGNAL_IN(dfzAdmittance, double);
DECLARE_SIGNAL_IN(vdcFrequency, double);
DECLARE_SIGNAL_IN(vdcDamping, double);
DECLARE_SIGNAL_IN(swingAdmittance, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrenchRightDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrenchLeftDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrenchRight, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrenchLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posRightDes, MatrixHomogeneous);
DECLARE_SIGNAL_IN(posLeftDes, MatrixHomogeneous);
DECLARE_SIGNAL_IN(posRight, MatrixHomogeneous);
DECLARE_SIGNAL_IN(posLeft, MatrixHomogeneous);
DECLARE_SIGNAL_INNER(dz_ctrl, double);
DECLARE_SIGNAL_INNER(dz_pos, double);
DECLARE_SIGNAL_OUT(vRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(vLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(gainRight, double);
DECLARE_SIGNAL_OUT(gainLeft, double);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
protected:
Eigen::Vector3d calcSwingAdmittance(
const dynamicgraph::Vector& wrench,
const dynamicgraph::Vector& swingAdmittance);
double m_eps;
bool
m_initSucceeded; /// true if the entity has been successfully initialized
}; // class FootForceDifferenceController
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_foot_force_difference_controller_H__
include/sot/talos_balance/ft-calibration.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2019
* LAAS-CNRS
* F. Bailly
* T. Flayols
*/
#ifndef __sot_talos_balance_ft_calibration_H__
#define __sot_talos_balance_ft_calibration_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(__sot_talos_balance_ft_calibration_H__)
#define SOTFTCALIBRATION_EXPORT __declspec(dllexport)
#else
#define SOTFTCALIBRATION_EXPORT __declspec(dllimport)
#endif
#else
#define SOTFTCALIBRATION_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinocchio first
#include <dynamic-graph/real-time-logger.h>
#include <dynamic-graph/signal-helper.h>
#include <map>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
#include <sot/talos_balance/robot/robot-wrapper.hh>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTFTCALIBRATION_EXPORT FtCalibration : public ::dynamicgraph::Entity {
// typedef FtCalibration EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
FtCalibration(const std::string &name);
/// Initialize
void init(const std::string &robotRef);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(right_foot_force_in, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(left_foot_force_in, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_force_out, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_force_out, dynamicgraph::Vector);
/* --- COMMANDS --- */
/// Commands for setting the feet weight
void setRightFootWeight(const double &rightW);
void setLeftFootWeight(const double &leftW);
/// Command to calibrate the foot sensors when the robot is standing in the
/// air with horizontal feet
void calibrateFeetSensor();
void displayRobotUtil();
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
/* --- TYPEDEFS ---- */
typedef Eigen::Matrix<double, 6, 1> Vector6d;
protected:
RobotUtilShrPtr m_robot_util;
int m_right_calibration_iter =
-1; /// Number of iteration left for calibration (-1= not cailbrated,
/// 0=caibration done)
int m_left_calibration_iter =
-1; /// Number of iteration left for calibration (-1= not cailbrated,
/// 0=caibration done)
Vector6d
m_right_FT_offset; /// Offset or bias to be removed from Right FT sensor
Vector6d
m_left_FT_offset; /// Offset or bias to be removed from Left FT sensor
Vector6d m_right_FT_offset_calibration_sum; /// Variable used durring average
/// computation of the offset
Vector6d m_left_FT_offset_calibration_sum; /// Variable used durring average
/// computation of the offset
bool
m_initSucceeded; /// true if the entity has been successfully initialized
Vector6d
m_right_foot_weight; // weight of the right feet underneath the ft sensor
Vector6d
m_left_foot_weight; // weight of the left feet underneath the ft sensor
}; // class FtCalibration
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_ft_calibration_H__
include/sot/talos_balance/ft-wrist-calibration.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2019
* LAAS-CNRS
* F. Bailly
* T. Flayol
* F. Risbourg
*/
#ifndef __sot_talos_balance_ft_wrist_calibration_H__
#define __sot_talos_balance_ft_wrist_calibration_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(__sot_talos_balance_ft_wrist_calibration_H__)
#define SOTFTWRISTCALIBRATION_EXPORT __declspec(dllexport)
#else
#define SOTFTWRISTCALIBRATION_EXPORT __declspec(dllimport)
#endif
#else
#define SOTFTWRISTCALIBRATION_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinocchio first
#include <dynamic-graph/real-time-logger.h>
#include <dynamic-graph/signal-helper.h>
#include <map>
#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/spatial/motion.hpp>
#include <pinocchio/spatial/se3.hpp>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
#include <sot/talos_balance/robot/robot-wrapper.hh>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTFTWRISTCALIBRATION_EXPORT FtWristCalibration
: public ::dynamicgraph::Entity {
// typedef FtCalibration EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
FtWristCalibration(const std::string &name);
/// Initialize
void init(const std::string &robotRef);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(rightWristForceIn, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(leftWristForceIn, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(rightWeight, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(leftWeight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(rightWristForceOut, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(leftWristForceOut, dynamicgraph::Vector);
/* --- COMMANDS --- */
/// Commands for setting the hand weight
void setRightHandConf(const double &rightW, const Vector &rightLeverArm);
void setLeftHandConf(const double &leftW, const Vector &leftLeverArm);
/// Command to calibrate the wrist sensors when the robot is in half sitting
/// with the hands aligned
void calibrateWristSensor();
/**
* @brief Set to true if you want to remove the weight from the force
*
* @param[in] removeWeight Boolean used to remove the weight
*/
void setRemoveWeight(const bool &removeWeight);
void displayRobotUtil();
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
/* --- TYPEDEFS ---- */
typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<double, 3, 1> Vector3d;
protected:
/// Robot Util instance to get the sensor frame
RobotUtilShrPtr m_robot_util;
/// Pinocchio robot model
pinocchio::Model m_model;
/// Pinocchio robot data
pinocchio::Data *m_data;
/// Id of the force sensor frame
pinocchio::FrameIndex m_rightSensorId;
/// Id of the joint of the end-effector
pinocchio::FrameIndex m_leftSensorId;
/// Number of iteration for right calibration (-2 = not calibrated, -1 =
/// caibration done)
int m_rightCalibrationIter = -2;
/// Number of iteration for right calibration (-2 = not calibrated, -1 =
/// caibration done)
int m_leftCalibrationIter = -2;
/// Offset or bias to be removed from Right FT sensor
Vector6d m_right_FT_offset;
/// Offset or bias to be removed from Left FT sensor
Vector6d m_left_FT_offset;
/// Variable used during average computation of the offset
Vector6d m_right_FT_offset_calibration_sum;
/// Variable used during average computation of the offset
Vector6d m_left_FT_offset_calibration_sum;
/// Variable used during average computation of the weight
Vector6d m_right_weight_calibration_sum;
/// Variable used during average computation of the weight
Vector6d m_left_weight_calibration_sum;
/// true if the entity has been successfully initialized
bool m_initSucceeded;
/// weight of the right hand
Vector6d m_rightHandWeight;
/// weight of the left hand
Vector6d m_leftHandWeight;
/// right hand lever arm
Vector3d m_rightLeverArm;
/// left hand lever arm
Vector3d m_leftLeverArm;
/// If true, the weight of the end effector is removed from the force
bool m_removeWeight;
}; // class FtWristCalibration
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_ft_wrist_calibration_H__
include/sot/talos_balance/hip-flexibility-compensation.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-talos-balance is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_hip_flexibility_compensation_H__
#define __sot_talos_balance_hip_flexibility_compensation_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(hip_flexibility_compensation_EXPORTS)
#define HIPFLEXIBILITYCOMPENSATION_EXPORT __declspec(dllexport)
#else
#define HIPFLEXIBILITYCOMPENSATION_EXPORT __declspec(dllimport)
#endif
#else
#define HIPFLEXIBILITYCOMPENSATION_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinocchio first
#include <dynamic-graph/signal-helper.h>
#include <map>
#include <sot/core/robot-utils.hh>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class HIPFLEXIBILITYCOMPENSATION_EXPORT HipFlexibilityCompensation
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
HipFlexibilityCompensation(const std::string& name);
/* --- SIGNALS --- */
/// \brief Walking phase
DECLARE_SIGNAL_IN(phase, int);
/// \brief Desired joint configuration of the robot
DECLARE_SIGNAL_IN(q_des, dynamicgraph::Vector);
/// \brief Current torque mesured at each joint
DECLARE_SIGNAL_IN(tau, dynamicgraph::Vector);
/// \brief Left flexibility correction for the angular computation
DECLARE_SIGNAL_IN(K_l, double);
/// \brief Right flexibility correction for the angular computation
DECLARE_SIGNAL_IN(K_r, double);
/// \brief Derivative gain (double) for the error
// DECLARE_SIGNAL_IN(K_d, double);
/// \brief Low pass filter of the signal tau
DECLARE_SIGNAL_OUT(tau_filt, dynamicgraph::Vector);
/// \brief Angular correction of the flexibility
DECLARE_SIGNAL_OUT(delta_q, dynamicgraph::Vector);
/// \brief Corrected desired joint configuration of the robot with
/// flexibility joint configuration q_cmd = q_des + RateLimiter(delta_q)
DECLARE_SIGNAL_OUT(q_cmd, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
/// \brief Initialize the entity
void init(const double& dt, const std::string& robotName);
/// \brief Set the LowPassFilter frequency for the torque computation.
void setTorqueLowPassFilterFrequency(const double& frequency);
/// \brief Set the value of the saturation for the angular correction
/// computation.
void setAngularSaturation(const double& saturation);
/// \brief Set the value of the limiter for the the rate limiter of delta_q.
void setRateLimiter(const double& rate);
/// \brief Compute the low pass filter of a signal given a frequency and the
/// previous signal.
dynamicgraph::Vector lowPassFilter(const double& frequency,
const dynamicgraph::Vector& signal,
dynamicgraph::Vector& previous_signal);
/// \brief Compute the limiter of a signal given the previous signal (based on
/// first derivative).
void rateLimiter(const dynamicgraph::Vector& signal,
dynamicgraph::Vector& previous_signal,
dynamicgraph::Vector& output);
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
// time step of the robot
double m_dt;
double m_torqueLowPassFilterFrequency;
double m_delta_q_saturation;
double m_rate_limiter;
dynamicgraph::Vector m_previous_delta_q;
dynamicgraph::Vector m_previous_tau;
dynamicgraph::Vector m_limitedSignal;
RobotUtilShrPtr m_robot_util;
}; // class HipFlexibilityCompensation
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_hip_flexibility_compensation_H__
include/sot/talos_balance/imu_offset_compensation.hh deleted 100644 → 0
View file @ f4144c44
/*
* Copyright 2017, Andrea Del Prete, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_torque_control_imu_offset_compensation_H__
#define __sot_torque_control_imu_offset_compensation_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (imu_offset_compensation_EXPORTS)
# define SOTIMUOFFSETCOMPENSATION_EXPORT __declspec(dllexport)
# else
# define SOTIMUOFFSETCOMPENSATION_EXPORT __declspec(dllimport)
# endif
#else
# define SOTIMUOFFSETCOMPENSATION_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <sot/talos_balance/utils/signal-helper.hh>
#include <sot/talos_balance/utils/vector-conversions.hh>
#include <sot/talos_balance/utils/logger.hh>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTIMUOFFSETCOMPENSATION_EXPORT ImuOffsetCompensation
:public::dynamicgraph::Entity
{
typedef ImuOffsetCompensation EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
typedef Eigen::Vector3d Vector3;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
ImuOffsetCompensation( const std::string & name );
/* --- COMMANDS --- */
void init(const double& dt);
void update_offset(const double & duration);
void setGyroDCBlockerParameter(const double & alpha);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(accelerometer_in, dynamicgraph::Vector); /// raw accelerometer data
DECLARE_SIGNAL_IN(gyrometer_in, dynamicgraph::Vector); /// raw gyrometer data
DECLARE_SIGNAL_OUT(accelerometer_out, dynamicgraph::Vector); /// compensated accelerometer data
DECLARE_SIGNAL_OUT(gyrometer_out, dynamicgraph::Vector); /// compensated gyrometer data
protected:
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
/* --- METHODS --- */
void update_offset_impl(int iter);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("[ImuOffsetCompensation-"+name+"] "+msg, t, file, line);
}
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
float m_dt; /// sampling time in seconds
int m_update_cycles_left; /// number of update cycles left
int m_update_cycles; /// total number of update cycles to perform
double m_a_gyro_DC_blocker; /// filter parameter to remove DC from gyro online (should be close to <1.0 and equal to 1.0 for disabling)
Vector3 m_gyro_offset; /// gyrometer offset
Vector3 m_acc_offset; /// accelerometer offset
Vector3 m_gyro_sum; /// tmp variable to store the sum of the gyro measurements during update phase
Vector3 m_acc_sum; /// tmp variable to store the sum of the acc measurements during update phase
}; // class ImuOffsetCompensation
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_imu_offset_compensation_H__