clean *-benchmark.cpp

src/pinocchio-benchmark.cpp

@@ -11,28 +11,51 @@
 static void BM_Pinocchio_RNEA(benchmark::State& state)
 {
   se3::Model model;
-
-  // Load an urdf file provided by the user
   se3::urdf::buildModel("models/simple_humanoid.urdf", se3::JointModelFreeFlyer(), model);
-  std::cout << "Pinocchio Benchmark" << std::endl;
-  std::cout << "  model: " << "models/simple_humanoid.urdf" << std::endl;
-  std::cout << "  nq: " << model.nq << std::endl;
-  std::cout << "  nv: " << model.nv << std::endl;
+  se3::Data data(model);
+
+  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
+  Eigen::VectorXd qdot = Eigen::VectorXd::Zero(model.nv);
+  Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv);
+  Eigen::VectorXd qddot = Eigen::VectorXd::Zero(model.nv);
+
+  for (auto _: state)
+  {
+    state.PauseTiming();
+    q = Eigen::VectorXd::Random(model.nq);
+    qdot = Eigen::VectorXd::Random(model.nv);
+    qddot = Eigen::VectorXd::Random(model.nv);
+    state.ResumeTiming();
+
+    se3::rnea(model, data, q, qdot, tau);
+  }
+}
 
+
+static void BM_Pinocchio_ABA(benchmark::State& state)
+{
+  se3::Model model;
+  se3::urdf::buildModel("models/simple_humanoid.urdf", se3::JointModelFreeFlyer(), model);
   se3::Data data(model);
 
+  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
+  Eigen::VectorXd qdot = Eigen::VectorXd::Zero(model.nv);
+  Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv);
+  Eigen::VectorXd qddot = Eigen::VectorXd::Zero(model.nv);
+
   for (auto _: state)
   {
-    Eigen::VectorXd q = Eigen::VectorXd::Random(model.nq);
-    Eigen::VectorXd qdot = Eigen::VectorXd::Random(model.nv);
-    Eigen::VectorXd tau = Eigen::VectorXd::Random(model.nv);
-    Eigen::VectorXd qddot = Eigen::VectorXd::Random(model.nv);
+    state.PauseTiming();
+    q = Eigen::VectorXd::Random(model.nq);
+    qdot = Eigen::VectorXd::Random(model.nv);
+    tau = Eigen::VectorXd::Random(model.nv);
+    state.ResumeTiming();
 
     se3::aba(model, data, q, qdot, tau);
-    //std::cout << "qddot after ABA: " << data.ddq.transpose() << std::endl;
   }
 }
 
 BENCHMARK(BM_Pinocchio_RNEA);
+BENCHMARK(BM_Pinocchio_ABA);
 
 BENCHMARK_MAIN();

src/rbdl-benchmark.cpp

@@ -9,35 +9,57 @@
 static void BM_RBDL_RNEA(benchmark::State& state)
 {
   RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
+  RigidBodyDynamics::Addons::URDFReadFromFile("models/simple_humanoid.urdf", model, true);
+
+  RigidBodyDynamics::Math::VectorNd q =
+    RigidBodyDynamics::Math::VectorNd::Zero(model->q_size);
+  RigidBodyDynamics::Math::VectorNd qdot =
+    RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
+  RigidBodyDynamics::Math::VectorNd tau =
+    RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
+  RigidBodyDynamics::Math::VectorNd qddot =
+    RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
 
-  // Load an urdf file provided by the user
+  for (auto _: state)
+  {
+    state.PauseTiming();
+    q = RigidBodyDynamics::Math::VectorNd::Random(model->q_size);
+    qdot = RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
+    qddot = RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
+    state.ResumeTiming();
+
+    RigidBodyDynamics::InverseDynamics(*model, q, qdot, tau, qddot);
+  }
+}
+
+static void BM_RBDL_ABA(benchmark::State& state)
+{
+  RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
   RigidBodyDynamics::Addons::URDFReadFromFile("models/simple_humanoid.urdf", model, true);
-  std::cout << "RBDL Benchmark" << std::endl;
-  std::cout << "  model: " << "models/simple_humanoid.urdf" << std::endl;
-  std::cout << "  nq: " << model->q_size << std::endl;
-  std::cout << "  nv: " << model->qdot_size << std::endl;
 
   RigidBodyDynamics::Math::VectorNd q =
-    RigidBodyDynamics::Math::VectorNd::Random(model->q_size);
+    RigidBodyDynamics::Math::VectorNd::Zero(model->q_size);
   RigidBodyDynamics::Math::VectorNd qdot =
-    RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
+    RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
   RigidBodyDynamics::Math::VectorNd tau =
-    RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
+    RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
   RigidBodyDynamics::Math::VectorNd qddot =
-    RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
+    RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
 
   for (auto _: state)
   {
+    state.PauseTiming();
     q = RigidBodyDynamics::Math::VectorNd::Random(model->q_size);
     qdot = RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
     tau = RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
-    qddot = RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
+    state.ResumeTiming();
 
     RigidBodyDynamics::ForwardDynamics(*model, q, qdot, tau, qddot);
-    //std::cout << "qddot after ABA: " << qddot.transpose() << std::endl;
   }
 }
 
+
 BENCHMARK(BM_RBDL_RNEA);
+BENCHMARK(BM_RBDL_ABA);
 
 BENCHMARK_MAIN();