17 #ifndef __sot_talos_balance_distribute_wrench_H__ 18 #define __sot_talos_balance_distribute_wrench_H__ 25 # if defined (position_controller_EXPORTS) 26 # define DISTRIBUTE_WRENCH_EXPORT __declspec(dllexport) 28 # define DISTRIBUTE_WRENCH_EXPORT __declspec(dllimport) 31 # define DISTRIBUTE_WRENCH_EXPORT 39 #include <pinocchio/fwd.hpp> 40 #include <dynamic-graph/signal-helper.h> 43 #include "boost/assign.hpp" 44 #include <sot/core/robot-utils.hh> 46 #include <pinocchio/multibody/model.hpp> 47 #include <pinocchio/multibody/data.hpp> 49 #include <eigen-quadprog/QuadProg.h> 53 namespace talos_balance {
60 :
public ::dynamicgraph::Entity
62 DYNAMIC_GRAPH_ENTITY_DECL();
65 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
70 void init(
const std::string& robotName);
75 DECLARE_SIGNAL_IN(rho,
double);
76 DECLARE_SIGNAL_IN(phase,
int);
77 DECLARE_SIGNAL_IN(frictionCoefficient,
double);
79 DECLARE_SIGNAL_IN(wSum,
double);
80 DECLARE_SIGNAL_IN(wNorm,
double);
81 DECLARE_SIGNAL_IN(wRatio,
double);
84 DECLARE_SIGNAL_INNER(kinematics_computations,
int);
85 DECLARE_SIGNAL_INNER(qp_computations,
int);
98 DECLARE_SIGNAL_OUT(emergencyStop,
bool);
103 virtual void display( std::ostream& os )
const;
105 Eigen::Vector3d computeCoP(
const dynamicgraph::Vector & wrench,
const pinocchio::SE3 & pose)
const;
133 void computeWrenchFaceMatrix(
const double mu);
164 void distributeWrench(
const Eigen::VectorXd & wrenchDes,
const double rho,
const double mu);
165 void saturateWrench(
const Eigen::VectorXd & wrenchDes,
const int phase,
const double mu);
176 #endif // #ifndef __sot_talos_balance_distribute_wrench_H__
Eigen::Vector4d m_right_foot_sizes
sizes of the left foot (pos x, neg x, pos y, neg y)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
pinocchio::Data m_data
Pinocchio robot model.
Eigen::Vector4d m_left_foot_sizes
pinocchio::SE3 m_contactRight
#define DISTRIBUTE_WRENCH_EXPORT
Eigen::Matrix< double, 16, 6 > m_wrenchFaceMatrix
pinocchio::FrameIndex m_left_foot_id
ankle to sole transformation
pinocchio::SE3 m_contactLeft
pinocchio::FrameIndex m_right_foot_id
Eigen::Matrix< double, 6, 1 > m_wrenchRight
RobotUtilShrPtr m_robot_util
Pinocchio robot data.
Eigen::QuadProgDense m_qp1
Eigen::Matrix< double, 6, 1 > m_wrenchLeft
Eigen::QuadProgDense m_qp2
pinocchio::Model m_model
true if the entity has been successfully initialized
bool m_emergency_stop_triggered
pinocchio::SE3 m_ankle_M_sole