@@ -64,24 +64,28 @@ int main(int argc, char* argv[]) {
6464 const ocs2::vector_t initSystemState = anymalInterface->getInitialState ().head (switched_model::STATE_DIM);
6565
6666 // ====== Scenario settings ========
67- ocs2::scalar_t forwardVelocity{0.6 };
68- ocs2::scalar_t forwardDistance{8.0 };
67+ ocs2::scalar_t forwardVelocity{0.0 };
68+ nodeHandle.getParam (" /forward_velocity"
69+ ocs2::scalar_t gaitDuration{0.8 };
70+ ocs2::scalar_t forwardDistance{3.0 };
6971
7072 ocs2::scalar_t initTime = 0.0 ;
71- ocs2::scalar_t stanceTime = 1.5 ;
72- ocs2::scalar_t finalTime = forwardDistance / forwardVelocity + stanceTime;
73+ ocs2::scalar_t stanceTime = 1.0 ;
74+ int numGaitCycles = std::ceil ((forwardDistance / forwardVelocity) / gaitDuration);
75+ ocs2::scalar_t walkTime = numGaitCycles * gaitDuration;
76+ ocs2::scalar_t finalTime = walkTime + 2 * stanceTime;
7377 ocs2::scalar_t f_mpc = 100 ; // hz
7478
7579 // Load a map
7680 const std::string elevationLayer{" elevation"
7781 const std::string frameId{" world"
78- std::string terrainFile = " gaps.png"
79- double heightScale = 1.0 ;
82+ std::string terrainFile{" "
83+ nodeHandle.getParam (" /terrain_name"
84+ double heightScale{1.0 };
85+ nodeHandle.getParam (" /terrain_scale"
8086 auto gridMap = convex_plane_decomposition::loadGridmapFromImage (terrainFolder + " /"
8187 perceptionConfig.preprocessingParameters .resolution , heightScale);
82- if (terrainFile == " gaps.png"
83- gridMap.get (elevationLayer).array () -= static_cast <float >(heightScale);
84- }
88+ gridMap.get (elevationLayer).array () -= gridMap.atPosition (elevationLayer, {0 ., 0 .});
8589
8690 // Gait
8791 using MN = switched_model::ModeNumber;
@@ -91,19 +95,26 @@ int main(int argc, char* argv[]) {
9195 stance.modeSequence = {MN::STANCE};
9296
9397 switched_model::Gait gait;
94- gait.duration = 0.8 ;
95- gait.eventPhases = {0.45 , 0.5 , 0.95 };
96- gait.modeSequence = {MN::LF_RH, MN::STANCE, MN:: RF_LH, MN::STANCE };
98+ gait.duration = gaitDuration ;
99+ gait.eventPhases = {0.5 };
100+ gait.modeSequence = {MN::LF_RH, MN::RF_LH};
97101
98- GaitSchedule::GaitSequence gaitSequence{stance, gait};
102+ GaitSchedule::GaitSequence gaitSequence{stance};
103+ for (int i = 0 ; i < numGaitCycles; ++i) {
104+ gaitSequence.push_back (gait);
105+ }
106+ gaitSequence.push_back (stance);
99107
100108 // Reference trajectory
109+ bool adaptReferenceToTerrain = true ;
110+ nodeHandle.getParam (" /adaptReferenceToTerrain"
111+
101112 const double dtRef = 0.1 ;
102- const BaseReferenceHorizon commandHorizon{dtRef, size_t ((finalTime - stanceTime) / dtRef) + 1 };
113+ const BaseReferenceHorizon commandHorizon{dtRef, size_t (walkTime / dtRef) + 1 };
103114 BaseReferenceCommand command;
104115 command.baseHeight = getPositionInOrigin (getBasePose (initSystemState)).z ();
105116 command.yawRate = 0.0 ;
106- command.headingVelocity = 0.6 ;
117+ command.headingVelocity = forwardVelocity ;
107118 command.lateralVelocity = 0.0 ;
108119
109120 // ====== Run the perception pipeline ========
@@ -128,11 +139,11 @@ int main(int argc, char* argv[]) {
128139 getOrientation (getBasePose (initSystemState))};
129140
130141 BaseReferenceTrajectory terrainAdaptedBaseReference;
131- if (terrainFile == " gaps.png"
132- terrainAdaptedBaseReference = generateExtrapolatedBaseReference (commandHorizon, initialBaseState, command, TerrainPlane ());
133- } else {
142+ if (adaptReferenceToTerrain) {
134143 terrainAdaptedBaseReference = generateExtrapolatedBaseReference (commandHorizon, initialBaseState, command, planarTerrain.gridMap ,
135144 nominalStanceWidthInHeading, nominalStanceWidthLateral);
145+ } else {
146+ terrainAdaptedBaseReference = generateExtrapolatedBaseReference (commandHorizon, initialBaseState, command, TerrainPlane ());
136147 }
137148
138149 ocs2::TargetTrajectories targetTrajectories;
@@ -154,6 +165,12 @@ int main(int argc, char* argv[]) {
154165 targetTrajectories.stateTrajectory .push_back (std::move (costReference));
155166 targetTrajectories.inputTrajectory .push_back (vector_t::Zero (INPUT_DIM));
156167 }
168+ targetTrajectories.timeTrajectory .push_back (stanceTime + walkTime);
169+ targetTrajectories.timeTrajectory .push_back (finalTime);
170+ targetTrajectories.stateTrajectory .push_back (targetTrajectories.stateTrajectory .back ());
171+ targetTrajectories.stateTrajectory .push_back (targetTrajectories.stateTrajectory .back ());
172+ targetTrajectories.inputTrajectory .push_back (vector_t::Zero (INPUT_DIM));
173+ targetTrajectories.inputTrajectory .push_back (vector_t::Zero (INPUT_DIM));
157174
158175 // ====== Set the scenario to the correct interfaces ========
159176 auto referenceManager = anymalInterface->getQuadrupedInterface ().getSwitchedModelModeScheduleManagerPtr ();
@@ -171,20 +188,22 @@ int main(int argc, char* argv[]) {
171188 const auto mpcSettings = ocs2::mpc::loadSettings (taskFolder + configName + " /task.info"
172189
173190 std::unique_ptr<ocs2::MPC_BASE> mpcPtr;
191+ const auto sqpSettings = ocs2::multiple_shooting::loadSettings (taskFolder + configName + " /multiple_shooting.info"
174192 switch (anymalInterface->modelSettings ().algorithm_ ) {
175193 case switched_model::Algorithm::DDP: {
176194 const auto ddpSettings = ocs2::ddp::loadSettings (taskFolder + configName + " /task.info"
177195 mpcPtr = getDdpMpc (*anymalInterface, mpcSettings, ddpSettings);
178196 break ;
179197 }
180198 case switched_model::Algorithm::SQP: {
181- const auto sqpSettings = ocs2::multiple_shooting::loadSettings (taskFolder + configName + " /multiple_shooting.info"
182199 mpcPtr = getSqpMpc (*anymalInterface, mpcSettings, sqpSettings);
183200 break ;
184201 }
185202 }
186203 ocs2::MPC_MRT_Interface mpcInterface (*mpcPtr);
187204
205+ std::unique_ptr<ocs2::RolloutBase> rollout (anymalInterface->getRollout ().clone ());
206+
188207 // ====== Execute the scenario ========
189208 ocs2::SystemObservation observation;
190209 observation.time = initTime;
@@ -199,6 +218,7 @@ int main(int argc, char* argv[]) {
199218
200219 // run MPC till final time
201220 ocs2::PrimalSolution closedLoopSolution;
221+ std::vector<ocs2::PerformanceIndex> performances;
202222 while (observation.time < finalTime) {
203223 std::cout << " t: " time << " \n "
204224 try {
@@ -207,9 +227,13 @@ int main(int argc, char* argv[]) {
207227 mpcInterface.advanceMpc ();
208228 mpcInterface.updatePolicy ();
209229
210- // Evaluate the optimized solution
230+ performances.push_back (mpcInterface.getPerformanceIndices ());
231+
232+ // Evaluate the optimized solution - change to optimal controller
211233 ocs2::vector_t tmp;
212234 mpcInterface.evaluatePolicy (observation.time , observation.state , tmp, observation.input , observation.mode );
235+ observation.input = ocs2::LinearInterpolation::interpolate (observation.time , mpcInterface.getPolicy ().timeTrajectory_ ,
236+ mpcInterface.getPolicy ().inputTrajectory_ );
213237
214238 closedLoopSolution.timeTrajectory_ .push_back (observation.time );
215239 closedLoopSolution.stateTrajectory_ .push_back (observation.state );
@@ -221,11 +245,17 @@ int main(int argc, char* argv[]) {
221245 closedLoopSolution.modeSchedule_ .eventTimes .push_back (observation.time - 0.5 / f_mpc);
222246 }
223247
224- // Forward stepping: use optimal state for the next observation:
225- observation.time += 1.0 / f_mpc;
226- size_t tmpMode;
227- mpcInterface.evaluatePolicy (observation.time , ocs2::vector_t::Zero (switched_model_loopshaping::STATE_DIM), observation.state , tmp,
228- tmpMode);
248+ // perform a rollout
249+ scalar_array_t timeTrajectory;
250+ size_array_t postEventIndicesStock;
251+ vector_array_t stateTrajectory, inputTrajectory;
252+ const scalar_t finalTime = observation.time + 1.0 / f_mpc;
253+ auto modeschedule = mpcInterface.getPolicy ().modeSchedule_ ;
254+ rollout->run (observation.time , observation.state , finalTime, mpcInterface.getPolicy ().controllerPtr_ .get (), modeschedule,
255+ timeTrajectory, postEventIndicesStock, stateTrajectory, inputTrajectory);
256+
257+ observation.time = finalTime;
258+ observation.state = stateTrajectory.back ();
229259 observation.input .setZero (switched_model_loopshaping::INPUT_DIM); // reset
230260 } catch (std::exception& e) {
231261 std::cout << " MPC failed\n "
@@ -236,6 +266,36 @@ int main(int argc, char* argv[]) {
236266 const auto closedLoopSystemSolution =
237267 ocs2::loopshapingToSystemPrimalSolution (closedLoopSolution, *anymalInterface->getLoopshapingDefinition ());
238268
269+ // ====== Print result ==========
270+ const auto totalCost = std::accumulate (performances.cbegin (), performances.cend (), 0.0 ,
271+ [](double v, const ocs2::PerformanceIndex& p) { return v + p.cost ; });
272+ const auto totalDynamics =
273+ std::accumulate (performances.cbegin (), performances.cend (), 0.0 ,
274+ [](double v, const ocs2::PerformanceIndex& p) { return v + std::sqrt (p.dynamicsViolationSSE ); });
275+ const auto maxDynamics = std::sqrt (std::max_element (performances.cbegin (), performances.cend (),
276+ [](const ocs2::PerformanceIndex& lhs, const ocs2::PerformanceIndex& rhs) {
277+ return lhs.dynamicsViolationSSE < rhs.dynamicsViolationSSE ;
278+ })
279+ ->dynamicsViolationSSE );
280+ const auto totalEquality =
281+ std::accumulate (performances.cbegin (), performances.cend (), 0.0 ,
282+ [](double v, const ocs2::PerformanceIndex& p) { return v + std::sqrt (p.equalityConstraintsSSE ); });
283+ const auto maxEquality = std::sqrt (std::max_element (performances.cbegin (), performances.cend (),
284+ [](const ocs2::PerformanceIndex& lhs, const ocs2::PerformanceIndex& rhs) {
285+ return lhs.equalityConstraintsSSE < rhs.equalityConstraintsSSE ;
286+ })
287+ ->equalityConstraintsSSE );
288+
289+ double achievedWalkTime = observation.time - stanceTime;
290+ std::cout << " Speed: " " \n "
291+ std::cout << " Scale: " " \n "
292+ std::cout << " Completed: " std::min (1.0 , (achievedWalkTime / walkTime)) * 100.0 << " \n "
293+ std::cout << " average Cost: " size () << " \n "
294+ std::cout << " average Dynamics constr: " size () << " \n "
295+ std::cout << " max Dynamics constr: " " \n "
296+ std::cout << " average Equality constr: " size () << " \n "
297+ std::cout << " max Equality constr: " " \n "
298+
239299 // ====== Visualize ==========
240300 QuadrupedVisualizer visualizer (anymalInterface->getKinematicModel (), anymalInterface->getJointNames (), anymalInterface->getBaseName (),
241301 nodeHandle);
0 commit comments