9#ifndef CROCODDYL_MULTIBODY_UTILS_QUADRUPED_GAITS_HPP_
10#define CROCODDYL_MULTIBODY_UTILS_QUADRUPED_GAITS_HPP_
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>
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/residuals/com-position.hpp"
28#include "crocoddyl/multibody/residuals/frame-translation.hpp"
29#include "crocoddyl/multibody/residuals/frame-velocity.hpp"
30#include "crocoddyl/multibody/residuals/state.hpp"
31#include "crocoddyl/core/residuals/control.hpp"
32#include "crocoddyl/core/activations/weighted-quadratic.hpp"
33#include "crocoddyl/core/optctrl/shooting.hpp"
40 const std::string& lh_foot,
const std::string& rh_foot);
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);
49 std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > createFootStepModels(
50 const double timeStep, Eigen::Vector3d& comPos0, std::vector<Eigen::Vector3d>& feetPos0,
const double stepLength,
51 const double stepHeight,
const std::size_t numKnots,
const std::vector<pinocchio::FrameIndex>& supportFootIds,
52 const std::vector<pinocchio::FrameIndex>& swingFootIds);
54 boost::shared_ptr<ActionModelAbstract> createSwingFootModel(
55 const 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<pinocchio::FrameIndex>& swingFootIds = std::vector<pinocchio::FrameIndex>(),
58 const std::vector<pinocchio::SE3>& swingFootTask = std::vector<pinocchio::SE3>());
60 boost::shared_ptr<ActionModelAbstract> createFootSwitchModel(
61 const std::vector<pinocchio::FrameIndex>& supportFootIds,
const std::vector<pinocchio::FrameIndex>& swingFootIds,
62 const std::vector<pinocchio::SE3>& swingFootTask,
const bool pseudoImpulse =
false);
64 boost::shared_ptr<ActionModelAbstract> createPseudoImpulseModel(
65 const std::vector<pinocchio::FrameIndex>& supportFootIds,
const std::vector<pinocchio::FrameIndex>& swingFootIds,
66 const std::vector<pinocchio::SE3>& swingFootTask);
68 boost::shared_ptr<ActionModelAbstract> createImpulseModel(
const std::vector<pinocchio::FrameIndex>& supportFootIds,
69 const std::vector<pinocchio::FrameIndex>& swingFootIds,
70 const std::vector<pinocchio::SE3>& ref_swingFootTask);
72 const Eigen::VectorXd& get_defaultState()
const;
75 pinocchio::Model rmodel_;
76 pinocchio::Data rdata_;
77 pinocchio::FrameIndex lf_foot_id_, rf_foot_id_, lh_foot_id_, rh_foot_id_;
78 boost::shared_ptr<StateMultibody> state_;
79 boost::shared_ptr<ActuationModelFloatingBase> actuation_;
81 Eigen::VectorXd defaultstate_;