sot-talos-balance  1.7.0
distribute-wrench.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2018, Gepetto team, LAAS-CNRS
3  *
4  * This file is part of sot-talos-balance.
5  * sot-talos-balance is free software: you can redistribute it and/or
6  * modify it under the terms of the GNU Lesser General Public License
7  * as published by the Free Software Foundation, either version 3 of
8  * the License, or (at your option) any later version.
9  * sot-talos-balance is distributed in the hope that it will be
10  * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details. You should
13  * have received a copy of the GNU Lesser General Public License along
14  * with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
15  */
16 
17 #ifndef __sot_talos_balance_distribute_wrench_H__
18 #define __sot_talos_balance_distribute_wrench_H__
19 
20 /* --------------------------------------------------------------------- */
21 /* --- API ------------------------------------------------------------- */
22 /* --------------------------------------------------------------------- */
23 
24 #if defined (WIN32)
25 # if defined (position_controller_EXPORTS)
26 # define DISTRIBUTE_WRENCH_EXPORT __declspec(dllexport)
27 # else
28 # define DISTRIBUTE_WRENCH_EXPORT __declspec(dllimport)
29 # endif
30 #else
31 # define DISTRIBUTE_WRENCH_EXPORT
32 #endif
33 
34 
35 /* --------------------------------------------------------------------- */
36 /* --- INCLUDE --------------------------------------------------------- */
37 /* --------------------------------------------------------------------- */
38 
39 #include <pinocchio/fwd.hpp>
40 #include <dynamic-graph/signal-helper.h>
41 
42 #include <map>
43 #include "boost/assign.hpp"
44 #include <sot/core/robot-utils.hh>
45 
46 #include <pinocchio/multibody/model.hpp>
47 #include <pinocchio/multibody/data.hpp>
48 
49 #include <eigen-quadprog/QuadProg.h>
50 
51 namespace dynamicgraph {
52  namespace sot {
53  namespace talos_balance {
54 
55  /* --------------------------------------------------------------------- */
56  /* --- CLASS ----------------------------------------------------------- */
57  /* --------------------------------------------------------------------- */
58 
60  : public ::dynamicgraph::Entity
61  {
62  DYNAMIC_GRAPH_ENTITY_DECL();
63 
64  public:
65  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66 
67  /* --- CONSTRUCTOR ---- */
68  DistributeWrench( const std::string & name );
69 
70  void init(const std::string& robotName);
71 
72  /* --- SIGNALS --- */
73  DECLARE_SIGNAL_IN(wrenchDes, dynamicgraph::Vector);
74  DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
75  DECLARE_SIGNAL_IN(rho, double);
76  DECLARE_SIGNAL_IN(phase, int);
77  DECLARE_SIGNAL_IN(frictionCoefficient, double);
78 
79  DECLARE_SIGNAL_IN(wSum, double);
80  DECLARE_SIGNAL_IN(wNorm, double);
81  DECLARE_SIGNAL_IN(wRatio, double);
82  DECLARE_SIGNAL_IN(wAnkle, dynamicgraph::Vector);
83 
84  DECLARE_SIGNAL_INNER(kinematics_computations, int);
85  DECLARE_SIGNAL_INNER(qp_computations, int);
86 
87  DECLARE_SIGNAL_OUT(wrenchLeft, dynamicgraph::Vector);
88  DECLARE_SIGNAL_OUT(ankleWrenchLeft, dynamicgraph::Vector);
89  DECLARE_SIGNAL_OUT(surfaceWrenchLeft, dynamicgraph::Vector);
90  DECLARE_SIGNAL_OUT(copLeft, dynamicgraph::Vector);
91  DECLARE_SIGNAL_OUT(wrenchRight, dynamicgraph::Vector);
92  DECLARE_SIGNAL_OUT(ankleWrenchRight, dynamicgraph::Vector);
93  DECLARE_SIGNAL_OUT(surfaceWrenchRight, dynamicgraph::Vector);
94  DECLARE_SIGNAL_OUT(copRight, dynamicgraph::Vector);
95 
96  DECLARE_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector);
97  DECLARE_SIGNAL_OUT(zmpRef, dynamicgraph::Vector);
98  DECLARE_SIGNAL_OUT(emergencyStop, bool);
99 
100  public:
101  /* --- COMMANDS --- */
102  /* --- ENTITY INHERITANCE --- */
103  virtual void display( std::ostream& os ) const;
104 
105  Eigen::Vector3d computeCoP(const dynamicgraph::Vector & wrench, const pinocchio::SE3 & pose) const;
106 
107  void set_right_foot_sizes(const dynamicgraph::Vector & s);
108  void set_left_foot_sizes(const dynamicgraph::Vector & s);
109 
110  double m_eps;
111 
112  protected:
114  pinocchio::Model m_model;
115  pinocchio::Data m_data;
116  RobotUtilShrPtr m_robot_util;
117 
118 // pinocchio::SE3 m_ankle_M_ftSens; /// ankle to F/T sensor transformation
119  pinocchio::SE3 m_ankle_M_sole;
120 
121  pinocchio::FrameIndex m_left_foot_id;
122  pinocchio::FrameIndex m_right_foot_id;
123 
124  pinocchio::SE3 m_contactLeft;
125  pinocchio::SE3 m_contactRight;
126 
127  Eigen::Matrix<double,6,1> m_wrenchLeft;
128  Eigen::Matrix<double,6,1> m_wrenchRight;
129 
130  Eigen::Vector4d m_left_foot_sizes;
131  Eigen::Vector4d m_right_foot_sizes;
132 
133  void computeWrenchFaceMatrix(const double mu);
134  Eigen::Matrix<double, 16, 6> m_wrenchFaceMatrix; // for modelling contact
135 
136  Eigen::QuadProgDense m_qp1; // saturate wrench
137  Eigen::QuadProgDense m_qp2; // distribute wrench
138 
139  // QP problem matrices -- SSP
140  Eigen::MatrixXd m_Q1;
141  Eigen::VectorXd m_C1;
142 
143  Eigen::MatrixXd m_Aeq1;
144  Eigen::VectorXd m_Beq1;
145 
146  Eigen::MatrixXd m_Aineq1;
147  Eigen::VectorXd m_Bineq1;
148 
149  // QP problem matrices -- DSP
150  Eigen::MatrixXd m_Q2;
151  Eigen::VectorXd m_C2;
152 
153  Eigen::MatrixXd m_Aeq2;
154  Eigen::VectorXd m_Beq2;
155 
156  Eigen::MatrixXd m_Aineq2;
157  Eigen::VectorXd m_Bineq2;
158 
159  double m_wSum;
160  double m_wNorm;
161  double m_wRatio;
162  Eigen::VectorXd m_wAnkle;
163 
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);
166 
168  }; // class DistributeWrench
169 
170  } // namespace talos_balance
171  } // namespace sot
172 } // namespace dynamicgraph
173 
174 
175 
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
Definition: math/fwd.hh:40
pinocchio::Data m_data
Pinocchio robot model.
#define DISTRIBUTE_WRENCH_EXPORT
pinocchio::FrameIndex m_left_foot_id
ankle to sole transformation
RobotUtilShrPtr m_robot_util
Pinocchio robot data.
pinocchio::Model m_model
true if the entity has been successfully initialized