Skip to content
Snippets Groups Projects
Verified Commit 6bed7344 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

bench: measure timing in micro seconds + divide by number of tests

parent f8a970b2
No related branches found
No related tags found
No related merge requests found
......@@ -35,7 +35,7 @@ void benchmark_pinocchio_rnea(std::string model, std::string log_filename)
for (size_t i=0; i<NBT; i++)
{
qs[i] = Eigen::VectorXd::Random(robot.nq);
if (robot.nq > robot.nv) qs[i].segment(0, 4).normalize();
if (robot.nq > robot.nv) qs[i].segment<4>(3).normalize();
qdots[i] = Eigen::VectorXd::Random(robot.nv);
qddots[i] = Eigen::VectorXd::Random(robot.nv);
}
......@@ -49,7 +49,7 @@ void benchmark_pinocchio_rnea(std::string model, std::string log_filename)
}
end = std::chrono::high_resolution_clock::now();
std::chrono::nanoseconds time = end - start;
file << time.count() << std::endl;
file << 1e-3 * (time.count() / (double)NBT) << std::endl;
file.close();
}
......@@ -75,21 +75,21 @@ void benchmark_pinocchio_aba(std::string model, std::string log_filename)
for(size_t i=0; i<NBT; i++)
{
qs[i] = Eigen::VectorXd::Random(robot.nq);
if (robot.nq > robot.nv) qs[i].segment(0, 4).normalize();
if (robot.nq > robot.nv) qs[i].segment<4>(3).normalize();
qdots[i] = Eigen::VectorXd::Random(robot.nv);
taus[i] = Eigen::VectorXd::Random(robot.nv);
}
std::chrono::time_point<std::chrono::high_resolution_clock> start, end;
std::ofstream file(log_filename);
start = std::chrono::high_resolution_clock::now();
start = std::chrono::high_resolution_clock::now();
for(size_t i=0; i<NBT; i++)
{
pinocchio::aba(robot, data, qs[i], qdots[i], taus[i]);
}
end = std::chrono::high_resolution_clock::now();
std::chrono::nanoseconds time = end - start;
file << time.count() << std::endl;
end = std::chrono::high_resolution_clock::now();
std::chrono::nanoseconds time = end - start;
file << 1e-3 * (time.count() / (double)NBT) << std::endl;
file.close();
}
......@@ -113,7 +113,7 @@ void benchmark_pinocchio_crba(std::string model, std::string log_filename)
for(size_t i=0; i<NBT; i++)
{
qs[i] = Eigen::VectorXd::Random(robot.nq);
if (robot.nq > robot.nv) qs[i].segment(0, 4).normalize();
if (robot.nq > robot.nv) qs[i].segment<4>(3).normalize();
}
std::chrono::time_point<std::chrono::high_resolution_clock> start, end;
......@@ -124,8 +124,8 @@ void benchmark_pinocchio_crba(std::string model, std::string log_filename)
pinocchio::crba(robot, data, qs[i]);
}
end = std::chrono::high_resolution_clock::now();
std::chrono::nanoseconds time = end - start;
file << time.count() << std::endl;
std::chrono::duration<double,std::nano> time = end - start;
file << 1e-3 * (time.count() / (double)NBT) << std::endl;
file.close();
}
......@@ -136,9 +136,9 @@ int main()
benchmark_pinocchio_rnea(model, pinocchio_benchmarks::get_log_filename(
"Pinocchio", "ID", model));
benchmark_pinocchio_aba(model, pinocchio_benchmarks::get_log_filename(
"Pinocchio", "HD", model));
benchmark_pinocchio_crba(model, pinocchio_benchmarks::get_log_filename(
"Pinocchio", "FD", model));
benchmark_pinocchio_crba(model, pinocchio_benchmarks::get_log_filename(
"Pinocchio", "CRBA", model));
}
return 0;
......
......@@ -40,7 +40,7 @@ void benchmark_rbdl_rnea(std::string model, std::string log_filename)
}
end = std::chrono::high_resolution_clock::now();
std::chrono::nanoseconds time = end - start;
file << time.count() << std::endl;
file << 1e-3*time.count() / (double)NBT << std::endl;
file.close();
}
......@@ -65,14 +65,14 @@ void benchmark_rbdl_crba(std::string model, std::string log_filename)
std::chrono::time_point<std::chrono::high_resolution_clock> start, end;
std::ofstream file(log_filename);
start = std::chrono::high_resolution_clock::now();
start = std::chrono::high_resolution_clock::now();
for(size_t i=0; i<NBT; i++)
{
RigidBodyDynamics::CompositeRigidBodyAlgorithm(robot, qs[i], h);
}
end = std::chrono::high_resolution_clock::now();
std::chrono::nanoseconds time = end - start;
file << time.count() << std::endl;
end = std::chrono::high_resolution_clock::now();
std::chrono::nanoseconds time = end - start;
file << 1e-3*time.count() / (double)NBT << std::endl;
file.close();
}
......@@ -106,7 +106,7 @@ void benchmark_rbdl_aba(std::string model, std::string log_filename)
}
end = std::chrono::high_resolution_clock::now();
std::chrono::nanoseconds time = end - start;
file << time.count() << std::endl;
file << 1e-3*time.count() / (double)NBT << std::endl;
file.close();
}
......@@ -117,9 +117,9 @@ int main()
benchmark_rbdl_rnea(model, pinocchio_benchmarks::get_log_filename(
"RBDL", "ID", model));
benchmark_rbdl_crba(model, pinocchio_benchmarks::get_log_filename(
"RBDL", "FD", model));
"RBDL", "CRBA", model));
benchmark_rbdl_aba(model, pinocchio_benchmarks::get_log_filename(
"RBDL", "HD", model));
"RBDL", "FD", model));
}
return 0;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment