crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
quadruped-gaits.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019, LAAS-CNRS
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_UTILS_QUADRUPED_GAITS_HPP_
10 #define CROCODDYL_MULTIBODY_UTILS_QUADRUPED_GAITS_HPP_
11 
12 #include <pinocchio/spatial/se3.hpp>
13 #include <pinocchio/multibody/model.hpp>
14 #include <pinocchio/multibody/frame.hpp>
15 #include <pinocchio/algorithm/frames.hpp>
16 #include <pinocchio/algorithm/center-of-mass.hpp>
17 #include <pinocchio/algorithm/kinematics.hpp>
18 
19 #include "crocoddyl/core/fwd.hpp"
20 #include "crocoddyl/multibody/fwd.hpp"
21 #include "crocoddyl/multibody/actions/contact-fwddyn.hpp"
22 #include "crocoddyl/multibody/actions/impulse-fwddyn.hpp"
23 #include "crocoddyl/core/integrator/euler.hpp"
24 #include "crocoddyl/multibody/actuations/floating-base.hpp"
25 #include "crocoddyl/multibody/contacts/contact-3d.hpp"
26 #include "crocoddyl/multibody/impulses/impulse-3d.hpp"
27 #include "crocoddyl/multibody/costs/com-position.hpp"
28 #include "crocoddyl/multibody/costs/frame-translation.hpp"
29 #include "crocoddyl/multibody/costs/frame-velocity.hpp"
30 #include "crocoddyl/multibody/costs/state.hpp"
31 #include "crocoddyl/core/costs/control.hpp"
32 #include "crocoddyl/core/activations/weighted-quadratic.hpp"
33 #include "crocoddyl/core/optctrl/shooting.hpp"
34 
35 namespace crocoddyl {
36 
38  public:
39  SimpleQuadrupedGaitProblem(const pinocchio::Model& rmodel, const std::string& lf_foot, const std::string& rf_foot,
40  const std::string& lh_foot, const std::string& rh_foot);
42 
43  boost::shared_ptr<crocoddyl::ShootingProblem> createWalkingProblem(const Eigen::VectorXd& x0,
44  const double stepLength, const double stepHeight,
45  const double timeStep,
46  const std::size_t stepKnots,
47  const std::size_t supportKnots);
48 
49  std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > createFootStepModels(
50  double timeStep, Eigen::Vector3d& comPos0, std::vector<Eigen::Vector3d>& feetPos0, double stepLength,
51  double stepHeight, std::size_t numKnots, const std::vector<pinocchio::FrameIndex>& supportFootIds,
52  const std::vector<pinocchio::FrameIndex>& swingFootIds);
53 
54  boost::shared_ptr<ActionModelAbstract> createSwingFootModel(
55  double timeStep, const std::vector<pinocchio::FrameIndex>& supportFootIds,
56  const Eigen::Vector3d& comTask = Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity()),
57  const std::vector<crocoddyl::FramePlacement>& swingFootTask = std::vector<crocoddyl::FramePlacement>());
58 
59  boost::shared_ptr<ActionModelAbstract> createFootSwitchModel(
60  const std::vector<pinocchio::FrameIndex>& supportFootIds, const std::vector<FramePlacement>& swingFootTask,
61  bool pseudoImpulse = false);
62 
63  boost::shared_ptr<ActionModelAbstract> createPseudoImpulseModel(
64  const std::vector<pinocchio::FrameIndex>& supportFootIds, const std::vector<FramePlacement>& swingFootTask);
65 
66  boost::shared_ptr<ActionModelAbstract> createImpulseModel(const std::vector<pinocchio::FrameIndex>& supportFootIds,
67  const std::vector<FramePlacement>& swingFootTask);
68 
69  const Eigen::VectorXd& get_defaultState() const;
70 
71  protected:
72  pinocchio::Model rmodel_;
73  pinocchio::Data rdata_;
74  pinocchio::FrameIndex lf_foot_id_, rf_foot_id_, lh_foot_id_, rh_foot_id_;
75  boost::shared_ptr<StateMultibody> state_;
76  boost::shared_ptr<ActuationModelFloatingBase> actuation_;
77  bool firtstep_;
78  Eigen::VectorXd defaultstate_;
79 };
80 } // namespace crocoddyl
81 
82 #endif // CROCODDYL_MULTIBODY_UTILS_QUADRUPED_GAITS_HPP_