crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
quadruped-gaits.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-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/multibody/fwd.hpp"
20 #include "crocoddyl/multibody/actions/contact-fwddyn.hpp"
21 #include "crocoddyl/multibody/actions/impulse-fwddyn.hpp"
22 #include "crocoddyl/core/integrator/euler.hpp"
23 #include "crocoddyl/multibody/actuations/floating-base.hpp"
24 #include "crocoddyl/multibody/contacts/contact-3d.hpp"
25 #include "crocoddyl/multibody/impulses/impulse-3d.hpp"
26 #include "crocoddyl/multibody/costs/com-position.hpp"
27 #include "crocoddyl/multibody/costs/frame-translation.hpp"
28 #include "crocoddyl/multibody/costs/frame-velocity.hpp"
29 #include "crocoddyl/multibody/costs/state.hpp"
30 #include "crocoddyl/multibody/costs/control.hpp"
31 #include "crocoddyl/core/activations/weighted-quadratic.hpp"
32 #include "crocoddyl/core/optctrl/shooting.hpp"
33 
34 namespace crocoddyl {
35 
37  public:
38  SimpleQuadrupedGaitProblem(const pinocchio::Model& rmodel, const std::string& lf_foot, const std::string& rf_foot,
39  const std::string& lh_foot, const std::string& rh_foot);
41 
42  boost::shared_ptr<crocoddyl::ShootingProblem> createWalkingProblem(const Eigen::VectorXd& x0,
43  const double stepLength, const double stepHeight,
44  const double timeStep,
45  const std::size_t stepKnots,
46  const std::size_t supportKnots);
47 
48  std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > createFootStepModels(
49  double timeStep, Eigen::Vector3d& comPos0, std::vector<Eigen::Vector3d>& feetPos0, double stepLength,
50  double stepHeight, std::size_t numKnots, const std::vector<pinocchio::FrameIndex>& supportFootIds,
51  const std::vector<pinocchio::FrameIndex>& swingFootIds);
52 
53  boost::shared_ptr<ActionModelAbstract> createSwingFootModel(
54  double timeStep, const std::vector<pinocchio::FrameIndex>& supportFootIds,
55  const Eigen::Vector3d& comTask = Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity()),
56  const std::vector<crocoddyl::FramePlacement>& swingFootTask = std::vector<crocoddyl::FramePlacement>());
57 
58  boost::shared_ptr<ActionModelAbstract> createFootSwitchModel(
59  const std::vector<pinocchio::FrameIndex>& supportFootIds, const std::vector<FramePlacement>& swingFootTask,
60  bool pseudoImpulse = false);
61 
62  boost::shared_ptr<ActionModelAbstract> createPseudoImpulseModel(
63  const std::vector<pinocchio::FrameIndex>& supportFootIds, const std::vector<FramePlacement>& swingFootTask);
64 
65  boost::shared_ptr<ActionModelAbstract> createImpulseModel(const std::vector<pinocchio::FrameIndex>& supportFootIds,
66  const std::vector<FramePlacement>& swingFootTask);
67 
68  const Eigen::VectorXd& get_defaultState() const;
69 
70  protected:
71  pinocchio::Model rmodel_;
72  pinocchio::Data rdata_;
73  pinocchio::FrameIndex lf_foot_id_, rf_foot_id_, lh_foot_id_, rh_foot_id_;
74  boost::shared_ptr<StateMultibody> state_;
75  boost::shared_ptr<ActuationModelFloatingBase> actuation_;
76  bool firtstep_;
77  Eigen::VectorXd defaultstate_;
78 };
79 } // namespace crocoddyl
80 
81 #endif // CROCODDYL_MULTIBODY_UTILS_QUADRUPED_GAITS_HPP_