Commits

Martin Felis committed 551297c Draft

some progress

  • Participants
  • Parent commits 4acd9c7
  • Branches ginac

Comments (0)

Files changed (9)

 	 * \param com  the position of the center of mass in the bodies coordinates
 	 * \param gyration_radii the radii of gyration at the center of mass of the body
 	 */
-	Body(const double &mass,
+	Body(const value_type &mass,
 			const Vector3d &com,
 			const Vector3d &gyration_radii) :
 		mMass (mass),
 	 * \param com  the position of the center of mass in the bodies coordinates
 	 * \param inertia_C the inertia at the center of mass
 	 */
-	Body(const double &mass,
+	Body(const value_type &mass,
 			const Vector3d &com,
 			const Matrix3d &inertia_C) :
 		mMass (mass),
 	 * \param length the length of the segment (needed to compute the inertia at the CoM
 	 * \param gyration_radii the radii of gyration at the center of mass of the body in percentage of the segment length
 	 */
-	Body(const double &mass,
+	Body(const value_type &mass,
 			const Vector3d &com,
-			const double &length,
+			const value_type &length,
 			const Vector3d &gyration_radii) :
 		mMass (mass),
 		mCenterOfMass(com) {
 	~Body() {};
 
 	/// \brief The mass of the body
-	double mMass;
+	value_type mMass;
 	/// \brief The position of the center of mass in body coordinates
 	Vector3d mCenterOfMass;
 	/// \brief The spatial inertia that contains both mass and inertia information

File src/Contacts.h

 	Vector3d point;
 	/// \brief Normal of the contact point in base coordinates
 	Vector3d normal;
-	/// \brief Acceleration value of the constraint along the normal
+	/// \brief Acceleration value_type of the constraint along the normal
 	double acceleration;
 	/// \brief Force acting along the normal
 	double force;
  * \param ContactData	a list of all contact points
  * \param QDDot accelerations of the internals joints (output)
  *
- * \note During execution of this function the values ContactData[i].force
- * 	get modified and will contain the value of the force acting along
+ * \note During execution of this function the value_types ContactData[i].force
+ * 	get modified and will contain the value_type of the force acting along
  * 	the normal.
  */
 void ForwardDynamicsContactsLagrangian (
  * \param ContactData	a list of all contact points
  * \param QDotPlus velocities of the internals joints after the impact (output)
  *
- * \note During execution of this function the values ContactInfo::force
- * 	get modified and will contain the value of the impulse acting along
+ * \note During execution of this function the value_types ContactInfo::force
+ * 	get modified and will contain the value_type of the impulse acting along
  * 	the normal.
  */
 void ComputeContactImpulsesLagrangian (
  * \param ContactData	a list of all contact points and their desired accelerations
  * \param Fext  constraint forces that enforce desired acceleration on the constraints
  *
- * \note During execution of this function the values ContactData[i].force
- * 	get modified and will contain the value of the force acting along
+ * \note During execution of this function the value_types ContactData[i].force
+ * 	get modified and will contain the value_type of the force acting along
  * 	the normal.
  */
 void ComputeContactForces (
  * \param ContactData	a list of all contact points
  * \param QDDot accelerations of the internals joints (output)
  *
- * \note During execution of this function the values ContactData[i].force
- * 	get modified and will contain the value of the force acting along
+ * \note During execution of this function the value_types ContactData[i].force
+ * 	get modified and will contain the value_type of the force acting along
  * 	the normal.
  */
 void ForwardDynamicsContacts (

File src/Dynamics.cc

 		assert (0 && "Experimental floating base not supported");
 	}
 
-	SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]);
+	SpatialVector spatial_gravity (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]);
 
 	unsigned int i;
 
 		SpatialMatrix X_lambda = model.X_lambda[i];
 
 		if (lambda == 0) {
-			model.a[i] = X_lambda * spatial_gravity * (-1.) + model.c[i];
+			// ginacfix
+			//		model.a[i] = X_lambda * spatial_gravity * (-1.) + model.c[i];
+			model.a[i] = X_lambda * spatial_gravity + model.c[i];
 		} else {
 			model.a[i] = X_lambda * model.a[lambda] + model.c[i];
 		}
 	CompositeRigidBodyAlgorithm (model, Q, H);
 
 	LOG << "A = " << std::endl << H << std::endl;
-	LOG << "b = " << std::endl << C * -1. + Tau << std::endl;
+// ginacfix
+// LOG << "b = " << std::endl << C * -1. + Tau << std::endl;
+	LOG << "b = " << std::endl << C + Tau << std::endl;
 
 #ifndef RBDL_USE_SIMPLE_MATH
 	QDDot = H.colPivHouseholderQr().solve (C * -1. + Tau);
 	if (model.experimental_floating_base)
 		assert (0 && !"InverseDynamics not supported for experimental floating base models!");
 
-	SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]);
+	SpatialVector spatial_gravity (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]);
 
 	unsigned int i;
 
 
 	// Reset the velocity of the root body
 	model.v[0].setZero();
-	model.a[0] = spatial_gravity * -1.;
+	model.a[0] = spatial_gravity;
 
 	for (i = 1; i < model.mBodies.size(); i++) {
 		SpatialMatrix X_J;
 		if (lambda == 0) {
 			model.X_base[i] = model.X_lambda[i];
 			model.v[i] = v_J;
-			model.a[i] = model.X_base[i] * spatial_gravity * -1. + model.S[i] * model.qddot[i];
+			model.a[i] = model.X_base[i] * spatial_gravity + model.S[i] * model.qddot[i];
 		}	else {
 			model.X_base[i] = model.X_lambda[i] * model.X_base.at(lambda);
 			model.v[i] = model.X_lambda[i] * model.v[lambda] + v_J;
 		std::vector<ContactInfo> &ContactData,
 		VectorNd &QDDot
 		) {
+#ifdef GINAC_MATH
+	assert (0 && !"Function not supported with ginac math");
+#else
 	LOG << "-------- " << __func__ << " --------" << std::endl;
 
 	// Note: InverseDynamics must be called *before*
 	for (i = 0; i < ContactData.size(); i++) {
 		ContactData[i].force = x[model.dof_count + i];
 	}
+#endif
 }
 
 void ComputeContactImpulsesLagrangian (
 		std::vector<ContactInfo> &ContactData,
 		VectorNd &QDotPlus
 		) {
+#ifdef GINAC_MATH
+	assert (0 && !"Function not supported in ginac math");
+#else
 	LOG << "-------- " << __func__ << " --------" << std::endl;
 
 	// Compute H
 		ContactData[i].force = x[model.dof_count + i];
 	}
 
+#endif
 }
 
 
 	a_B = model.a[0];
 }
 
-void ComputeContactForces (
-		Model &model,
-		const VectorNd &Q,
-		const VectorNd &QDot,
-		const VectorNd &Tau,
-		std::vector<ContactInfo> &ContactData,
-		std::vector<SpatialAlgebra::SpatialVector> &Fext
-		) {
-	LOG << "-------- ComputeContactForces ------" << std::endl;
-
-	// so far we only allow one constraint
-	unsigned int contact_count = ContactData.size();
-
-	// Steps to perform the contact algorithm suggested by Kokkevis and
-	// Metaxas
-	//
-	// 1. Set external forces at P to zero and compute link accelerations and
-	// compute a^0_P (the magnitude of the acceleration of P) and evaluate
-	// C^0
-	//
-	// 2. Apply a unit force at P and compute a^1_P (the magnitude of the
-	// resulting acceleration) and compute the net effect a^e_P of f^1.
-	//
-	// 3. Compute the required constraint force.
-	
-	// Step one, compute the standard forward dynamics without external
-	// forces at P.
-	
-	// Step 1:
-	
-	// save current external forces:
-
-	MatrixNd Ae;
-	Ae.resize(contact_count, contact_count);
-	VectorNd C0 (contact_count);
-	VectorNd a0 (contact_count);
-
-	std::vector<SpatialVector> current_f_ext (model.f_ext);
-	std::vector<SpatialVector> zero_f_ext (model.f_ext.size(), SpatialVector (0., 0., 0., 0., 0., 0.));
-	Vector3d gravity_backup = model.gravity;
-
-	model.f_ext = zero_f_ext;
-
-	LOG << "-------- ZERO_EXT ------" << std::endl;
-	VectorNd QDDot_zero_ext (QDot);
-	{
-		SUPPRESS_LOGGING;
-		ForwardDynamics (model, Q, QDot, Tau, QDDot_zero_ext);
-	}
-
-	unsigned int ci;
-	for (ci = 0; ci < contact_count; ci++) {
-		ContactInfo contact_info = ContactData[ci];
-		LOG << "ContactData[" << ci << "].acceleration = " << contact_info.acceleration << std::endl;
-
-		// compute point accelerations
-		Vector3d point_accel;
-		{
-			SUPPRESS_LOGGING;
-			point_accel = CalcPointAcceleration (model, Q, QDot, QDDot_zero_ext, contact_info.body_id, contact_info.point);
-		}
-
-		// evaluate a0 and C0
-//		double a0i = cml::dot(contact_info.normal,point_accel);
-		double a0i = contact_info.normal.dot(point_accel);
-
-		a0[ci] = a0i;
-		C0[ci] = - (a0i - contact_info.acceleration);
-	}
-
-	// Step 2:
-	std::vector<SpatialVector> test_forces (contact_count);
-
-	unsigned int cj;
-	// Compute the test force
-	for (cj = 0; cj < contact_count; cj++) {
-		ContactInfo contact_info = ContactData[cj];
-		SpatialVector test_force (0., 0., 0., contact_info.normal[0], contact_info.normal[1], contact_info.normal[2]);
-
-		// transform the test force from the point coordinates to base
-		// coordinates
-		Vector3d contact_point_position = model.CalcBodyToBaseCoordinates(contact_info.body_id, contact_info.point);
-
-		test_forces[cj] = spatial_adjoint(Xtrans (contact_point_position)) * test_force;
-		LOG << "body_id         = " << contact_info.body_id << std::endl;
-
-		// apply the test force
-		model.f_ext[contact_info.body_id] = test_forces[cj];
-		VectorNd QDDot_test_ext (QDot);
-
-		LOG << "-------- TEST_EXT -------" << std::endl;
-		LOG << "test_force_body = " << spatial_adjoint(Xtrans (model.GetBodyOrigin(contact_info.body_id) - contact_point_position)) * test_forces[cj] << std::endl;
-		LOG << "test_force_base = " << test_forces[cj] << std::endl;
-		{
-			SUPPRESS_LOGGING;
-			ForwardDynamics (model, Q, QDot, Tau, QDDot_test_ext);
-		}
-		LOG << "QDDot_test_ext  = " << QDDot_test_ext << std::endl;
-
-		for (ci = 0; ci < contact_count; ci++) {
-			ContactInfo test_contact_info = ContactData[ci];
-			// compute point accelerations after the test force
-			Vector3d point_test_accel;
-			{
-				SUPPRESS_LOGGING;
-				point_test_accel = CalcPointAcceleration (model, Q, QDot, QDDot_test_ext, test_contact_info.body_id, test_contact_info.point);
-			}
-
-			// acceleration due to the test force
-//			double a1j_i = cml::dot(test_contact_info.normal, point_test_accel);
-			double a1j_i = test_contact_info.normal.dot(point_test_accel);
-			LOG << "test_accel a1j_i = " << a1j_i << std::endl;
-			LOG << "a0[ci] = " << a0[ci] << std::endl;
-			Ae(ci,cj) = a1j_i - a0[ci];
-			LOG << "updating (" << ci << ", " << cj << ") = " << Ae(ci,cj) << std::endl;
-		}
-
-		// clear the test force
-		model.f_ext[contact_info.body_id].setZero();
-	}
-	
-	// solve the system!!!
-	VectorNd u (contact_count);
-
-	LOG << "Ae = " << std::endl << Ae << std::endl;
-	LOG << "C0 = " << C0 << std::endl;
-	LinSolveGaussElimPivot (Ae, C0, u);
-
-	// !!!
-	u[0] = 8.81;
-//	test_forces[0].setZero(); 
-
-	LOG << "u = " << u << std::endl;
-
-	// compute and apply the constraint forces to the system
-	model.f_ext = current_f_ext;
-
-	Fext = zero_f_ext;
-
-	for (ci = 0; ci < contact_count; ci++) {
-		ContactData[ci].force = u[ci];
-
-		test_forces[ci] = test_forces[ci] * u[ci];
-		// it is important to *add* the constraint force as multiple forces
-		// might act on the same body
-		Fext[ContactData[ci].body_id] += test_forces[ci];
-		LOG << "test_forces[" << ci << "] = " << test_forces[ci] << std::endl;
-		LOG << "f_ext[" << ContactData[ci].body_id << "] = " << Fext[ContactData[ci].body_id] << std::endl;
-	}
-}
-
-void ForwardDynamicsContacts (
-		Model &model,
-		const VectorNd &Q,
-		const VectorNd &QDot,
-		const VectorNd &Tau,
-		std::vector<ContactInfo> &ContactData,
-		VectorNd &QDDot
-		) {
-	LOG << "-------- ForwardDynamicsContacts ------" << std::endl;
-
-	std::vector<SpatialVector> contact_f_ext (model.f_ext.size(), SpatialVector (0., 0., 0., 0., 0., 0.));
-
-	ComputeContactForces (model, Q, QDot, Tau, ContactData, contact_f_ext);
-
-	assert (contact_f_ext.size() == model.f_ext.size());
-
-	unsigned int i;
-	for (i = 0; i < model.f_ext.size(); i++) {
-		model.f_ext[i] += contact_f_ext[i];
-		LOG << "f_ext[" << i << "] = " << model.f_ext[i] << std::endl;
-	}
-
-	LOG << "-------- APPLY_EXT ------" << std::endl;
-	{
-		SUPPRESS_LOGGING;
-		ForwardDynamics (model, Q, QDot, Tau, QDDot);
-	}
-	LOG << "apply q     = " << Q << std::endl;
-	LOG << "apply qdot  = " << QDot << std::endl;
-	LOG << "apply tau   = " << Tau << std::endl;
-	LOG << "apply qddot = " << QDDot << std::endl;
-}
-
 } /* namespace Experimental */
 
 } /* namespace RigidBodyDynamics */

