Commits

Martin Felis committed ee227b3

Method for solving linear system in FDLagrangian can now be specified

Comments (0)

Files changed (6)

addons/benchmark/benchmark.cc

 				sample_data.q_data[i],
 				sample_data.qdot_data[i],
 				sample_data.tau_data[i],
-				sample_data.qddot_data[i]);
+				sample_data.qddot_data[i],
+				LinearSolverPartialPivLU);
 	}
 
 	double duration = timer_stop (&tinfo);
 	}
 
 	if (benchmark_run_fd_lagrangian) {
-		cout << "= ForwardDynamics Lagrangian =" << endl;
+		cout << "= ForwardDynamics Lagrangian (Piv. LU decomposition) =" << endl;
 		for (int depth = 1; depth <= benchmark_model_max_depth; depth++) {
 			model = new Model();
 			model->Init();
 
 #ifndef RBDL_USE_SIMPLE_MATH
 	switch (CS.linear_solver) {
-		case (ConstraintSet::LinearSolverPartialPivLU) :
+		case (LinearSolverPartialPivLU) :
 			CS.x = CS.A.partialPivLu().solve(CS.b);
 			break;
-		case (ConstraintSet::LinearSolverColPivHouseholderQR) :
+		case (LinearSolverColPivHouseholderQR) :
 			CS.x = CS.A.colPivHouseholderQr().solve(CS.b);
 			break;
 		default:
 
 #ifndef RBDL_USE_SIMPLE_MATH
 	switch (CS.linear_solver) {
-		case (ConstraintSet::LinearSolverPartialPivLU) :
+		case (LinearSolverPartialPivLU) :
 			CS.x = CS.A.partialPivLu().solve(CS.b);
 			break;
-		case (ConstraintSet::LinearSolverColPivHouseholderQR) :
+		case (LinearSolverColPivHouseholderQR) :
 			CS.x = CS.A.colPivHouseholderQr().solve(CS.b);
 			break;
 		default:
 
 #ifndef RBDL_USE_SIMPLE_MATH
 	switch (CS.linear_solver) {
-		case (ConstraintSet::LinearSolverPartialPivLU) :
+		case (LinearSolverPartialPivLU) :
 			CS.constraint_force = CS.K.partialPivLu().solve(CS.a);
 			break;
-		case (ConstraintSet::LinearSolverColPivHouseholderQR) :
+		case (LinearSolverColPivHouseholderQR) :
 			CS.constraint_force = CS.K.colPivHouseholderQr().solve(CS.a);
 			break;
 		default:
 #ifndef _CONTACTS_H
 #define _CONTACTS_H
 
-#include "rbdl_math.h"
+#include <rbdl_math.h>
+#include <rbdl_mathutils.h>
 
 namespace RigidBodyDynamics {
 
  */
 struct ConstraintSet {
 	ConstraintSet() :
-		linear_solver (LinearSolverColPivHouseholderQR),
+		linear_solver (Math::LinearSolverColPivHouseholderQR),
 		bound (false)
 	{}
 
-	/** \brief Available solver methods for the linear systems.
-	 *
-	 * Please note that these methods are only available when Eigen3 is used.
-	 * When the math library SimpleMath is used it will always use a slow
-	 * column pivoting gauss elimination.
-	 *
-	 * Use ConstraintSet::SetSolver() to specify which solver should be used.
-	 */
-	enum LinearSolver {
-		LinearSolverUnknown = 0,
-		LinearSolverPartialPivLU,
-		LinearSolverColPivHouseholderQR,
-		LinearSolverLast,
-	};
-
 	/** \brief Adds a constraint to the constraint set.
 	 *
 	 * \param body_id the body which is affected directly by the constraint
 
 	/** \brief Specifies which method should be used for solving undelying linear systems.
 	 */
-	void SetSolver (LinearSolver solver) {
+	void SetSolver (Math::LinearSolver solver) {
 		linear_solver = solver;
 	}
 
 	void clear ();
 
 	/// Method that should be used to solve internal linear systems.
-	LinearSolver linear_solver;
+	Math::LinearSolver linear_solver;
 	/// Whether the constraint set was bound to a model (mandatory!).
 	bool bound;
 
 		const VectorNd &QDot,
 		const VectorNd &Tau,
 		VectorNd &QDDot,
+		Math::LinearSolver linear_solver,
 		std::vector<SpatialVector> *f_ext
 		) {
 	LOG << "-------- " << __func__ << " --------" << std::endl;
 	LOG << "b = " << std::endl << C * -1. + Tau << std::endl;
 
 #ifndef RBDL_USE_SIMPLE_MATH
-	QDDot = H.colPivHouseholderQr().solve (C * -1. + Tau);
+	switch (linear_solver) {
+		case (LinearSolverPartialPivLU) :
+			QDDot = H.partialPivLu().solve (C * -1. + Tau);
+			break;
+		case (LinearSolverColPivHouseholderQR) :
+			QDDot = H.colPivHouseholderQr().solve (C * -1. + Tau);
+			break;
+		default:
+			LOG << "Error: Invalid linear solver: " << linear_solver << std::endl;
+			assert (0);
+			break;
+	}
 #else
 	bool solve_successful = LinSolveGaussElimPivot (H, C * -1. + Tau, QDDot);
 	assert (solve_successful);
 #ifndef _DYNAMICS_H
 #define _DYNAMICS_H
 
-#include <rbdl_math.h>
 #include <assert.h>
 #include <iostream>
+
+#include <rbdl_math.h>
+#include <rbdl_mathutils.h>
+
 #include "Logging.h"
 
 namespace RigidBodyDynamics {
  * \param QDot  velocity vector of the internal joints
  * \param Tau   actuations of the internal joints
  * \param QDDot accelerations of the internal joints (output)
+ * \param linear_solver specification which method should be used for solving the linear system
  * \param f_ext External forces acting on the body in base coordinates (optional, defaults to NULL)
  */
 void ForwardDynamicsLagrangian (
 		const Math::VectorNd &QDot,
 		const Math::VectorNd &Tau,
 		Math::VectorNd &QDDot,
+		Math::LinearSolver linear_solver = Math::LinearSolverColPivHouseholderQR,
 		std::vector<Math::SpatialVector> *f_ext = NULL
 		);
 

src/rbdl_mathutils.h

 
 namespace Math {
 
+/** \brief Available solver methods for the linear systems.
+ *
+ * Please note that these methods are only available when Eigen3 is used.
+ * When the math library SimpleMath is used it will always use a slow
+ * column pivoting gauss elimination.
+ */
+enum LinearSolver {
+	LinearSolverUnknown = 0,
+	LinearSolverPartialPivLU,
+	LinearSolverColPivHouseholderQR,
+	LinearSolverLast,
+};
+
 extern Vector3d Vector3dZero;
 extern Matrix3d Matrix3dIdentity;
 extern Matrix3d Matrix3dZero;