Efficiency with larger legged systems

[IoannisDadiotis]

Dear ocs2 development team,

First and foremost congratulations for the great work and thank you for sharing this with the community.

I am exploring ocs2 for our quadruped bi-manual Centauro robot which is quite challenging due to the dimension of the system (total 39 actuated DoFs). I would like to ask if you could please provide any insights about the efficiency of the framework for larger systems? E.g. how large systems can ocs2 handle for real-time planning based on your experience? Which solver is more promising in this direction? As far as I know from your published work ocs2 has shown to be efficient for a system including Anymal C + 4 DoF arm + manipulated object dynamics (real-time MPC found in https://arxiv.org/pdf/2103.00946.pdf), but this is still quite far from the Centauro case.

I tried to create a simple example for our robot similar to your legged robot example using the continuous DDP and SRBD model. For now I have kept exactly the same constraints with your legged robot example with a different cost (instead of penalizing feet EE cartesian velocities the whole control input vector is penalized). Only the 4 feet EE are considered as contact points. I have tuned some task parameters so as to target motion suitable for the specific robot (in short I have set smaller touchDownVelocity, liftOffVelocity, targetDisplacementVelocity, mpcDesiredFrequency, larger eventTimes and switchingTimes). You can check my full .info files here. As you can see on the video below I am getting the following output: WARNING: The solution time window might be shorter than the MPC delay!.

https://user-images.githubusercontent.com/75118133/160574989-4a757d69-b91a-4dc6-b17f-b7d1f6717152.mp4

Any advice on how to increase the efficiency of the current formulation?

Finally I would like to ask if it is possible to configure the framework for offline trajectory optimization and what is the suggested changes in the code for this?

Thank you in advance!

[rubengrandia]

Hi Ioannis,

Great to see an application to Centauro! That is indeed a large amount of DoF. With the multiple shooting solver we have been running systems with a state dimension of 48 and input dimension of 24 with less than 10ms per update. Those are the numbers I have on the top of my head, but in general the DDP solver should scale in a similar fashion. The 2-3 seconds I see in the bottom left screen seems too much.

In your task file I see you have checkNumericalStability: true. This is great for development because it verifies numerical assumptions. However, it does expensive eigenvalues checks. If you want to go fast you should set this to false.

Furthermore, you can enable some more prints to get an insight into which part is slow: displayInfo: true displayShortSummary: true Keep an eye out for the average stepsize taken by the adaptive stepping scheme in DDP. If the problem is stiff, it might take many steps. The short summary should reports where in the solver you are spending time.

To do offline trajectories I would recommend to instantiate a solver object directly. Have a look at the MPC_BASE and MPC_DDP how those are calling the solver. That should give a hint on how you might use it yourself.

[IoannisDadiotis]

Hi Ruben,

Thanks for the useful advice.

I managed to have the MPC running with the robot in place, as you can see in the video below. I used the gaits and the horizon 1 sec. of your example. This is with the SRBD but I managed to have something similar with Centroidal dynamics (although for now i did not investigate a lot on this).

https://user-images.githubusercontent.com/75118133/161511326-85b5c6aa-2d25-4127-9a3e-d666f17ad570.mp4

Although in the following I plan to explore planning of slower gaits for our robot I would like to report few things from the experience I had so far:

1. For lower frequencies the DDP controller does not generate a stable rollout. E.g. at 10 Hz the static_walk (cycle duration 1.2 sec) does not run while the standing_trot (with a shorter cycle duration 0.7 sec) runs. I thought this can make sense due to the fact that a lower MPC frequency corresponds to a larger shift of the planning horizon (i.e. the problem to be optimized gets more different with respect to the one of the previous optimization). Do you think that this is really the case?
2. When setting the desired MPC frequency to very high, e.g. 80-100 Hz, the robot on rviz moves slower, like if it is slow downed, although the gait to be planned is the exactly the same. Do you have any idea where is this difference coming from?
3. I haven't managed to set target displacements on centauro. Even at stance, when entering a target displacement 0.05 m at the longitudinal direction (with targetDisplacementVelocity=0.05 ) the terminal of the legged_robot_target node closes and the robot does not move.

[farbod-farshidian]

Answer to Q1) This is hard to say why. Slower gaits are often more stable and less sensitive to MPC frequency. Moreover, they usually have more state-input equalities, which means that the effective dimension of the problem is lower since we use projection to resolve these constraints. However, you might have some issues with constraint satisfaction. Check this in the solver printouts. Another suspect here can be the initialization scheme (ocs2_core/include/ocs2_core/initialization/Initializer.h). As I said, it is tough to provide a definitive answer with this little bit of information.

Answer to Q2) When testing in the dummy simulator, we artificially slow down the simulator to ensure that the ratio of mrtDesiredFrequency/mpcDesiredFrequency remains constant. For example for mpcDesiredFrequency=50 [Hz] and mrtDesiredFrequency=400[Hz], for every 4 simulation step (dt = 1/400), we call one MPC and wait for its solution. Therefore, the slowdown effect in rviz means that the MPC loop is slow.