File src/Joint.cc

 		SpatialVector &S,
 		SpatialVector &v_J,
 		SpatialVector &c_J,
-		const double &q,
-		const double &qdot
+		const value_type &q,
+		const value_type &qdot
 		) {
 	// exception if we calculate it for the root body
 	assert (joint_id > 0);
 		SpatialAlgebra::SpatialVector &S,
 		SpatialAlgebra::SpatialVector &v_J,
 		SpatialAlgebra::SpatialVector &c_J,
-		const double &q,
-		const double &qdot
+		const value_type &q,
+		const value_type &qdot
 		);
 
 }

File src/SimpleMath.h

 	unsigned int parent_row_index;
 	unsigned int parent_col_index;
 	bool transposed;
+	typedef val_type value_type;
 
 	val_type *parent;
 };
 			for (i = 0; i < nrows * ncols; i++)
 				mData[i] = matrix.mData[i];
 		}
+
 		Matrix& operator=(const Matrix &matrix) {
 			if (this != &matrix) {
 				unsigned int i;
 		}
 
 		// access operators
-		const double& operator[](const unsigned int &index) const {
+		const val_type& operator[](const unsigned int &index) const {
 			assert (index	>= 0 && index < nrows * ncols);
 			return mData[index];
 		};
-		double& operator[](const unsigned int &index) {
+		val_type& operator[](const unsigned int &index) {
 			assert (index	>= 0 && index < nrows * ncols);
 			return mData[index];
 		}
 
-		const double& operator()(const unsigned int &row, const unsigned int &col) const {
+		const val_type& operator()(const unsigned int &row, const unsigned int &col) const {
 			assert (row	>= 0 && row < nrows && col	>= 0 && col < ncols);
 			return mData[row*ncols + col];
 		};
-		double& operator()(const unsigned int &row, const unsigned int &col) {
+		val_type& operator()(const unsigned int &row, const unsigned int &col) {
 			assert (row	>= 0 && row < nrows && col	>= 0 && col < ncols);
 			return mData[row*ncols + col];
 		};
 		Block<val_type, blockrows, blockcols> block (unsigned int i, unsigned int j) const {
 			COMPILE_ASSERT (nrows > blockrows);
 			COMPILE_ASSERT (ncols > blockcols);
-			return Block<val_type, blockrows, blockcols> (const_cast<double*> (this->mData), i, j, nrows, ncols);
+			return Block<val_type, blockrows, blockcols> (const_cast<val_type*> (this->mData), i, j, nrows, ncols);
 		}
 
 		// Operators with scalars
 			return mData[0];
 		}
 
+		typedef val_type value_type;
+
 	private:
 		val_type mData[nrows * ncols];
 };
 
-template <unsigned int blockrows, unsigned int blockcols>
-inline std::ostream& operator<<(std::ostream& output, const Block<double, blockrows, blockcols> &block) {
+template <typename val_type ,unsigned int blockrows, unsigned int blockcols>
+inline std::ostream& operator<<(std::ostream& output, const Block<val_type, blockrows, blockcols> &block) {
 	output << std::endl;
 
 	unsigned int i,j;
 	return output;
 }
 
+/*
+template <typename val_type, unsigned int nrows, unsigned int ncols>
+inline val_type operator=(const Matrix<val_type, nrows, ncols> &matrix) {
+	COMPILE_ASSERT (nrows == 1);
+	COMPILE_ASSERT (ncols == 1);
+
+	return matrix[0];
+}
+*/
+
 template <typename val_type, unsigned int nrows, unsigned int ncols>
 inline Matrix<val_type, nrows, ncols> operator*(val_type scalar, const Matrix<val_type, nrows, ncols> &matrix) {
 	Matrix<val_type, nrows, ncols> result (matrix);
 		}
 
 		// access operators
-		const double& operator[](const unsigned int &index) const {
+		const val_type& operator[](const unsigned int &index) const {
 			assert (index	>= 0 && index < nrows * ncols);
 			return mData[index];
 		};
-		double& operator[](const unsigned int &index) {
+		val_type& operator[](const unsigned int &index) {
 			assert (index	>= 0 && index < nrows * ncols);
 			return mData[index];
 		}
 
-		const double& operator()(const unsigned int &row, const unsigned int &col) const {
+		const val_type& operator()(const unsigned int &row, const unsigned int &col) const {
 			assert (row	>= 0 && row < nrows && col >= 0 && col < ncols);
 			return mData[row*ncols + col];
 		};
-		double& operator()(const unsigned int &row, const unsigned int &col) {
+		val_type& operator()(const unsigned int &row, const unsigned int &col) {
 			assert (row	>= 0 && row < nrows && col >= 0 && col < ncols);
 			return mData[row*ncols + col];
 		};
 		Block<val_type, blockrows, blockcols> block (unsigned int i, unsigned int j) const {
 			assert (nrows > blockrows);
 			assert (ncols > blockcols);
-			return Block<val_type, blockrows, blockcols> (const_cast<double*> (this->mData), i, j, nrows, ncols);
+			return Block<val_type, blockrows, blockcols> (const_cast<val_type*> (this->mData), i, j, nrows, ncols);
 		}
 
 		// Operators with scalars
 			return mData[0];
 		}
 
+		typedef val_type value_type;
+
 	private:
 		unsigned int nrows;
 		unsigned int ncols;
 		val_type* mData;
 };
 
-template <unsigned int blockrows, unsigned int blockcols>
-inline std::ostream& operator<<(std::ostream& output, const Block<double, blockrows, blockcols> &block) {
+template <typename val_type ,unsigned int blockrows, unsigned int blockcols>
+inline std::ostream& operator<<(std::ostream& output, const Block<val_type, blockrows, blockcols> &block) {
 	output << std::endl;
 
 	unsigned int i,j;

File src/mathutils.cc

 }
 
 bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x) {
+#ifdef GINAC_MATH
+	assert (0 && !"function not supported with GiNaC");
+#else
 	x = VectorNd::Zero(x.size());
 
 	// We can only solve quadratic systems
 
 	// temporary result vector which contains the pivoted result
 	VectorNd px(x);
-	
+
 	unsigned int i,j,k;
 
 	for (i = 0; i < n; i++)
 
 	for (j = 0; j < n; j++) {
 		pi = j;
-		double pv = fabs (A(j,pivot[j]));
+		value_type pv = fabs (A(j,pivot[j]));
 
 		// LOG << "j = " << j << " pv = " << pv << std::endl;
 		// find the pivot
 		for (k = j; k < n; k++) {
-			double pt = fabs (A(j,pivot[k]));
+			value_type pt = fabs (A(j,pivot[k]));
 			if (pt > pv) {
 				pv = pt;
 				pi = k;
 
 	delete[] pivot;
 
+#endif
 	return true;
 }
 
 }
 
 bool SpatialMatrixCompareEpsilon (const SpatialMatrix &matrix_a, const SpatialMatrix &matrix_b, double epsilon) {
+#ifdef GINAC_MATH
+	assert (0 && !"Function not supported with GiNaC!");
+#else
 	assert (epsilon >= 0.);
 	unsigned int i, j;
 
 			}
 		}
 	}
-
+#endif
 	return true;
 }
 
 bool SpatialVectorCompareEpsilon (const SpatialVector &vector_a, const SpatialVector &vector_b, double epsilon) {
+#ifdef GINAC_MATH
+	assert (0 && !"Function not supported with GiNaC!");
+#else
 	assert (epsilon >= 0.);
 	unsigned int i;
 
 			return false;
 		}
 	}
-
+#endif
 	return true;
 }
 
 			);
 }
 
-SpatialMatrix Xrotx (const double &xrot) {
-	double s, c;
+SpatialMatrix Xrotx (const value_type &xrot) {
+	value_type s, c;
 	s = sin (xrot);
 	c = cos (xrot);
 
 			);
 }
 
-SpatialMatrix Xroty (const double &yrot) {
-	double s, c;
+SpatialMatrix Xroty (const value_type &yrot) {
+	value_type s, c;
 	s = sin (yrot);
 	c = cos (yrot);
 
 			);
 }
 
-SpatialMatrix Xrotz (const double &zrot) {
-	double s, c;
+SpatialMatrix Xrotz (const value_type &zrot) {
+	value_type s, c;
 	s = sin (zrot);
 	c = cos (zrot);
 

File src/mathutils.h

  *
  * \param zrot Rotation angle in radians.
  */
-SpatialAlgebra::SpatialMatrix Xrotz (const double &zrot);
+SpatialAlgebra::SpatialMatrix Xrotz (const value_type &zrot);
 
 /** \brief Creates a rotational transformation around the Y-axis
  *
  *
  * \param yrot Rotation angle in radians.
  */
-SpatialAlgebra::SpatialMatrix Xroty (const double &yrot);
+SpatialAlgebra::SpatialMatrix Xroty (const value_type &yrot);
 
 /** \brief Creates a rotational transformation around the X-axis
  *
  *
  * \param xrot Rotation angle in radians.
  */
-SpatialAlgebra::SpatialMatrix Xrotx (const double &xrot);
+SpatialAlgebra::SpatialMatrix Xrotx (const value_type &xrot);
 
 /** \brief Creates a spatial transformation for given parameters 
  *

File src/mathwrapper.h

 
 #include "rbdlconfig.h"
 
-#ifdef RBDL_USE_SIMPLE_MATH
+// #define GINAC_MATH
+
+#ifdef GINAC_MATH
+	#define RBDL_USE_SIMPLE_MATH
+	
+	#include "ginac.h"
   #include "SimpleMath.h"
 	#include <vector>
 
+	using namespace GiNaC;
+
+	typedef GiNaC::ex value_type;
+
+	typedef SimpleMath::Fixed::Matrix<value_type, 3,1> Vector3d;
+	typedef SimpleMath::Fixed::Matrix<value_type, 3,3> Matrix3d;
+
+	namespace SpatialAlgebra {
+		typedef SimpleMath::Fixed::Matrix<value_type, 6,1> SpatialVector;
+		typedef SimpleMath::Fixed::Matrix<value_type, 6,6> SpatialMatrix;
+	}
+
+	typedef SimpleMath::Dynamic::Matrix<value_type> MatrixNd;
+	typedef SimpleMath::Dynamic::Matrix<value_type> VectorNd;
+#elif defined RBDL_USE_SIMPLE_MATH
+  #include "SimpleMath.h"
+	#include <vector>
+
+	typedef double value_type;
+
 	typedef SimpleMath::Fixed::Matrix<double, 3,1> Vector3d;
 	typedef SimpleMath::Fixed::Matrix<double, 3,3> Matrix3d;
 
 
 	typedef SimpleMath::Dynamic::Matrix<double> MatrixNd;
 	typedef SimpleMath::Dynamic::Matrix<double> VectorNd;
-
 #else
 	#define EIGEN_DEFAULT_TO_ROW_MAJOR
 	#define EIGEN_MATRIX_PLUGIN "MatrixAddons.h"
 	#include "Eigen/Dense"
 	#include "Eigen/StdVector"
 
+	typedef double value_type;
+
 	typedef Eigen::Matrix< double, 3, 1> Vector3d;
 	typedef Eigen::Matrix< double, 3, 3> Matrix3d;