9 #ifndef SOBEC_OCP_WALK_HPP_
10 #define SOBEC_OCP_WALK_HPP_
12 #include <crocoddyl/core/fwd.hpp>
13 #include <crocoddyl/core/solvers/fddp.hpp>
14 #include <crocoddyl/multibody/fwd.hpp>
15 #include <crocoddyl/multibody/states/multibody.hpp>
16 #include <pinocchio/spatial/force.hpp>
22 using namespace crocoddyl;
66 void readParamsFromYamlStr(std::string& StringToParse);
67 void readParamsFromYamlFile(
const std::string& Filename);
71 boost::shared_ptr<pinocchio::Model>
model;
72 boost::shared_ptr<pinocchio::Data>
data;
74 std::map<pinocchio::FrameIndex, pinocchio::FrameIndex> towIds,
heelIds;
80 const std::string& contactKey,
81 const std::string& referencePosture =
"half_sitting");
85 const Eigen::Ref<const Eigen::MatrixXd> contact_pattern,
int duration,
89 typedef typename MathBaseTpl<double>::VectorXs VectorXd;
90 typedef typename MathBaseTpl<double>::VectorXs Vector3d;
91 typedef typename Eigen::Matrix<double, Eigen::Dynamic, 2> MatrixX2d;
92 typedef typename Eigen::Matrix<double, Eigen::Dynamic, 6> MatrixX6d;
93 typedef std::vector<AMA> ActionList;
95 typename crocoddyl::DifferentialActionModelContactFwdDynamicsTpl<double>
99 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
101 explicit OCPWalk(boost::shared_ptr<OCPRobotWrapper> robot,
102 boost::shared_ptr<OCPWalkParams> params,
103 const Eigen::Ref<const Eigen::MatrixXd> contact_pattern);
107 std::vector<AMA> buildRunningModels();
108 AMA buildTerminalModel();
110 std::pair<std::vector<Eigen::VectorXd>, std::vector<Eigen::VectorXd>>
113 void computeReferenceForces();
123 boost::shared_ptr<StateMultibody>
state;
125 boost::shared_ptr<ActuationModelFloatingBase>
actuation;
132 boost::shared_ptr<OCPRobotWrapper>
robot;
virtual ~OCPWalk()
Definition: ocp.hpp:105
std::vector< std::vector< pinocchio::Force > > referenceForces
Definition: ocp.hpp:127
boost::shared_ptr< StateMultibody > state
Keep a direct reference to the terminal state.
Definition: ocp.hpp:123
Eigen::MatrixXd contact_pattern
Definition: ocp.hpp:128
boost::shared_ptr< OCPWalkParams > params
Definition: ocp.hpp:131
boost::shared_ptr< SolverFDDP > solver
the OCP solver.
Definition: ocp.hpp:120
boost::shared_ptr< ShootingProblem > problem
the OCP problem used for solving.
Definition: ocp.hpp:117
boost::shared_ptr< OCPRobotWrapper > robot
Definition: ocp.hpp:132
boost::shared_ptr< ActuationModelFloatingBase > actuation
Definition: ocp.hpp:125
Definition: activation-quad-ref.hpp:19
boost::shared_ptr< crocoddyl::DifferentialActionModelContactFwdDynamics > DAM
Definition: fwd.hpp:114
Eigen::MatrixXd computeWeightShareSmoothProfile(const Eigen::Ref< const Eigen::MatrixXd > contact_pattern, int duration, double saturation)
Definition: ocp-ref-forces.cpp:16
boost::shared_ptr< crocoddyl::ActionModelAbstract > AMA
Definition: fwd.hpp:111
Eigen::VectorXd x0
Definition: ocp.hpp:75
double robotGravityForce
Definition: ocp.hpp:77
Eigen::Vector3d com0
Definition: ocp.hpp:76
boost::shared_ptr< pinocchio::Model > model
Definition: ocp.hpp:71
std::map< pinocchio::FrameIndex, pinocchio::FrameIndex > heelIds
Definition: ocp.hpp:74
std::vector< pinocchio::FrameIndex > contactIds
Definition: ocp.hpp:73
boost::shared_ptr< pinocchio::Data > data
Definition: ocp.hpp:72
OCP builder.
Definition: ocp.hpp:27
double vcomWeight
Definition: ocp.hpp:46
double refMainJointsAtImpactWeight
Definition: ocp.hpp:54
double impactRotationWeight
Definition: ocp.hpp:53
std::vector< std::string > mainJointIds
Definition: ocp.hpp:29
Eigen::VectorXd controlImportance
Definition: ocp.hpp:33
Eigen::VectorXd vcomImportance
Definition: ocp.hpp:34
double footMinimalDistance
Definition: ocp.hpp:59
double flyHighSlope
Definition: ocp.hpp:56
bool withNormalForceBoundOnly
Definition: ocp.hpp:40
double conePenaltyWeight
Definition: ocp.hpp:48
double comWeight
Definition: ocp.hpp:45
double footSize
Definition: ocp.hpp:39
Eigen::Vector2d baumgartGains
Definition: ocp.hpp:30
Eigen::Vector3d vcomRef
Definition: ocp.hpp:37
int transitionDuration
Definition: ocp.hpp:64
Eigen::VectorXd forceImportance
Definition: ocp.hpp:35
Eigen::VectorXd stateTerminalImportance
Definition: ocp.hpp:32
double verticalFootVelWeight
Definition: ocp.hpp:55
double copWeight
Definition: ocp.hpp:47
double coneAxisWeight
Definition: ocp.hpp:49
double impactVelocityWeight
Definition: ocp.hpp:52
double DT
Definition: ocp.hpp:28
double refStateWeight
Definition: ocp.hpp:43
Eigen::VectorXd stateImportance
Definition: ocp.hpp:31
double groundColWeight
Definition: ocp.hpp:58
double solver_th_stop
Definition: ocp.hpp:63
double impactAltitudeWeight
Definition: ocp.hpp:51
double feetCollisionWeight
Definition: ocp.hpp:60
double stateTerminalWeight
Definition: ocp.hpp:62
double refTorqueWeight
Definition: ocp.hpp:44
double kktDamping
Definition: ocp.hpp:61
double flyHighWeight
Definition: ocp.hpp:57
double minimalNormalForce
Definition: ocp.hpp:41
double refForceWeight
Definition: ocp.hpp:50