Skip to content

Commit 8bf04a5

Browse files
edantecedantec
committed
New benchmark for go2 + update talos benchmark
1 parent 1a6b615 commit 8bf04a5

File tree

3 files changed

+162
-1
lines changed

3 files changed

+162
-1
lines changed

‎benchmark/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,3 +16,4 @@ function(create_bench exfile)
1616
endfunction()
1717

1818
create_bench("talos.cpp")
19+
create_bench("go2.cpp")

‎benchmark/go2.cpp‎

Lines changed: 154 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,154 @@
1+
#include "simple-mpc/fulldynamics.hpp"
2+
#include "simple-mpc/mpc.hpp"
3+
#include "simple-mpc/ocp-handler.hpp"
4+
#include "simple-mpc/robot-handler.hpp"
5+
#include <pinocchio/algorithm/model.hpp>
6+
#include <pinocchio/fwd.hpp>
7+
#include <pinocchio/parsers/srdf.hpp>
8+
#include <pinocchio/parsers/urdf.hpp>
9+
10+
using simple_mpc::ContactMap;
11+
using simple_mpc::RobotDataHandler;
12+
using simple_mpc::RobotModelHandler;
13+
using PoseVec = aligator::StdVectorEigenAligned<Eigen::Vector3d>;
14+
using simple_mpc::FullDynamicsOCP;
15+
using simple_mpc::FullDynamicsSettings;
16+
using simple_mpc::MPC;
17+
using simple_mpc::MPCSettings;
18+
using simple_mpc::OCPHandler;
19+
20+
int main()
21+
{
22+
// Load pinocchio model from example robot data
23+
pinocchio::Model model;
24+
std::string urdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/go2_description/urdf/go2.urdf";
25+
std::string srdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/go2_description/srdf/go2.srdf";
26+
std::string base_joint_name = "root_joint";
27+
28+
pinocchio::urdf::buildModel(urdf_path, pinocchio::JointModelFreeFlyer(), model);
29+
pinocchio::srdf::loadReferenceConfigurations(model, srdf_path, false);
30+
// pinocchio::srdf::loadRotorParameters(model, srdf_path, false);
31+
32+
simple_mpc::RobotModelHandler model_handler = simple_mpc::RobotModelHandler(model, "standing", base_joint_name);
33+
34+
/// Add reference foot for walking
35+
pinocchio::SE3 ref_FL_foot = pinocchio::SE3::Identity();
36+
pinocchio::SE3 ref_FR_foot = pinocchio::SE3::Identity();
37+
pinocchio::SE3 ref_RL_foot = pinocchio::SE3::Identity();
38+
pinocchio::SE3 ref_RR_foot = pinocchio::SE3::Identity();
39+
ref_FL_foot.translation() = Eigen::Vector3d(0.17, 0.15, 0.0);
40+
ref_FR_foot.translation() = Eigen::Vector3d(0.17, -0.15, 0.0);
41+
ref_RL_foot.translation() = Eigen::Vector3d(-0.24, 0.15, 0.0);
42+
ref_RR_foot.translation() = Eigen::Vector3d(-0.24, -0.15, 0.0);
43+
model_handler.addFoot("FL_foot", base_joint_name, ref_FL_foot);
44+
model_handler.addFoot("FR_foot", base_joint_name, ref_FR_foot);
45+
model_handler.addFoot("RL_foot", base_joint_name, ref_RL_foot);
46+
model_handler.addFoot("RR_foot", base_joint_name, ref_RR_foot);
47+
48+
RobotDataHandler data_handler(model_handler);
49+
50+
int nu = model_handler.getModel().nv - 6;
51+
int nv = model_handler.getModel().nv;
52+
int ndx = nv * 2;
53+
Eigen::Vector3d gravity;
54+
gravity << 0., 0., -9.81;
55+
56+
// Define settings for OCP
57+
simple_mpc::FullDynamicsSettings problem_settings;
58+
59+
Eigen::VectorXd w_x_vec(ndx);
60+
w_x_vec << 0, 0, 0, 0., 0., 0, // Base pos/ori
61+
10., 10., 10., 10., 10., 10., // FL FR leg
62+
10., 10., 10., 10., 10., 10., // RL RR leg
63+
10., 10., 10., 10., 10., 10., // Base vel
64+
.1, .1, .1, .1, .1, .1, // FL FR vel
65+
.1, .1, .1, .1, .1, .1; // RL RR vel
66+
Eigen::VectorXd w_cent(6);
67+
w_cent << 0, 0, 0, 0, 0, 0;
68+
Eigen::VectorXd w_forces(3);
69+
w_forces << 0.0001, 0.0001, 0.0001;
70+
71+
Eigen::VectorXd u0 = Eigen::VectorXd::Zero(nu);
72+
73+
problem_settings.timestep = 0.01;
74+
problem_settings.w_x = Eigen::MatrixXd::Zero(ndx, ndx);
75+
problem_settings.w_x.diagonal() = w_x_vec;
76+
problem_settings.w_u = Eigen::MatrixXd::Identity(nu, nu) * 1e-4;
77+
problem_settings.w_cent = Eigen::MatrixXd::Zero(6, 6);
78+
problem_settings.w_cent.diagonal() = w_cent;
79+
problem_settings.gravity = gravity;
80+
problem_settings.force_size = 3;
81+
problem_settings.Kp_correction = Eigen::VectorXd::Zero(3);
82+
problem_settings.Kd_correction = Eigen::VectorXd::Zero(3);
83+
problem_settings.w_forces = Eigen::MatrixXd::Zero(3, 3);
84+
problem_settings.w_forces.diagonal() = w_forces;
85+
problem_settings.w_frame = Eigen::MatrixXd::Identity(3, 3) * 1000;
86+
problem_settings.umin = -model_handler.getModel().effortLimit.tail(nu);
87+
problem_settings.umax = model_handler.getModel().effortLimit.tail(nu);
88+
problem_settings.qmin = model_handler.getModel().lowerPositionLimit.tail(nu);
89+
problem_settings.qmax = model_handler.getModel().upperPositionLimit.tail(nu);
90+
problem_settings.mu = 0.8;
91+
problem_settings.Lfoot = 0.01;
92+
problem_settings.Wfoot = 0.01;
93+
problem_settings.torque_limits = false;
94+
problem_settings.kinematics_limits = false;
95+
problem_settings.force_cone = false;
96+
97+
std::shared_ptr<simple_mpc::OCPHandler> ocpPtr =
98+
std::make_shared<simple_mpc::FullDynamicsOCP>(problem_settings, model_handler);
99+
100+
// Create an OCP problem with horizon size T
101+
size_t T = 50;
102+
ocpPtr->createProblem(model_handler.getReferenceState(), T, 3, gravity[2], false);
103+
104+
// Define settings for MPC
105+
int T_fly = 30;
106+
int T_contact = 5;
107+
simple_mpc::MPCSettings mpc_settings;
108+
mpc_settings.ddpIteration = 1;
109+
mpc_settings.support_force = -gravity[2] * model_handler.getMass();
110+
mpc_settings.TOL = 1e-4;
111+
mpc_settings.mu_init = 1e-8;
112+
mpc_settings.max_iters = 1;
113+
mpc_settings.num_threads = 1;
114+
mpc_settings.swing_apex = 0.2;
115+
mpc_settings.T_fly = T_fly;
116+
mpc_settings.T_contact = T_contact;
117+
mpc_settings.timestep = 0.01;
118+
119+
std::shared_ptr<simple_mpc::MPC> mpc = std::make_shared<simple_mpc::MPC>(mpc_settings, ocpPtr);
120+
121+
// Define the walking gait
122+
std::vector<std::map<std::string, bool>> contact_states;
123+
std::map<std::string, bool> contact_state_quadru;
124+
std::map<std::string, bool> contact_phase_lift_FL_RR;
125+
std::map<std::string, bool> contact_phase_lift_RL_FR;
126+
127+
contact_state_quadru.insert({"FL_foot", true});
128+
contact_state_quadru.insert({"FR_foot", true});
129+
contact_state_quadru.insert({"RL_foot", true});
130+
contact_state_quadru.insert({"RR_foot", true});
131+
132+
contact_phase_lift_FL_RR.insert({"FL_foot", false});
133+
contact_phase_lift_FL_RR.insert({"FR_foot", true});
134+
contact_phase_lift_FL_RR.insert({"RL_foot", true});
135+
contact_phase_lift_FL_RR.insert({"RR_foot", false});
136+
137+
contact_phase_lift_RL_FR.insert({"FL_foot", true});
138+
contact_phase_lift_RL_FR.insert({"FR_foot", false});
139+
contact_phase_lift_RL_FR.insert({"RL_foot", false});
140+
contact_phase_lift_RL_FR.insert({"RR_foot", true});
141+
142+
for (int i = 0; i < T_contact; i++)
143+
contact_states.push_back(contact_state_quadru);
144+
for (int i = 0; i < T_fly; i++)
145+
contact_states.push_back(contact_phase_lift_FL_RR);
146+
for (int i = 0; i < T_contact; i++)
147+
contact_states.push_back(contact_state_quadru);
148+
for (int i = 0; i < T_fly; i++)
149+
contact_states.push_back(contact_phase_lift_RL_FR);
150+
151+
mpc->generateCycleHorizon(contact_states);
152+
153+
return 0;
154+
}

