diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt index 243fe085..f45e9342 100644 --- a/benchmark/CMakeLists.txt +++ b/benchmark/CMakeLists.txt @@ -16,3 +16,4 @@ function(create_bench exfile) endfunction() create_bench("talos.cpp") +create_bench("go2.cpp") diff --git a/benchmark/go2.cpp b/benchmark/go2.cpp new file mode 100644 index 00000000..f4db7269 --- /dev/null +++ b/benchmark/go2.cpp @@ -0,0 +1,154 @@ +#include "simple-mpc/fulldynamics.hpp" +#include "simple-mpc/mpc.hpp" +#include "simple-mpc/ocp-handler.hpp" +#include "simple-mpc/robot-handler.hpp" +#include +#include +#include +#include + +using simple_mpc::ContactMap; +using simple_mpc::RobotDataHandler; +using simple_mpc::RobotModelHandler; +using PoseVec = aligator::StdVectorEigenAligned; +using simple_mpc::FullDynamicsOCP; +using simple_mpc::FullDynamicsSettings; +using simple_mpc::MPC; +using simple_mpc::MPCSettings; +using simple_mpc::OCPHandler; + +int main() +{ + // Load pinocchio model from example robot data + pinocchio::Model model; + std::string urdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/go2_description/urdf/go2.urdf"; + std::string srdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/go2_description/srdf/go2.srdf"; + std::string base_joint_name = "root_joint"; + + pinocchio::urdf::buildModel(urdf_path, pinocchio::JointModelFreeFlyer(), model); + pinocchio::srdf::loadReferenceConfigurations(model, srdf_path, false); + // pinocchio::srdf::loadRotorParameters(model, srdf_path, false); + + simple_mpc::RobotModelHandler model_handler = simple_mpc::RobotModelHandler(model, "standing", base_joint_name); + + /// Add reference foot for walking + pinocchio::SE3 ref_FL_foot = pinocchio::SE3::Identity(); + pinocchio::SE3 ref_FR_foot = pinocchio::SE3::Identity(); + pinocchio::SE3 ref_RL_foot = pinocchio::SE3::Identity(); + pinocchio::SE3 ref_RR_foot = pinocchio::SE3::Identity(); + ref_FL_foot.translation() = Eigen::Vector3d(0.17, 0.15, 0.0); + ref_FR_foot.translation() = Eigen::Vector3d(0.17, -0.15, 0.0); + ref_RL_foot.translation() = Eigen::Vector3d(-0.24, 0.15, 0.0); + ref_RR_foot.translation() = Eigen::Vector3d(-0.24, -0.15, 0.0); + model_handler.addFoot("FL_foot", base_joint_name, ref_FL_foot); + model_handler.addFoot("FR_foot", base_joint_name, ref_FR_foot); + model_handler.addFoot("RL_foot", base_joint_name, ref_RL_foot); + model_handler.addFoot("RR_foot", base_joint_name, ref_RR_foot); + + RobotDataHandler data_handler(model_handler); + + int nu = model_handler.getModel().nv - 6; + int nv = model_handler.getModel().nv; + int ndx = nv * 2; + Eigen::Vector3d gravity; + gravity << 0., 0., -9.81; + + // Define settings for OCP + simple_mpc::FullDynamicsSettings problem_settings; + + Eigen::VectorXd w_x_vec(ndx); + w_x_vec << 0, 0, 0, 0., 0., 0, // Base pos/ori + 10., 10., 10., 10., 10., 10., // FL FR leg + 10., 10., 10., 10., 10., 10., // RL RR leg + 10., 10., 10., 10., 10., 10., // Base vel + .1, .1, .1, .1, .1, .1, // FL FR vel + .1, .1, .1, .1, .1, .1; // RL RR vel + Eigen::VectorXd w_cent(6); + w_cent << 0, 0, 0, 0, 0, 0; + Eigen::VectorXd w_forces(3); + w_forces << 0.0001, 0.0001, 0.0001; + + Eigen::VectorXd u0 = Eigen::VectorXd::Zero(nu); + + problem_settings.timestep = 0.01; + problem_settings.w_x = Eigen::MatrixXd::Zero(ndx, ndx); + problem_settings.w_x.diagonal() = w_x_vec; + problem_settings.w_u = Eigen::MatrixXd::Identity(nu, nu) * 1e-4; + problem_settings.w_cent = Eigen::MatrixXd::Zero(6, 6); + problem_settings.w_cent.diagonal() = w_cent; + problem_settings.gravity = gravity; + problem_settings.force_size = 3; + problem_settings.Kp_correction = Eigen::VectorXd::Zero(3); + problem_settings.Kd_correction = Eigen::VectorXd::Zero(3); + problem_settings.w_forces = Eigen::MatrixXd::Zero(3, 3); + problem_settings.w_forces.diagonal() = w_forces; + problem_settings.w_frame = Eigen::MatrixXd::Identity(3, 3) * 1000; + problem_settings.umin = -model_handler.getModel().effortLimit.tail(nu); + problem_settings.umax = model_handler.getModel().effortLimit.tail(nu); + problem_settings.qmin = model_handler.getModel().lowerPositionLimit.tail(nu); + problem_settings.qmax = model_handler.getModel().upperPositionLimit.tail(nu); + problem_settings.mu = 0.8; + problem_settings.Lfoot = 0.01; + problem_settings.Wfoot = 0.01; + problem_settings.torque_limits = false; + problem_settings.kinematics_limits = false; + problem_settings.force_cone = false; + + std::shared_ptr ocpPtr = + std::make_shared(problem_settings, model_handler); + + // Create an OCP problem with horizon size T + size_t T = 50; + ocpPtr->createProblem(model_handler.getReferenceState(), T, 3, gravity[2], false); + + // Define settings for MPC + int T_fly = 30; + int T_contact = 5; + simple_mpc::MPCSettings mpc_settings; + mpc_settings.ddpIteration = 1; + mpc_settings.support_force = -gravity[2] * model_handler.getMass(); + mpc_settings.TOL = 1e-4; + mpc_settings.mu_init = 1e-8; + mpc_settings.max_iters = 1; + mpc_settings.num_threads = 1; + mpc_settings.swing_apex = 0.2; + mpc_settings.T_fly = T_fly; + mpc_settings.T_contact = T_contact; + mpc_settings.timestep = 0.01; + + std::shared_ptr mpc = std::make_shared(mpc_settings, ocpPtr); + + // Define the walking gait + std::vector> contact_states; + std::map contact_state_quadru; + std::map contact_phase_lift_FL_RR; + std::map contact_phase_lift_RL_FR; + + contact_state_quadru.insert({"FL_foot", true}); + contact_state_quadru.insert({"FR_foot", true}); + contact_state_quadru.insert({"RL_foot", true}); + contact_state_quadru.insert({"RR_foot", true}); + + contact_phase_lift_FL_RR.insert({"FL_foot", false}); + contact_phase_lift_FL_RR.insert({"FR_foot", true}); + contact_phase_lift_FL_RR.insert({"RL_foot", true}); + contact_phase_lift_FL_RR.insert({"RR_foot", false}); + + contact_phase_lift_RL_FR.insert({"FL_foot", true}); + contact_phase_lift_RL_FR.insert({"FR_foot", false}); + contact_phase_lift_RL_FR.insert({"RL_foot", false}); + contact_phase_lift_RL_FR.insert({"RR_foot", true}); + + for (int i = 0; i < T_contact; i++) + contact_states.push_back(contact_state_quadru); + for (int i = 0; i < T_fly; i++) + contact_states.push_back(contact_phase_lift_FL_RR); + for (int i = 0; i < T_contact; i++) + contact_states.push_back(contact_state_quadru); + for (int i = 0; i < T_fly; i++) + contact_states.push_back(contact_phase_lift_RL_FR); + + mpc->generateCycleHorizon(contact_states); + + return 0; +} diff --git a/benchmark/talos.cpp b/benchmark/talos.cpp index bbaa6ed2..fcb79b49 100644 --- a/benchmark/talos.cpp +++ b/benchmark/talos.cpp @@ -79,6 +79,7 @@ int main() RobotDataHandler data_handler(model_handler); + // Define settings for OCP size_t T = 100; FullDynamicsSettings problem_settings; @@ -125,10 +126,14 @@ int main() problem_settings.mu = 0.8; problem_settings.Lfoot = 0.1; problem_settings.Wfoot = 0.075; + problem_settings.torque_limits = false; + problem_settings.kinematics_limits = false; + problem_settings.force_cone = false; - std::shared_ptr ocpPtr = std::make_shared(problem_settings, model_handler, data_handler); + std::shared_ptr ocpPtr = std::make_shared(problem_settings, model_handler); ocpPtr->createProblem(model_handler.getReferenceState(), T, 6, problem_settings.gravity[2], true); + // Define settings for MPC MPCSettings mpc_settings; mpc_settings.support_force = -problem_settings.gravity[2] * model_handler.getMass(); mpc_settings.TOL = 1e-4; @@ -138,6 +143,7 @@ int main() MPC mpc{mpc_settings, ocpPtr}; + // Define the walking gait std::vector> contact_states; // std::vector> contact_states; for (std::size_t i = 0; i < 10; i++)