Commits

Martin Felis committed a4cec8e

added benchmark for the contact routines (not run automatically)

Comments (0)

Files changed (4)

addons/benchmark/CMakeLists.txt

 # Options
 SET ( BENCHMARK_SOURCES 
 	model_generator.cc
+	Human36Model.cc
 	benchmark.cc
 	)
 

addons/benchmark/Human36Model.cc

+#include "Human36Model.h"
+
+#include "rbdl.h"
+
+using namespace RigidBodyDynamics;
+using namespace RigidBodyDynamics::Math;
+
+enum SegmentName {
+	SegmentPelvis = 0,
+	SegmentThigh,
+	SegmentShank,
+	SegmentFoot,
+	SegmentMiddleTrunk,
+	SegmentUpperTrunk,
+	SegmentUpperArm,
+	SegmentLowerArm,
+	SegmentHand,
+	SegmentHead,
+	SegmentNameLast
+};
+
+double SegmentLengths[SegmentNameLast] = {
+	0.1457,
+	0.4222,
+	0.4403,
+	0.1037,
+	0.2155,
+	0.2421,
+	0.2817,
+	0.2689,
+	0.0862,
+	0.2429
+};
+
+double SegmentMass[SegmentNameLast] = {
+	0.8154,
+	10.3368,
+	3.1609,
+	1.001,
+	16.33,
+	15.96,
+	1.9783,
+	1.1826,
+	0.4453,
+	5.0662
+};
+
+double SegmentCOM[SegmentNameLast][3] = {
+	{ 0., 0.,  0.0891},
+	{ 0., 0., -0.1729},
+	{ 0., 0., -0.1963},
+	{ 0.1254, 0., -0.0516},
+	{ 0., 0.,  0.1185},
+	{ 0., 0.,  0.1195},
+	{ 0., 0., -0.1626},
+	{ 0., 0., -0.1230},
+	{ 0., 0., -0.0680},
+	{ 0., 0.,  1.1214}
+};
+
+double SegmentRadiiOfGyration[SegmentNameLast][3] = {
+	{ 0.0897, 0.0855, 0.0803},
+	{ 0.1389, 0.0629, 0.1389},
+	{ 0.1123, 0.0454, 0.1096},
+	{ 0.0267, 0.0129, 0.0254},
+	{ 0.0970, 0.1009, 0.0825},
+ 	{ 0.1273, 0.1172, 0.0807},
+	{ 0.0803, 0.0758, 0.0445},
+	{ 0.0742, 0.0713, 0.0325},
+	{ 0.0541, 0.0442, 0.0346},
+	{ 0.0736, 0.0634, 0.0765}
+};
+
+Body create_body (SegmentName segment) {
+	return Body (
+			SegmentMass[segment],
+			Vector3d (
+				SegmentCOM[segment][0],
+				SegmentCOM[segment][1],
+				SegmentCOM[segment][2]
+				),
+			Vector3d (
+				SegmentRadiiOfGyration[segment][0],
+				SegmentRadiiOfGyration[segment][1],
+				SegmentRadiiOfGyration[segment][2]
+				)
+			);
+}
+
+void generate_human36model (RigidBodyDynamics::Model *model) {
+	Body pelvis_body = create_body (SegmentPelvis);
+	Body thigh_body = create_body (SegmentThigh);
+	Body shank_body = create_body (SegmentShank);
+	Body foot_body = create_body (SegmentFoot);
+	Body middle_trunk_body = create_body (SegmentMiddleTrunk);
+	Body upper_trunk_body = create_body (SegmentUpperTrunk);
+	Body upperarm_body = create_body (SegmentUpperArm);
+	Body lowerarm_body = create_body (SegmentLowerArm);
+	Body hand_body = create_body (SegmentHand);
+	Body head_body = create_body (SegmentHead);
+
+	Joint free_flyer (
+		SpatialVector (0., 0., 0., 1., 0., 0.),
+		SpatialVector (0., 0., 0., 0., 1., 0.),
+		SpatialVector (0., 0., 0., 0., 0., 1.),
+		SpatialVector (0., 1., 0., 0., 0., 0.),
+		SpatialVector (0., 0., 1., 0., 0., 0.),
+		SpatialVector (1., 0., 0., 0., 0., 0.)
+		);
+
+	Joint rot_yzx (
+			SpatialVector (0., 1., 0., 0., 0., 0.),
+			SpatialVector (0., 0., 1., 0., 0., 0.),
+			SpatialVector (1., 0., 0., 0., 0., 0.)
+			);
+
+	Joint rot_yz (
+			SpatialVector (0., 1., 0., 0., 0., 0.),
+			SpatialVector (0., 0., 1., 0., 0., 0.)
+			);
+
+	Joint rot_y (
+			SpatialVector (0., 1., 0., 0., 0., 0.)
+			);
+
+	Joint fixed (JointTypeFixed);
+
+	model->gravity = Vector3d (0., 0., -9.81);
+
+	unsigned int pelvis_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), free_flyer, pelvis_body, "pelvis");
+
+	// right leg
+	model->AddBody (pelvis_id, Xtrans(Vector3d(0., -0.0872, 0.)), rot_yzx, thigh_body, "thigh_r");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_r");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_r");
+
+	// left leg
+	model->AddBody (pelvis_id, Xtrans(Vector3d(0., 0.0872, 0.)), rot_yzx, thigh_body, "thigh_l");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_l");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_l");
+
+	// trunk
+	model->AddBody (pelvis_id, Xtrans(Vector3d(0., 0., SegmentLengths[SegmentPelvis])), rot_yzx, middle_trunk_body, "middletrunk");
+	unsigned int uppertrunk_id = model->AppendBody (Xtrans(Vector3d(0., 0., SegmentLengths[SegmentMiddleTrunk])), fixed, upper_trunk_body, "uppertrunk");
+
+	// right arm
+	model->AddBody (uppertrunk_id, Xtrans(Vector3d(0., -0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yzx, upperarm_body, "upperarm_r");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_r");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_r");
+
+	// left arm
+	model->AddBody (uppertrunk_id, Xtrans(Vector3d(0.,  0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yzx, upperarm_body, "upperarm_l");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_l");
+	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_l");
+
+	// head	
+	model->AddBody (uppertrunk_id, Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yzx, upperarm_body, "head");
+}

addons/benchmark/Human36Model.h

+#ifndef _HUMAN36MODEL_H
+#define _HUMAN36MODEL_H
+
+namespace RigidBodyDynamics {
+	class Model;
+}
+
+void generate_human36model (RigidBodyDynamics::Model *model);
+
+/* _HUMAN36MODEL_H */
+#endif

addons/benchmark/benchmark.cc

 
 #include "rbdl.h"
 #include "model_generator.h"
+#include "Human36Model.h"
 #include "SampleData.h"
 #include "Timer.h"
 
 
 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;
 bool benchmark_run_crba = true;
+bool benchmark_run_contacts = false;
+
+enum ContactsBenchmark {
+	ContactsBenchmarkLagrangian = 0,
+	ContactsBenchmarkKokkevis
+};
 
 double run_forward_dynamics_ABA_benchmark (Model *model, int sample_count) {
 	SampleData sample_data;
 				sample_data.q_data[i],
 				sample_data.qdot_data[i],
 				sample_data.tau_data[i],
-				sample_data.qddot_data[i],
-				LinearSolverPartialPivLU);
+				sample_data.qddot_data[i]
+);
 	}
 
 	double duration = timer_stop (&tinfo);
 	SampleData sample_data;
 	sample_data.fill_random_data(model->dof_count, sample_count);
 