Now, what to do? Activate either displayShortSummary or displayInfo. If you are interested in a specific gait, switch to that gait immediately after the robot appears in rviz. Then run dummy for a while. Stop the program with ctrl-c. At the termination, the solver prints out statistics of the computation time of the different parts of optimization. Based on this, you can catch the most expensive part of your MPC.

If the Backward pass is the most costly part, it can be partially because of the dimension of your problem or the stiffness of Riccati equations; you can affect the stiffness of the problem by playing with your cost functions gains (making the problem softer).

If the LQ approximation is the most expensive part, try to figure out which of your cost terms, constraints terms, or dynamics are the slowest and improve them. You can benchmark each of these components separately with any program you prefer. We have some implementation based on std::chrono library (ocs2_core/include/ocs2_core/misc/Benchmark.h).

Answer to Q3) Use gdb to see what goes wrong. You should adapt your target trajectory node since the dimension of the system is changed.

Best, Farbod

[IoannisDadiotis]

Hi Farbod,

Thank you very much for the detailed response!

There has been some progress since your response. For future users I just report that wrt to Q1) in my previous comment the Equality constraints SSE as well as rollout merit and rollout cost were exploding when trying to plan on a low mpc frequency static_gait. I tried to deal with this by playing with my input cost. This also helped in reducing the time needed for the backward pass which is the most expensive part.

Answer to Q2) When testing in the dummy simulator, we artificially slow down the simulator to ensure that the ratio of mrtDesiredFrequency/mpcDesiredFrequency remains constant. For example for mpcDesiredFrequency=50 [Hz] and mrtDesiredFrequency=400[Hz], for every 4 simulation step (dt = 1/400), we call one MPC and wait for its solution. Therefore, the slowdown effect in rviz means that the MPC loop is slow.

I have few enquiries on this: 1) I didn't get how the mpc-mrt relation is derived in your answer. Since mrtDesiredFrequency/mpcDesiredFrequency = 8, I would think that for every 8 simulation steps one MPC is called. Isn't it like this? What is the value of this ratio when I set mpcDesiredFrequency=-1 ? 2) Overall what is your advise regarding setting the value of the mrt (or equivalently the ratio mrt/mpc)? My guess is that a high mrt frequency can be important if a robot model different than the one in the mpc is simulated or feedback policy is activated. On the other hand when the same model as the mpc is used and there is no feedback policy I understand that the exact value of mrtDesiredFrequency does not really affect the generated motion (since the simulated state will coincide with the planned one), right? 3) How can I check the actual frequency that my mpc manages to run? From the terminal of the dummy node we can get a rough idea if the mpc is very slow (since current time is printed) but in many cases it is not clear if the mpc manages to run at the desired frequency or not. Another guess is to sum up the elements of the solver benchmarking information that is displayed when stopping the program with ctrl-c (if this includes all the main time consuming elements of the mpc).

I have tried to explore the addition of various constraints in my formulation and thus I have few enquiries.

4) Is the Augmented Lagrangian method for handling inequalities supported? I was trying to find such a constraint example but I noticed that the relaxed log barrier method is used in the examples. 5) This question regards caching/precomputation. Does the legged robot example support caching? In the documentation is shown that the legged robot example does not support caching. However in the LeggedRobotInterface.cpp the use of the LeggedRobotPreComputation class (e.g. in the following lines of code) make me think that caching is used.

  // Pre-computation
  problemPtr_->preComputationPtr.reset(new LeggedRobotPreComputation(*pinocchioInterfacePtr_, centroidalModelInfo_,
                                                                       *referenceManagerPtr_->getSwingTrajectoryPlanner(), modelSettings_));

If this is the case, a new question arises: How are CppAd and caching combined (since from the documentation, as well as the MobileManipulator example, it seems that the user has to select only one of them)?

6) I am trying to implement a soft constraint for an arm EE pose of centauro by having as template the mobile manipulator example. Based on this, in my LeggedRobotInterface::setupOptimalConrolProblem() I add the following code:

  const std::string arm_name = "arm1_8";
  // create a pinocchio mapping and EEKinematics object for the specific EE frame
  const CentroidalModelPinocchioMappingCppAd pinocchioMappingCppAd(centroidalModelInfo_.toCppAd());
  std::unique_ptr<EndEffectorKinematics<scalar_t>> armEeKinematicsPtr(new PinocchioEndEffectorKinematicsCppAd(
                                                                      *pinocchioInterfacePtr_, pinocchioMappingCppAd, {arm_name},
                                                                      centroidalModelInfo_.stateDim, centroidalModelInfo_.inputDim,
                                                                      "arm_end_effector_kinematics", modelSettings_.modelFolderCppAd,
                                                                      modelSettings_.recompileLibrariesCppAd, modelSettings_.verboseCppAd));

  //  pass the EEKinematics object to getEndEffectorConstraint2 
  problemPtr_->stateSoftConstraintPtr->add("armEndEffector", getEndEffectorConstraint2(*armEeKinematicsPtr, taskFile, "armEndEffector"));

with

std::unique_ptr<StateCost> LeggedRobotInterface::getEndEffectorConstraint2(const EndEffectorKinematics<scalar_t>& armEeKinematics,
                                                                          const std::string& taskFile, const std::string& prefix) {
  scalar_t muPosition = 0.001;
  scalar_t muOrientation = 0.0005;
  // get weights from taskfile
  boost::property_tree::ptree pt;
  boost::property_tree::read_info(taskFile, pt);
  std::cerr << "\n #### " << prefix << " Settings: ";
  std::cerr << "\n #### =============================================================================\n";
  loadData::loadPtreeValue(pt, muPosition, prefix + ".muPosition", true);
  loadData::loadPtreeValue(pt, muOrientation, prefix + ".muOrientation", true);
  std::cerr << " #### =============================================================================\n";

  if (referenceManagerPtr_ == nullptr) {
    throw std::runtime_error("[getEndEffectorConstraint] referenceManagerPtr_ should be set first!");
  }

  // create StateConstraint object
  std::unique_ptr<StateConstraint> constraint;
  constraint.reset(new EndEffectorConstraint(armEeKinematics, *referenceManagerPtr_));

  std::vector<std::unique_ptr<PenaltyBase>> penaltyArray(6);
  std::generate_n(penaltyArray.begin(), 3, [&] { return std::unique_ptr<PenaltyBase>(new QuadraticPenalty(muPosition)); });
  std::generate_n(penaltyArray.begin() + 3, 3, [&] { return std::unique_ptr<PenaltyBase>(new QuadraticPenalty(muOrientation)); });

  // create StateSoftConstraint based on the above constraint object
  return std::unique_ptr<StateCost>(new StateSoftConstraint(std::move(constraint), std::move(penaltyArray)));
}

The EndEffectorConstraint() class is similar to the one for the mobile manipulator (just using the corresponding classes for legged robot) It seems to me that the cost derivative is not finite. As soon as I start the mpc I am getting the following output:

/tmp/ocs2/arm_end_effector_kinematics_velocity/cppad_generated/arm_end_effector_kinematics_velocity_forward_zero.c: In function ‘arm_end_effector_kinematics_velocity_forward_zero’:
/tmp/ocs2/arm_end_effector_kinematics_velocity/cppad_generated/arm_end_effector_kinematics_velocity_forward_zero.c:98:12: error: ‘inf’ undeclared (first use in this function); did you mean ‘in’?
   98 |    v[43] = inf. * v[37] + -nan * v[38] + -nan * v[39] + -nan * v[40] + -nan * v[41] + -nan * v[42];
      |            ^~~
      |            in
/tmp/ocs2/arm_end_effector_kinematics_velocity/cppad_generated/arm_end_effector_kinematics_velocity_forward_zero.c:98:12: note: each undeclared identifier is reported only once for each function it appears in
/tmp/ocs2/arm_end_effector_kinematics_velocity/cppad_generated/arm_end_effector_kinematics_velocity_forward_zero.c:98:17: error: expected identifier before ‘*’ token
   98 |    v[43] = inf. * v[37] + -nan * v[38] + -nan * v[39] + -nan * v[40] + -nan * v[41] + -nan * v[42];
      |                 ^
/tmp/ocs2/arm_end_effector_kinematics_velocity/cppad_generated/arm_end_effector_kinematics_velocity_forward_zero.c:101:12: error: wrong type argument to unary minus
  101 |    v[46] = -nan * v[37] + inf. * v[38] + -nan * v[39] + -nan * v[40] + -nan * v[41] + -nan * v[42];
      |            ^
.... 
(manually removed lines to reduce output size in github comment)...                                                      
.....
/tmp/ocs2/arm_end_effector_kinematics_velocity/cppad_generated/arm_end_effector_kinematics_velocity_forward_zero.c:155:93: error: wrong type argument to unary minus
  155 |  -nan + v[41] * v[6] + v[28] * v[48] + -0.174 * (-nan + v[4] + x[99]) - -0.049 * (-nan + v[28] * v[40] + v[35] * v[39]);
      |                                                                                   ^

/tmp/ocs2/arm_end_effector_kinematics_velocity/cppad_generated/arm_end_effector_kinematics_velocity_forward_zero.c:156:12: error: wrong type argument to unary minus
  156 |    v[40] = -nan + v[31] * v[42] + v[5] * v[48];
      |            ^
/tmp/ocs2/arm_end_effector_kinematics_velocity/cppad_generated/arm_end_effector_kinematics_velocity_forward_zero.c:157:12: error: wrong type argument to unary minus
  157 |    v[48] = -nan + v[38] * v[42] + v[31] * v[48];
      |            ^
Failed to delete temporary file '/tmp/ocs2/arm_end_effector_kinematics_velocity/cppad_generated/cppadcg_tmp671220482/arm_end_effector_kinematics_velocity_forward_zero.c.o'
terminate called after throwing an instance of 'CppAD::cg::CGException'
  what():  Executable '/usr/bin/gcc' (pid 221393) exited with code 1

I can provide the complete code if needed.