‎benchmark/talos.cpp‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -79,6 +79,7 @@ int main()
7979

8080
RobotDataHandler data_handler(model_handler);
8181

82+
// Define settings for OCP
8283
size_t T = 100;
8384

8485
FullDynamicsSettings problem_settings;
@@ -125,10 +126,14 @@ int main()
125126
problem_settings.mu = 0.8;
126127
problem_settings.Lfoot = 0.1;
127128
problem_settings.Wfoot = 0.075;
129+
problem_settings.torque_limits = false;
130+
problem_settings.kinematics_limits = false;
131+
problem_settings.force_cone = false;
128132

129-
std::shared_ptr<OCPHandler> ocpPtr = std::make_shared<FullDynamicsOCP>(problem_settings, model_handler, data_handler);
133+
std::shared_ptr<OCPHandler> ocpPtr = std::make_shared<FullDynamicsOCP>(problem_settings, model_handler);
130134
ocpPtr->createProblem(model_handler.getReferenceState(), T, 6, problem_settings.gravity[2], true);
131135

136+
// Define settings for MPC
132137
MPCSettings mpc_settings;
133138
mpc_settings.support_force = -problem_settings.gravity[2] * model_handler.getMass();
134139
mpc_settings.TOL = 1e-4;
@@ -138,6 +143,7 @@ int main()
138143

139144
MPC mpc{mpc_settings, ocpPtr};
140145

146+
// Define the walking gait
141147
std::vector<std::map<std::string, bool>> contact_states;
142148
// std::vector<std::vector<bool>> contact_states;
143149
for (std::size_t i = 0; i < 10; i++)

0 commit comments

Comments
 (0)