# Commits

committed 551297c Draft

some progress

• Participants
• Parent commits 4acd9c7
• Branches ginac

# File src/Body.h

` 	 * \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);`

# File src/Joint.h

` 		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;`
` `