Switching time optimization (STO) example of robotoc::OCPSolver in C++

This page explains the example code in examples/anymal/jump.cpp.

Required header files are as follows.

#include <string>
#include <memory>
 
#include "Eigen/Core"
 
#include "robotoc/solver/ocp_solver.hpp"
#include "robotoc/ocp/ocp.hpp"
#include "robotoc/robot/robot.hpp"
#include "robotoc/planner/contact_sequence.hpp"
#include "robotoc/cost/cost_function.hpp"
#include "robotoc/cost/configuration_space_cost.hpp"
#include "robotoc/cost/task_space_3d_cost.hpp"
#include "robotoc/cost/com_cost.hpp"
#include "robotoc/cost/periodic_swing_foot_ref.hpp"
#include "robotoc/cost/periodic_com_ref.hpp"
#include "robotoc/constraints/constraints.hpp"
#include "robotoc/constraints/joint_position_lower_limit.hpp"
#include "robotoc/constraints/joint_position_upper_limit.hpp"
#include "robotoc/constraints/joint_velocity_lower_limit.hpp"
#include "robotoc/constraints/joint_velocity_upper_limit.hpp"
#include "robotoc/constraints/joint_torques_lower_limit.hpp"
#include "robotoc/constraints/joint_torques_upper_limit.hpp"
#include "robotoc/constraints/friction_cone.hpp"
#include "robotoc/sto/sto_cost_function.hpp"
#include "robotoc/sto/sto_constraints.hpp"
#include "robotoc/solver/solver_options.hpp"
 
