Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <cmath>
00031
00032 #include <towr/towr.h>
00033 #include <towr/models/centroidal_model.h>
00034
00035
00036 using namespace towr;
00037
00038
00039
00040 class FlatGround : public HeightMap {
00041 public:
00042 virtual double GetHeight(double x, double y) const override { return 0.0; };
00043 };
00044
00045
00046
00047 class MonopedKinematicModel : public KinematicModel {
00048 public:
00049 MonopedKinematicModel () : KinematicModel(1)
00050 {
00051 nominal_stance_.at(0) = Eigen::Vector3d( 0.0, 0.0, -0.58);
00052 max_dev_from_nominal_ << 0.25, 0.15, 0.2;
00053 }
00054 };
00055
00056
00057
00058 class MonopedDynamicModel : public CentroidalModel {
00059 public:
00060 MonopedDynamicModel()
00061 : CentroidalModel(20,
00062 1.2, 5.5, 6.0, 0.0, -0.2, -0.01,
00063 1) {}
00064 };
00065
00066
00067 int main()
00068 {
00069
00070 RobotModel model;
00071 model.dynamic_model_ = std::make_shared<MonopedDynamicModel>();
00072 model.kinematic_model_ = std::make_shared<MonopedKinematicModel>();
00073
00074
00075
00076 BaseState initial_base;
00077 initial_base.lin.at(kPos).z() = 0.5;
00078 Eigen::Vector3d initial_foot_pos_W = Eigen::Vector3d::Zero();
00079
00080
00081
00082 BaseState goal;
00083 goal.lin.at(towr::kPos) << 1.0, 0.0, 0.5;
00084
00085
00086
00087
00088 Parameters params;
00089 params.t_total_ = 1.6;
00090
00091
00092
00093 params.ee_phase_durations_.push_back({0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.2});
00094 params.ee_in_contact_at_start_.push_back(true);
00095
00096
00097
00098 TOWR towr;
00099 towr.SetInitialState(initial_base, {initial_foot_pos_W});
00100 towr.SetParameters(goal, params, model, std::make_shared<FlatGround>());
00101
00102 towr.SolveNLP();
00103 auto x = towr.GetSolution();
00104
00105
00106
00107 using namespace std;
00108 cout.precision(2);
00109 cout << fixed;
00110 cout << "\n====================\nMonoped trajectory:\n====================\n";
00111
00112 double t = 0.0;
00113 while (t<=params.t_total_+1e-5) {
00114
00115 cout << "t=" << t << "\n";
00116 cout << "Base linear position x,y,z: \t";
00117 cout << x.base_linear_->GetPoint(t).p().transpose() << "\t[m]" << endl;
00118
00119 cout << "Base Euler roll, pitch, yaw: \t";
00120 Eigen::Vector3d rad = x.base_angular_->GetPoint(t).p();
00121 cout << (rad/M_PI*180).transpose() << "\t[deg]" << endl;
00122
00123 cout << "Foot position x,y,z: \t";
00124 cout << x.ee_motion_.at(0)->GetPoint(t).p().transpose() << "\t[m]" << endl;
00125
00126 cout << "Contact force x,y,z: \t";
00127 cout << x.ee_force_.at(0)->GetPoint(t).p().transpose() << "\t[N]" << endl;
00128
00129 bool contact = x.phase_durations_.at(0)->IsContactPhase(t);
00130 std::string foot_in_contact = contact? "yes" : "no";
00131 cout << "Foot in contact: \t" + foot_in_contact << endl;
00132
00133 cout << endl;
00134
00135 t += 0.2;
00136 }
00137 }