relative-feet-inequalities.hh

Go to the documentation of this file.

/*
 * Copyright 2010,
 *
 * Mehdi      Benallegue
 * Andrei     Herdt
 * Olivier    Stasse
 *
 *
 * JRL, CNRS/AIST
 *
 * This file is part of walkGenJrl.
 * walkGenJrl 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.
 *
 * walkGenJrl 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 General Lesser Public License for more details.
 * You should have received a copy of the GNU Lesser General Public License
 * along with walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
 *
 *  Research carried out within the scope of the
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */
/* This object generate matrix representation of linear
   constraint based on foot position.
   It handles a stack of constraint on a sliding mode
   for QP solving. */
 
#ifndef _RELATIVE_FEET_INEQUALITIES_
#define _RELATIVE_FEET_INEQUALITIES_
 
#include <Mathematics/ConvexHull.hh>
#include <Mathematics/FootHalfSize.hh>
#include <PreviewControl/SupportFSM.hh>
#include <SimplePlugin.hh>
#include <jrl/walkgen/pgtypes.hh>
#include <jrl/walkgen/pinocchiorobot.hh>
#include <privatepgtypes.hh>
 
namespace PatternGeneratorJRL {
 
class RelativeFeetInequalities : public SimplePlugin {
  //
  // Public types
  //
 public:
  //
  // Public member functions
  //
 public:
  RelativeFeetInequalities(SimplePluginManager *aSPM, PinocchioRobot *aPR);
  ~RelativeFeetInequalities();
 
  void set_vertices(convex_hull_t &ConvexHull,
                    const support_state_t &SupportState, ineq_e type);
 
  void set_inequalities(convex_hull_t &ConvexHull,
                        const support_state_t &Support, ineq_e type);
 
  void compute_linear_system(convex_hull_t &ConvexHull,
                             const support_state_t &PrwSupport) const;
 
  virtual void CallMethod(std::string &Method, std::istringstream &Args);
 
  void getFeetSize(FootHalfSize &leftFootSize, FootHalfSize &rightFootSize);
 
  inline double DSFeetDistance() { return DSFeetDistance_; }
 
  //
  // Private member functions
  //
 private:
  int init_convex_hulls();
 
  int set_feet_dimensions(PinocchioRobot *aPR);
 
  int init_feet_constraints();
 
  //
  // Private members
  //
 private:
  struct edges_s {
    convex_hull_t LeftSS, RightSS, RightDS, LeftDS;
  };
  struct edges_s FootPosEdges_, ZMPPosEdges_;
 
  convex_hull_t CoMHull_;
 
  double LeftFPosEdgesX_[5], LeftFPosEdgesY_[5];
  double RightFPosEdgesX_[5], RightFPosEdgesY_[5];
 
  FootHalfSize LeftFootSize_, RightFootSize_;
 
  double SecurityMarginX_;
  double SecurityMarginY_;
 
  double DSFeetDistance_;
};
}  // namespace PatternGeneratorJRL
#endif /* _RELATIVE_FEET_INEQUALITIES_ */