+	Math::MatrixNd H = Math::MatrixNd(model->dof_count, model->dof_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);
 	}
 	return duration;
 }
 
+double run_contacts_lagrangian_benchmark (Model *model, ConstraintSet *constraint_set, 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++) {
+		ForwardDynamicsContactsLagrangian (*model, sample_data.q_data[i], sample_data.qdot_data[i], sample_data.tau_data[i], *constraint_set, sample_data.qddot_data[i]); 
+	}
+
+	double duration = timer_stop (&tinfo);
+
+	return duration;
+}
+
+double run_contacts_kokkevis_benchmark (Model *model, ConstraintSet *constraint_set, 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++) {
+		ForwardDynamicsContacts (*model, sample_data.q_data[i], sample_data.qdot_data[i], sample_data.tau_data[i], *constraint_set, sample_data.qddot_data[i]); 
+	}
+
+	double duration = timer_stop (&tinfo);
+
+	return duration;
+}
+
+double contacts_benchmark (int sample_count, ContactsBenchmark contacts_benchmark) {
+	// initialize the human model
+	Model *model = new Model();
+	model->Init();
+	generate_human36model(model);
+
+	// initialize the constraint sets
+	unsigned int foot_r = model->GetBodyId ("foot_r");
+	unsigned int foot_l = model->GetBodyId ("foot_l");
+	unsigned int hand_r = model->GetBodyId ("hand_r");
+	unsigned int hand_l = model->GetBodyId ("hand_l");
+
+	ConstraintSet one_body_one_constraint;
+
+	ConstraintSet two_bodies_one_constraint;
+	ConstraintSet four_bodies_one_constraint;
+
+	ConstraintSet two_bodies_four_constraints;
+	ConstraintSet four_bodies_four_constraints;
+
+	// one_body_one
+	one_body_one_constraint.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	one_body_one_constraint.Bind (*model);
+
+	// two_bodies_one
+	two_bodies_one_constraint.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	two_bodies_one_constraint.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	two_bodies_one_constraint.Bind (*model);
+
+	// four_bodies_one
+	four_bodies_one_constraint.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_one_constraint.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_one_constraint.AddConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_one_constraint.AddConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_one_constraint.Bind (*model);
+
+	// two_bodies_four
+	two_bodies_four_constraints.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	two_bodies_four_constraints.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
+	two_bodies_four_constraints.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
+	two_bodies_four_constraints.AddConstraint (foot_r, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	
+	two_bodies_four_constraints.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	two_bodies_four_constraints.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
+	two_bodies_four_constraints.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
+	two_bodies_four_constraints.AddConstraint (foot_l, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
+
+	two_bodies_four_constraints.Bind (*model);
+
+	// four_bodies_four
+	four_bodies_four_constraints.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_four_constraints.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
+	four_bodies_four_constraints.AddConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
+	four_bodies_four_constraints.AddConstraint (foot_r, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	
+	four_bodies_four_constraints.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_four_constraints.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
+	four_bodies_four_constraints.AddConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
+	four_bodies_four_constraints.AddConstraint (foot_l, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
+
+	four_bodies_four_constraints.AddConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_four_constraints.AddConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
+	four_bodies_four_constraints.AddConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
+	four_bodies_four_constraints.AddConstraint (hand_r, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	
+	four_bodies_four_constraints.AddConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
+	four_bodies_four_constraints.AddConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
+	four_bodies_four_constraints.AddConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
+	four_bodies_four_constraints.AddConstraint (hand_l, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
+
+	four_bodies_four_constraints.Bind (*model);
+
+	cout << "= #DOF: " << setw(3) << model->dof_count << endl;
+	cout << "= #samples: " << sample_count << endl;
+	cout << "= No constraints (Articulated Body Algorithm):" << endl;
+	run_forward_dynamics_ABA_benchmark (model, sample_count);
+	cout << "= Constraints:" << endl;
+	double duration;
+
+	// one body one
+	if (contacts_benchmark == ContactsBenchmarkLagrangian) {
+		duration = run_contacts_lagrangian_benchmark (model, &one_body_one_constraint, sample_count);
+	} else {
+		duration = run_contacts_kokkevis_benchmark (model, &one_body_one_constraint, sample_count);
+	}
+
+	cout << "ConstraintSet: 1 Body 1 Constraint   : "
+		<< " duration = " << setw(10) << duration << "(s)"
+		<< " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
+
+	// two_bodies_one
+	if (contacts_benchmark == ContactsBenchmarkLagrangian) {
+		duration = run_contacts_lagrangian_benchmark (model, &two_bodies_one_constraint, sample_count);
+	} else {
+		duration = run_contacts_kokkevis_benchmark (model, &two_bodies_one_constraint, sample_count);
+	}
+
+	cout << "ConstraintSet: 2 Bodies 1 Constraint : "
+		<< " duration = " << setw(10) << duration << "(s)"
+		<< " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
+
+
+	// four_bodies_one
+	if (contacts_benchmark == ContactsBenchmarkLagrangian) {
+		duration = run_contacts_lagrangian_benchmark (model, &four_bodies_one_constraint, sample_count);
+	} else {
+		duration = run_contacts_kokkevis_benchmark (model, &four_bodies_one_constraint, sample_count);
+	}
+
+	cout << "ConstraintSet: 4 Bodies 1 Constraint : "
+		<< " duration = " << setw(10) << duration << "(s)"
+		<< " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
+
+
+	// two_bodies_four
+	if (contacts_benchmark == ContactsBenchmarkLagrangian) {
+		duration = run_contacts_lagrangian_benchmark (model, &two_bodies_four_constraints, sample_count);
+	} else {
+		duration = run_contacts_kokkevis_benchmark (model, &two_bodies_four_constraints, sample_count);
+	}
+
+	cout << "ConstraintSet: 2 Bodies 4 Constraints: "
+		<< " duration = " << setw(10) << duration << "(s)"
+		<< " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
+
+
+	// four_bodies_four
+	if (contacts_benchmark == ContactsBenchmarkLagrangian) {
+		duration = run_contacts_lagrangian_benchmark (model, &four_bodies_four_constraints, sample_count);
+	} else {
+		duration = run_contacts_kokkevis_benchmark (model, &four_bodies_four_constraints, sample_count);
+	}
+
+	cout << "ConstraintSet: 4 Bodies 4 Constraints: "
+		<< " duration = " << setw(10) << duration << "(s)"
+		<< " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
+
+	delete model;
+
+	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 << "  --no-crba                   : disables benchmark for joint space inertia" << endl;
 	cout << "                                matrix computation using the composite rigid." << endl;
 	cout << "                                body algorithm." << endl;
+	cout << "  --only-contacts | -C        : only runs contact model benchmarks." << endl;
+	cout << "  --help | -h                 : prints this help." << endl;
+}
+
+void disable_all_benchmarks () {
+	benchmark_run_fd_aba = false;
+	benchmark_run_fd_lagrangian = false;
+	benchmark_run_id_rnea = false;
+	benchmark_run_crba = false;
+	benchmark_run_contacts = false;
 }
 
 void parse_args (int argc, char* argv[]) {
 			benchmark_run_id_rnea = false;
 		} else if (arg == "--no-crba" ) {
 			benchmark_run_crba = false;
+		} else if (arg == "--only-contacts") {
+			disable_all_benchmarks();
+			benchmark_run_contacts = true;
 		} else {
 			print_usage();
 			cerr << "Invalid argument '" << arg << "'." << endl;
 
 	Model *model = NULL;
 
+	model = new Model();
+	model->Init();
+	generate_human36model (model);
+	cout << "Human dofs = " << model->dof_count << endl;
+	delete model;
+
 	RigidBodyDynamics::rbdl_print_version();
 	cout << endl;
 
 		cout << endl;
 	}
 
+	if (benchmark_run_contacts) {
+		cout << "= Contacts: ForwardDynamicsContactsLagrangian" << endl;
+		contacts_benchmark (benchmark_sample_count, ContactsBenchmarkLagrangian);
+
+		cout << "= Contacts: ForwardDynamicsContactsKokkevis" << endl;
+		contacts_benchmark (benchmark_sample_count, ContactsBenchmarkKokkevis);
+	}
+
+
+
 	return 0;
 }