Commits

Martin Felis  committed abc370b

backported benchmark to older revision

  • Participants
  • Parent commits 12b8b22

Comments (0)

Files changed (7)

File CMakeLists.txt

 OPTION (BUILD_TESTS "Build the test executables" OFF)
 OPTION (RBDL_ENABLE_LOGGING "Enable logging (warning: major impact on performance!)" OFF)
 OPTION (RBDL_USE_SIMPLE_MATH "Use slow math instead of the fast Eigen3 library (faster compilation)" OFF)
+OPTION (BUILD_ADDON_BENCHMARK "Build the benchmarking tool" OFF)
 
 SET ( RBDL_SOURCES 
 	src/mathutils.cc
 
 ADD_LIBRARY ( rbdl SHARED ${RBDL_SOURCES} )
 
+IF (BUILD_ADDON_BENCHMARK)
+  ADD_SUBDIRECTORY ( addons/benchmark )
+ENDIF (BUILD_ADDON_BENCHMARK)
+
 IF (BUILD_STATIC)
   ADD_LIBRARY ( rbdl-static STATIC ${RBDL_SOURCES} )
   SET_TARGET_PROPERTIES ( rbdl-static PROPERTIES PREFIX "lib")

File addons/benchmark/CMakeLists.txt

+PROJECT (RBDL_ADDON_BENCHMARK)
+
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+
+LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake )
+
+INCLUDE_DIRECTORIES ( 
+	${CMAKE_CURRENT_BINARY_DIR}/include/rbdl
+)
+
+SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES
+		LINKER_LANGUAGE CXX
+	)
+
+# Perform the proper linking
+SET (CMAKE_SKIP_BUILD_RPATH FALSE)
+SET (CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+SET (CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
+SET (CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+
+# Options
+SET ( BENCHMARK_SOURCES 
+	model_generator.cc
+	benchmark.cc
+	)
+
+ADD_EXECUTABLE ( benchmark ${BENCHMARK_SOURCES} )
+
+TARGET_LINK_LIBRARIES ( benchmark
+	rbdl
+	)
+

File addons/benchmark/SampleData.h

+#ifndef _SAMPLE_DATA_H
+#define _SAMPLE_DATA_H
+
+struct SampleData {
+	SampleData() :
+		q_data(NULL), qdot_data(NULL), qddot_data(NULL), tau_data(NULL)
+	{}
+	~SampleData() {
+		delete_data();
+	}
+
+	VectorNd *q_data;
+	VectorNd *qdot_data;
+	VectorNd *qddot_data;
+	VectorNd *tau_data;
+
+	void delete_data() {
+		if (q_data)
+			delete[] q_data;
+		q_data = NULL;
+
+		if (qdot_data)
+			delete[] qdot_data;
+		qdot_data = NULL;
+
+		if (qddot_data)
+			delete[] qddot_data;
+		qddot_data = NULL;
+
+		if (tau_data)
+			delete[] tau_data;
+		tau_data = NULL;
+	}
+
+	void fill_random_data (int dof_count, int sample_count) {
+		delete_data();
+
+		q_data = new VectorNd[sample_count];
+		qdot_data = new VectorNd[sample_count];
+		qddot_data = new VectorNd[sample_count];
+		tau_data = new VectorNd[sample_count];
+
+		for (int si = 0; si < sample_count; si++) {
+			q_data[si].resize (dof_count);
+			qdot_data[si].resize (dof_count);
+			qddot_data[si].resize (dof_count);
+			tau_data[si].resize (dof_count);
+
+			for (int i = 0; i < dof_count; i++) {
+				q_data[si][i] = (rand() / RAND_MAX) * 2. -1.;
+				qdot_data[si][i] = (rand() / RAND_MAX) * 2. -1.;
+				qddot_data[si][i] = (rand() / RAND_MAX) * 2. -1.;
+				tau_data[si][i] = (rand() / RAND_MAX) * 2. -1.;
+			}
+		}
+	}
+};
+
+#endif

File addons/benchmark/Timer.h

+#ifndef _TIMER_H
+#define _TIMER_H
+
+#include <ctime>
+#include <sys/time.h>
+
+struct TimerInfo {
+	/// time stamp when timer_start() gets called
+	struct timeval clock_start_value;
+	/// time stamp when the timer was stopped
+	struct timeval clock_end_value;
+
+	/// duration between clock_start_value and clock_end_value in seconds
+	double duration_sec;
+};
+
+inline void timer_start (TimerInfo *timer) {
+	gettimeofday (&(timer->clock_start_value), NULL);
+}
+
+inline double timer_stop (TimerInfo *timer) {
+	gettimeofday(&(timer->clock_end_value), NULL);
+
+	timer->duration_sec = static_cast<double> (
+			timer->clock_end_value.tv_sec - timer->clock_start_value.tv_sec)
+		+ static_cast<double>(timer->clock_end_value.tv_usec - timer->clock_start_value.tv_usec) * 1.0e-6;
+
+	return timer->duration_sec;
+}
+
+#endif

File addons/benchmark/benchmark.cc

+#include <iostream>
+
+#include <algorithm>
+#include <string>
+#include <vector>
+#include <cstdlib>
+#include <iomanip>
+#include <sstream>
+
+#include "rbdl.h"
+#include "model_generator.h"
+#include "SampleData.h"
+#include "Timer.h"
+
+using namespace std;
+using namespace RigidBodyDynamics;
+
+int benchmark_sample_count = 1000;
+int benchmark_model_max_depth = 5;
+bool benchmark_run_fd_aba = true;
+bool benchmark_run_fd_lagrangian = true;
+bool benchmark_run_id_rnea = true;
+
+double run_forward_dynamics_ABA_benchmark (Model *model, int sample_count) {
+	SampleData sample_data;
+	sample_data.fill_random_data(model->dof_count, sample_count);
+
+	TimerInfo tinfo;
+	timer_start (&tinfo);
+
+	for (int i = 0; i < sample_count; i++) {
+		ForwardDynamics (*model,
+				sample_data.q_data[i],
+				sample_data.qdot_data[i],
+				sample_data.tau_data[i],
+				sample_data.qddot_data[i]);
+	}
+
+	double duration = timer_stop (&tinfo);
+
+	cout << "#DOF: " << setw(3) << model->dof_count 
+		<< " #samples: " << sample_count 
+		<< " duration = " << setw(10) << duration << "(s)"
+		<< " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
+	
+	return duration;
+}
+
+double run_forward_dynamics_lagrangian_benchmark (Model *model, int sample_count) {
+	SampleData sample_data;
+	sample_data.fill_random_data(model->dof_count, sample_count);
+
+	TimerInfo tinfo;
+	timer_start (&tinfo);
+
+	for (int i = 0; i < sample_count; i++) {
+		ForwardDynamicsLagrangian (*model,
+				sample_data.q_data[i],
+				sample_data.qdot_data[i],
+				sample_data.tau_data[i],
+				sample_data.qddot_data[i]);
+	}
+
+	double duration = timer_stop (&tinfo);
+
+	cout << "#DOF: " << setw(3) << model->dof_count 
+		<< " #samples: " << sample_count 
+		<< " duration = " << setw(10) << duration << "(s)"
+		<< " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
+	
+	return duration;
+}
+
+double run_inverse_dynamics_RNEA_benchmark (Model *model, int sample_count) {
+	SampleData sample_data;
+	sample_data.fill_random_data(model->dof_count, sample_count);
+
+	TimerInfo tinfo;
+	timer_start (&tinfo);
+
+	for (int i = 0; i < sample_count; i++) {
+		InverseDynamics (*model,
+				sample_data.q_data[i],
+				sample_data.qdot_data[i],
+				sample_data.qddot_data[i],
+				sample_data.tau_data[i]
+				);
+	}
+
+	double duration = timer_stop (&tinfo);
+
+	cout << "#DOF: " << setw(3) << model->dof_count 
+		<< " #samples: " << sample_count 
+		<< " duration = " << setw(10) << duration << "(s)"
+		<< " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
+
+	return duration;
+}
+
+void print_usage () {
+	cout << "Usage: benchmark [--count|-c <sample_count>] [--depth|-d <depth>]" << endl;
+	cout << "Simple benchmark tool for the Rigid Body Dynamics Library." << endl;
+	cout << "  --count | -c <sample_count> : sets the number of sample states that should" << endl;
+	cout << "                be calculated (default: 1000)" << endl;
+	cout << "  --depth | -d <depth>        : sets maximum depth for the branched test model" << endl;
+	cout << "                which is created increased from 1 to <depth> (default: 5)." << endl;
+	cout << "  --no-fd                     : disables benchmarking of forward dynamics." << endl;
+	cout << "  --no-fd-aba                 : disables benchmark for forwards dynamics using" << endl;
+	cout << "                                the Articulated Body Algorithm" << endl;
+	cout << "  --no-fd-lagrangian          : disables benchmark for forward dynamics via" << endl;
+	cout << "                                solving the lagrangian equation." << endl;
+	cout << "  --no-id-rnea                : disables benchmark for inverse dynamics using" << endl;
+	cout << "                                the recursive newton euler algorithm." << endl;
+}
+
+void parse_args (int argc, char* argv[]) {
+	int argi = 1;
+	while (argi < argc) {
+		string arg = argv[argi];
+
+		if (arg == "--help" || arg == "-h") {
+			print_usage();
+			exit (1);
+		} else if (arg == "--count" || arg == "-c" ) {
+			if (argi == argc - 1) {
+				print_usage();
+
+				cerr << "Error: missing number of samples!" << endl;
+				exit (1);
+			}
+
+			argi++;
+			stringstream count_stream (argv[argi]);
+
+			count_stream >> benchmark_sample_count;
+		} else if (arg == "--depth" || arg == "-d" ) {
+			if (argi == argc - 1) {
+				print_usage();
+
+				cerr << "Error: missing number for model depth!" << endl;
+				exit (1);
+			}
+
+			argi++;
+			stringstream depth_stream (argv[argi]);
+
+			depth_stream >> benchmark_model_max_depth;
+		} else if (arg == "--no-fd" ) {
+			benchmark_run_fd_aba = false;
+			benchmark_run_fd_lagrangian = false;
+		} else if (arg == "--no-fd-aba" ) {
+			benchmark_run_fd_aba = false;
+		} else if (arg == "--no-fd-lagrangian" ) {
+			benchmark_run_fd_lagrangian = false;
+		} else if (arg == "--no-id-rnea" ) {
+			benchmark_run_id_rnea = false;
+		} else {
+			print_usage();
+			cerr << "Invalid argument '" << arg << "'." << endl;
+			exit(1);
+		}
+		argi++;
+	}
+}
+
+int main (int argc, char *argv[]) {
+	parse_args (argc, argv);
+
+	Model *model = NULL;
+
+	if (benchmark_run_fd_aba) {
+		cout << "= ForwardDynamics ABA =" << endl;
+		for (int depth = 1; depth <= benchmark_model_max_depth; depth++) {
+			model = new Model();
+			model->Init();
+			model->gravity = Vector3d (0., -9.81, 0.);
+
+			generate_planar_tree (model, depth);
+
+			run_forward_dynamics_ABA_benchmark (model, benchmark_sample_count);
+
+			delete model;
+		}
+		cout << endl;
+	}
+
+	if (benchmark_run_fd_lagrangian) {
+		cout << "= ForwardDynamics Lagrangian =" << endl;
+		for (int depth = 1; depth <= benchmark_model_max_depth; depth++) {
+			model = new Model();
+			model->Init();
+			model->gravity = Vector3d (0., -9.81, 0.);
+
+			generate_planar_tree (model, depth);
+
+			run_forward_dynamics_lagrangian_benchmark (model, benchmark_sample_count);
+
+			delete model;
+		}
+		cout << endl;
+	}
+
+	if (benchmark_run_id_rnea) {
+		cout << "= InverseDynamics RNEA =" << endl;
+		for (int depth = 1; depth <= benchmark_model_max_depth; depth++) {
+			model = new Model();
+			model->Init();
+			model->gravity = Vector3d (0., -9.81, 0.);
+
+			generate_planar_tree (model, depth);
+
+			run_inverse_dynamics_RNEA_benchmark (model, benchmark_sample_count);
+
+			delete model;
+		}
+	}
+
+	return 0;
+}

File addons/benchmark/model_generator.cc

+#include "model_generator.h"
+
+#include "rbdl.h"
+
+using namespace RigidBodyDynamics;
+
+void generate_planar_tree_recursive (Model *model,
+		unsigned int parent_body_id,
+		int depth,
+		double length) {
+	if (depth == 0)
+		return;
+
+	// create left child
+	Joint joint_rot_z (JointTypeRevolute, Vector3d (0., 0., 1.));
+	Body body (length, Vector3d (0., -0.25 * length, 0.), Vector3d (length, length, length));
+
+	Vector3d displacement (-0.5 * length, -0.25 * length, 0.);
+	unsigned int child_left = model->AddBody (parent_body_id, Xtrans (displacement), joint_rot_z, body);
+
+	generate_planar_tree_recursive (model,
+			child_left,
+			depth - 1,
+			length * 0.4);
+
+	displacement.set (0.5 * length, -0.25 * length, 0.);
+	unsigned int child_right = model->AddBody (parent_body_id, Xtrans (displacement), joint_rot_z, body);
+
+	generate_planar_tree_recursive (model,
+			child_right,
+			depth - 1,
+			length * 0.4);
+}
+
+void generate_planar_tree (Model *model, int depth) {
+	// we first add a single body that is hanging straight down from 
+	// (0, 0, 0). After that we generate the tree recursively such that each
+	// call adds two children.
+	//
+	double length = 1.;
+
+	Joint joint_rot_z (JointTypeRevolute, Vector3d (0., 0., 1.));
+	Body body (length, Vector3d (0., -0.25 * length, 0.), Vector3d (length, length, length));
+
+	unsigned int base_child = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_rot_z, body);
+
+	generate_planar_tree_recursive (
+			model,
+			base_child,
+			depth,
+			length * 0.4);
+}
+

File addons/benchmark/model_generator.h

+#ifndef _MODEL_GENERATOR_H
+#define _MODEL_GENERATOR_H
+
+namespace RigidBodyDynamics {
+	class Model;
+}
+
+void generate_planar_tree (RigidBodyDynamics::Model *model, int depth);
+
+/* _MODEL_GENERATOR_H */
+#endif