int main(int argc, char *argv[]) {

First, we define the robot model. We speficy the URDF path, base joint type, and contact frames (in this case, the contact frames are the frames of all feet).

robotoc::RobotModelInfo model_info;
model_info.urdf_path = "../anymal_b_simple_description/urdf/anymal.urdf";
model_info.base_joint_type = robotoc::BaseJointType::FloatingBase;
const double baumgarte_time_step = 0.05;
model_info.point_contacts = {robotoc::ContactModelInfo("LF_FOOT", baumgarte_time_step),
                             robotoc::ContactModelInfo("LH_FOOT", baumgarte_time_step),
                             robotoc::ContactModelInfo("RF_FOOT", baumgarte_time_step),
                             robotoc::ContactModelInfo("RH_FOOT", baumgarte_time_step)};
robotoc::Robot robot(model_info);

Note

baumgarte_time_step is the stabilization parameter for acceleration-level rigid contact constraints. The best choice of baumgarte_time_step may be the time step of the optimal control problem. However, it is often too small to make the optimization problem high nonlinear. A moderate value such as several times of the time step of optimal control problem may be sufficient

Then set the parameters for the optimal control problem of the jump motion such as the jump length

const double dt = 0.02;
const Eigen::Vector3d jump_length = {0.8, 0, 0};
const double flying_up_time = 0.15;
const double flying_down_time = flying_up_time;
const double flying_time = flying_up_time + flying_down_time;
const double ground_time = 0.70;
const double t0 = 0;

Next, we construct the cost function. The below is the simple quadratic cost for the optimization variables.

auto cost = std::make_shared<robotoc::CostFunction>();
Eigen::VectorXd q_standing(Eigen::VectorXd::Zero(robot.dimq()));
q_standing << 0, 0, 0.4792, 0, 0, 0, 1, 
              -0.1,  0.7, -1.0, 
              -0.1, -0.7,  1.0, 
               0.1,  0.7, -1.0, 
               0.1, -0.7,  1.0;
Eigen::VectorXd q_ref = q_standing;
q_ref.coeffRef(0) += jump_length;
Eigen::VectorXd q_weight(Eigen::VectorXd::Zero(robot.dimv()));
q_weight << 1.0, 0, 0, 1.0, 1.0, 1.0, 
            0.001, 0.001, 0.001, 
            0.001, 0.001, 0.001,
            0.001, 0.001, 0.001,
            0.001, 0.001, 0.001;
Eigen::VectorXd v_weight = Eigen::VectorXd::Constant(robot.dimv(), 1.0);
Eigen::VectorXd a_weight = Eigen::VectorXd::Constant(robot.dimv(), 1.0e-06);
Eigen::VectorXd q_weight_impact(Eigen::VectorXd::Zero(robot.dimv()));
q_weight_impact << 0, 0, 0, 100.0, 100.0, 100.0,  
             0.1, 0.1, 0.1, 
             0.1, 0.1, 0.1,
             0.1, 0.1, 0.1,
             0.1, 0.1, 0.1;
Eigen::VectorXd v_weight_impact = Eigen::VectorXd::Constant(robot.dimv(), 1.0);
Eigen::VectorXd dv_weight_impact = Eigen::VectorXd::Constant(robot.dimv(), 1.0e-06);
auto config_cost = std::make_shared<robotoc::ConfigurationSpaceCost>(robot);
config_cost->set_q_ref(q_ref);
config_cost->set_q_weight(q_weight);
config_cost->set_q_weight_terminal(q_weight);
config_cost->set_q_weight_impact(q_weight_impact);
config_cost->set_v_weight(v_weight);
config_cost->set_v_weight_terminal(v_weight);
config_cost->set_v_weight_impact(v_weight_impact);
config_cost->set_dv_weight_impact(dv_weight_impact);
config_cost->set_a_weight(a_weight);
cost->add("config_cost", config_cost);

Next, we construct the constraints.

const double barrier_param = 1.0e-03;
const double fraction_to_boundary_rule = 0.995;
auto constraints          = std::make_shared<robotoc::Constraints>(barrier_param, fraction_to_boundary_rule);
auto joint_position_lower  = std::make_shared<robotoc::JointPositionLowerLimit>(robot);
auto joint_position_upper  = std::make_shared<robotoc::JointPositionUpperLimit>(robot);
auto joint_velocity_lower  = std::make_shared<robotoc::JointVelocityLowerLimit>(robot);
auto joint_velocity_upper  = std::make_shared<robotoc::JointVelocityUpperLimit>(robot);
auto joint_torques_lower   = std::make_shared<robotoc::JointTorquesLowerLimit>(robot);
auto joint_torques_upper   = std::make_shared<robotoc::JointTorquesUpperLimit>(robot);
auto friction_cone         = std::make_shared<robotoc::FrictionCone>(robot);
constraints->add("joint_position_lower", joint_position_lower);
constraints->add("joint_position_upper", joint_position_upper);
constraints->add("joint_velocity_lower", joint_velocity_lower);
constraints->add("joint_velocity_upper", joint_velocity_upper);
constraints->add("joint_torques_lower", joint_torques_lower);
constraints->add("joint_torques_upper", joint_torques_upper);
constraints->add("friction_cone", friction_cone);

Next, we construct the contact sequence robotoc::ContactSequence as

auto contact_sequence = std::make_shared<robotoc::ContactSequence>(robot);

Then we can set an impact event and a lift event to the contact sequence.

We set the contact positions and friction coefficients through the contact sequence. We then define the friction coefficients.

const double mu = 0.7;
const std::unordered_map<std::string, double> friction_coefficients = {{"LF_FOOT", mu}, 
                                                                       {"LH_FOOT", mu}, 
                                                                       {"RF_FOOT", mu}, 
                                                                       {"RH_FOOT", mu}};

We set the initial contact status of the robot. In the beginning, the robot is standing, so all the contacts are active.

robot.updateFrameKinematics(q_standing);
const Eigen::Vector3d x3d0_LF = robot.framePosition("LF_FOOT");
const Eigen::Vector3d x3d0_LH = robot.framePosition("LH_FOOT");
const Eigen::Vector3d x3d0_RF = robot.framePosition("RF_FOOT");
const Eigen::Vector3d x3d0_RH = robot.framePosition("RH_FOOT");
 
std::unordered_map<std::string, Eigen::Vector3d> contact_positions = {{"LF_FOOT", x3d0_LF}, 
                                                                      {"LH_FOOT", x3d0_LH}, 
                                                                      {"RF_FOOT", x3d0_RF}, 
                                                                      {"RH_FOOT", x3d0_RH}};
auto contact_status_standing = robot.createContactStatus();
contact_status_standing.activateContacts(std::vector<std::string>({"LF_FOOT", "LH_FOOT", "RF_FOOT", "RH_FOOT"}));
contact_status_standing.setContactPlacements(contact_positions);
contact_status_standing.setFrictionCoefficients(friction_coefficients);
contact_sequence->init(contact_status_standing);

Next, we set the contact status when the robot is flying. Then the all the contacts are inactive.

auto contact_status_flying = robot.createContactStatus();
contact_sequence->push_back(contact_status_flying, t0+ground_time-0.3, true);

Then a lift event is automatically appended into the contact sequence. By setting sto as true, the switching time of this event is regarded as an optimization variable. Finally, we set the contact status after touch-down as

contact_positions["LF_FOOT"].noalias() += jump_length;
contact_positions["LH_FOOT"].noalias() += jump_length;
contact_positions["RF_FOOT"].noalias() += jump_length;
contact_positions["RH_FOOT"].noalias() += jump_length;
contact_status_standing.setContactPlacements(contact_positions);
contact_sequence->push_back(contact_status_standing, 
                            t0+ground_time+flying_time-0.1, true);

Then an impact event is automatically appended into the contact sequence.

Note

We can check the contact sequence via

std::cout << contact_sequence << std::endl;

We further construct cost function for the switching time optimization (STO) problem

auto sto_cost = std::make_shared<robotoc::STOCostFunction>();

and the minimum dwell-time constraints for the STO problem

const std::vector<double> minimum_dwell_times = {0.15, 0.15, 0.65};
auto sto_constraints = std::make_shared<robotoc::STOConstraints>(minimum_dwell_times,
                                                                 barrier_param, 
                                                                 fraction_to_boundary_rule);

Finally, we can construct the optimal control solver!

const double T = t0 + flying_time + 2 * ground_time; 
const int N = std::floor(T / dt);
robotoc::OCP ocp(robot, cost, constraints, sto_cost, sto_constraints, 
                 contact_sequence, T, N);
auto solver_options = robotoc::SolverOptions();
solver_options.max_dt_mesh = T/N;
solver_options.kkt_tol_mesh = 0.1;
solver_options.max_iter = 200;
solver_options.nthreads = 4;
robotoc::OCPSolver ocp_solver(ocp, solver_options);

Note

Without robotoc::STOCostFunction and robotoc::STOConstraints, the solver does not optimize the switching times. Therefore, even if there are empty (e.g., the STO cost of this example is also empty), please pass them to the constructor of OCP.

In this example, we want to optimize the switching times as well as the whole-body trajectory of the robot. Then we need to carry on mesh refinement because the switching times change over the iterations. We set SolverOptions::max_dt_mesh that the maximum value of the time-steps. We also set SolverOptions::kkt_tol_mesh and we apply the mesh refinement when the KKT error is less than SolverOptions::kkt_tol_mesh and the maximum time step is larger than SolverOptions::max_dt_mesh.

Let's run the solver!

  const double t = 0;
  const Eigen::VectorXd q(q_standing); // initial state.
  const Eigen::VectorXd v(Eigen::VectorXd::Zero(robot.dimv())); // initial state.
 
  ocp_solver.discretize(t); // discretizes the optimal control problem.
  ocp_solver.setSolution("q", q); // set the initial guess of the solution.
  ocp_solver.setSolution("v", v); // set the initial guess of the solution.
  Eigen::Vector3d f_init;
  f_init << 0, 0, 0.25*robot.totalWeight();
  ocp_solver.setSolution("f", f_init); // set the initial guess of the solution.
 
  ocp_solver.initConstraints(); // initialize the slack and dual variables of the primal-dual interior point method.
  std::cout << "Initial KKT error: " << ocp_solver.KKTError(t, q, v) << std::endl;
  ocp_solver.solve(t, q, v);
  std::cout << "KKT error after convergence: " << ocp_solver.KKTError(t, q, v) << std::endl;
  std::cout << ocp_solver.getSolverStatistics() << std::endl; // print solver statistics
 
  return 0;
}

Note

We can check the discretization of the optimal control problem sololy via

robotoc::TimeDiscretization time_discretization(T, N);
time_discretization.discretize(contact_sequence, t);
std::cout << time_discretization << std::endl;