Commits

Martin Felis  committed 09390a6

added benchmark for CRBA

  • Participants
  • Parent commits ee227b3

Comments (0)

Files changed (1)

File addons/benchmark/benchmark.cc

 bool benchmark_run_fd_aba = true;
 bool benchmark_run_fd_lagrangian = true;
 bool benchmark_run_id_rnea = true;
+bool benchmark_run_crba = true;
 
 double run_forward_dynamics_ABA_benchmark (Model *model, int sample_count) {
 	SampleData sample_data;
 	return duration;
 }
 
+double run_CRBA_benchmark (Model *model, int sample_count) {
+	SampleData sample_data;
+	sample_data.fill_random_data(model->dof_count, sample_count);
+
+	TimerInfo tinfo;
+	timer_start (&tinfo);
+
+	Math::MatrixNd H = Math::MatrixNd(model->dof_count, model->dof_count);
+
+	for (int i = 0; i < sample_count; i++) {
+		CompositeRigidBodyAlgorithm (*model, sample_data.q_data[i], H, true);
+	}
+
+	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 << "                                solving the lagrangian equation." << endl;
 	cout << "  --no-id-rnea                : disables benchmark for inverse dynamics using" << endl;
 	cout << "                                the recursive newton euler algorithm." << endl;
+	cout << "  --no-crba                   : disables benchmark for joint space inertia" << endl;
+	cout << "                                matrix computation using the composite rigid." << endl;
+	cout << "                                body algorithm." << endl;
 }
 
 void parse_args (int argc, char* argv[]) {
 			benchmark_run_fd_lagrangian = false;
 		} else if (arg == "--no-id-rnea" ) {
 			benchmark_run_id_rnea = false;
+		} else if (arg == "--no-crba" ) {
+			benchmark_run_crba = false;
 		} else {
 			print_usage();
 			cerr << "Invalid argument '" << arg << "'." << endl;
 	cout << endl;
 
 	if (benchmark_run_fd_aba) {
-		cout << "= ForwardDynamics ABA =" << endl;
+		cout << "= Forward Dynamics: ABA =" << endl;
 		for (int depth = 1; depth <= benchmark_model_max_depth; depth++) {
 			model = new Model();
 			model->Init();
 	}
 
 	if (benchmark_run_fd_lagrangian) {
-		cout << "= ForwardDynamics Lagrangian (Piv. LU decomposition) =" << endl;
+		cout << "= Forward Dynamics: Lagrangian (Piv. LU decomposition) =" << endl;
 		for (int depth = 1; depth <= benchmark_model_max_depth; depth++) {
 			model = new Model();
 			model->Init();
 	}
 
 	if (benchmark_run_id_rnea) {
-		cout << "= InverseDynamics RNEA =" << endl;
+		cout << "= Inverse Dynamics: RNEA =" << endl;
 		for (int depth = 1; depth <= benchmark_model_max_depth; depth++) {
 			model = new Model();
 			model->Init();
 
 			delete model;
 		}
+		cout << endl;
+	}
+
+	if (benchmark_run_crba) {
+		cout << "= Joint Space Inertia Matrix: CRBA =" << 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_CRBA_benchmark (model, benchmark_sample_count);
+
+			delete model;
+		}
+		cout << endl;
 	}
 
 	return 0;