Commits

Martin Felis committed 9e8cf31 Merge

merged compactrbi into default

Comments (0)

Files changed (13)

examples/simple/example.cc

File contents unchanged.
 	Body() :
 		mMass (1.),
 		mCenterOfMass (0., 0., 0.),
-		mSpatialInertia (
-				0., 0., 0., 0., 0., 0.,	
-				0., 0., 0., 0., 0., 0.,	
-				0., 0., 0., 0., 0., 0.,	
-				0., 0., 0., 0., 0., 0.,	
-				0., 0., 0., 0., 0., 0.,	
-				0., 0., 0., 0., 0., 0.
-				)
+		mInertia (Math::Matrix3d::Zero(3,3)),
+		mSpatialInertia (Math::SpatialMatrix::Zero(6,6))
 	{ };
 	Body(const Body &body) :
 		mMass (body.mMass),
 		mCenterOfMass (body.mCenterOfMass),
+		mInertia (body.mInertia),
 		mSpatialInertia (body.mSpatialInertia)
 	{};
 	Body& operator= (const Body &body) {
 		if (this != &body) {
+			mMass = body.mMass;
+			mInertia = body.mInertia;
+			mCenterOfMass = body.mCenterOfMass;
 			mSpatialInertia = body.mSpatialInertia;
-			mCenterOfMass = body.mCenterOfMass;
-			mMass = body.mMass;
 		}
 
 		return *this;
 					mccT(1, 0), mccT(1, 1), mccT(1, 2), 0., mass, 0.,
 					mccT(2, 0), mccT(2, 1), mccT(2, 2), 0., 0., mass
 					);
+
+			mInertia = mSpatialInertia.block<3,3>(0,0);
 		}
 
 	/** \brief Constructs a body from mass, center of mass, and a 3x3 inertia matrix 
 			const Math::Vector3d &com,
 			const Math::Matrix3d &inertia_C) :
 		mMass (mass),
+		mInertia (inertia_C),
 		mCenterOfMass(com) {
 			Math::Matrix3d com_cross (
 					0., -com[2],  com[1],
 			Math::Matrix3d mcc = mass * com_cross;
 			Math::Matrix3d mccT = mcc.transpose();
 
+			mInertia.set (
+					gr[0], 0., 0.,
+					0., gr[1], 0.,
+					0., 0., gr[2]
+					);
+
 			mSpatialInertia.set (
 					gr[0] + pa(0, 0), pa(0, 1), pa(0, 2), mcc(0, 0), mcc(0, 1), mcc(0, 2),
 					pa(1, 0), gr[1] + pa(1, 1), pa(1, 2), mcc(1, 0), mcc(1, 1), mcc(1, 2),
 	double mMass;
 	/// \brief The position of the center of mass in body coordinates
 	Math::Vector3d mCenterOfMass;
+	/// \brief Inertia matrix at the center of mass
+	Math::Matrix3d mInertia;
 	/// \brief The spatial inertia that contains both mass and inertia information
 	Math::SpatialMatrix mSpatialInertia;
 };
 	H.setZero();
 
 	unsigned int i;
+	unsigned int dof_i = model.dof_count;
+
 	for (i = 1; i < model.mBodies.size(); i++) {
-		model.Ic[i] = model.mBodies[i].mSpatialInertia;
+		model.Ic[i].createFromMatrix(model.mBodies[i].mSpatialInertia);
 	}
 
-	LOG << "-- initialization --" << std::endl;
-	for (i = 0; i < model.mBodies.size(); i++) {
-		LOG << "Ic[" << i << "] = " << std::endl << model.Ic[i] << std::endl;
-	}
-	unsigned int dof_i = model.dof_count;
-
 	for (i = model.mBodies.size() - 1; i > 0; i--) {
 		unsigned int lambda = model.lambda[i];
 
 		if (lambda != 0) {
-			model.Ic[lambda] = model.Ic[lambda] + model.X_lambda[i].toMatrixTranspose() * model.Ic[i] * model.X_lambda[i].toMatrix();
+			model.Ic[lambda] = model.Ic[lambda] + model.X_lambda[i].apply(model.Ic[i]);
 		}
 
 		dof_i = i - 1;
 
 			dof_j = j - 1;
 
-			LOG << "i,j         = " << i << ", " << j << std::endl;
-			LOG << "dof_i,dof_j = " << dof_i << ", " << dof_j << std::endl;
 			H(dof_i,dof_j) = F.dot(model.S[j]);
 			H(dof_j,dof_i) = H(dof_i,dof_j);
 		}
 	d = VectorNd::Zero(1);
 
 	f.push_back (zero_spatial);
-	Ic.push_back (SpatialMatrixIdentity);
+	Ic.push_back (
+			SpatialRigidBodyInertia(
+				0.,
+				Vector3d (0., 0., 0.),
+				Matrix3d::Zero(3,3)
+				)
+			);
 
 	// Bodies
 	X_lambda.push_back(SpatialTransform());
 	u = VectorNd::Zero (mBodies.size());
 
 	f.push_back (SpatialVector (0., 0., 0., 0., 0., 0.));
-	Ic.push_back (SpatialMatrixIdentity);
+	Ic.push_back (
+			SpatialRigidBodyInertia(
+				body.mMass,
+				body.mCenterOfMass,
+				body.mInertia
+				)
+			);
 
 	if (mBodies.size() == fixed_body_discriminator) {
 		std::cerr << "Error: cannot add more than " << fixed_body_discriminator << " movable bodies. You need to modify Model::fixed_body_discriminator for this." << std::endl;
  * \link RigidBodyDynamics::Model::GetBodyId Model::GetBodyId(...)\endlink,
  * are used to initialize and construct the \ref model_structure.
  *
+ * \section model_construction Model Construction
+ *
  * The construction of \link RigidBodyDynamics::Model Models \endlink makes
  * use of carefully designed constructors of the classes \link
  * RigidBodyDynamics::Body Body \endlink and \link RigidBodyDynamics::Joint
 	Math::VectorNd u;
 	/// \brief Internal forces on the body (used only InverseDynamics())
 	std::vector<Math::SpatialVector> f;
-	/// \brief The spatial inertia of the bodies (used only in CompositeRigidBodyAlgorithm())
-	std::vector<Math::SpatialMatrix> Ic;
+	/// \brief The spatial inertia of body i (used only in CompositeRigidBodyAlgorithm())
+	std::vector<Math::SpatialRigidBodyInertia> Ic;
 
 	////////////////////////////////////
 	// Bodies

src/SpatialAlgebraOperators.h

 namespace RigidBodyDynamics {
 
 namespace Math {
+
 /** \brief Spatial algebra matrices, vectors, and operators. */
 
+struct SpatialRigidBodyInertia {
+	SpatialRigidBodyInertia() :
+		m (0.),
+		h (Vector3d::Zero(3,1)),
+		I (Matrix3d::Zero(3,3))
+	{}
+	SpatialRigidBodyInertia (
+			double mass, const Vector3d &com, const Matrix3d &inertia) : 
+		m (mass), h (com * mass), I (inertia)
+	{ }
+
+	inline Matrix3d VectorCrossMatrix (const Vector3d &vector) {
+		return Matrix3d (
+				0., -vector[2], vector[1],
+				vector[2], 0., -vector[0],
+				-vector[1], vector[0], 0.
+				);
+	}
+
+	SpatialVector operator* (const SpatialVector &mv) {
+		Vector3d mv_upper (mv[0], mv[1], mv[2]);
+		Vector3d mv_lower (mv[3], mv[4], mv[5]);
+
+		Vector3d res_upper = I * mv_upper + Vector3d (
+				mv_lower[2] * h[1] - mv_lower[1] * h[2],
+				mv_lower[0] * h[2] - mv_lower[2] * h[0],
+				mv_lower[1] * h[0] - mv_lower[0] * h[1]
+				);
+		Vector3d res_lower = m* mv_lower - Vector3d(
+				mv_upper[2] * h[1] - mv_upper[1] * h[2],
+				mv_upper[0] * h[2] - mv_upper[2] * h[0],
+				mv_upper[1] * h[0] - mv_upper[0] * h[1]
+				);
+
+		return SpatialVector (
+				res_upper[0], res_upper[1], res_upper[2],
+				res_lower[0], res_lower[1], res_lower[2]
+				);
+	}
+
+	SpatialRigidBodyInertia operator+ (const SpatialRigidBodyInertia &rbi) {
+		return SpatialRigidBodyInertia (
+				m + rbi.m,
+				(h + rbi.h) / (m + rbi.m),
+				I + rbi.I
+				);
+	}
+
+	void createFromMatrix (const SpatialMatrix &Ic) {
+		m = Ic(3,3);
+		h.set (-Ic(1,5), Ic(0,5), -Ic(0,4));
+		I = Ic.block<3,3>(0,0);
+	}
+
+	SpatialMatrix toMatrix() {
+		SpatialMatrix result;
+		result.block<3,3>(0,0) = I;
+		result.block<3,3>(0,3) = VectorCrossMatrix(h);
+		result.block<3,3>(3,0) = - VectorCrossMatrix(h);
+		result.block<3,3>(3,3) = Matrix3d::Identity(3,3) * m;
+
+		return result;
+	}
+
+	Vector3d h;
+	Matrix3d I;
+	double m;
+};
+
 /** \brief Compact representation of spatial transformations.
  *
  * Instead of using a verbose 6x6 matrix, this structure only stores a 3x3
 				);
 	}
 
+	inline Matrix3d VectorCrossMatrix (const Vector3d &vector) {
+		return Matrix3d (
+				0., -vector[2], vector[1],
+				vector[2], 0., -vector[0],
+				-vector[1], vector[0], 0.
+				);
+	}
 	/** Same as X^T * f.
 	 *
 	 * \returns (E^T * n + rx * E^T * f, E^T * f)
 				);
 	}
 
+	SpatialRigidBodyInertia apply (const SpatialRigidBodyInertia &rbi) {
+		return SpatialRigidBodyInertia (
+				rbi.m,
+				E.transpose() * (rbi.h / rbi.m) + r,
+				E.transpose() * rbi.I * E
+				- VectorCrossMatrix (r) * VectorCrossMatrix(E.transpose() * rbi.h)
+				- VectorCrossMatrix (E.transpose() * (rbi.h) + r * rbi.m) * VectorCrossMatrix (r)
+				);
+	}
+
 	SpatialMatrix toMatrix () const {
 		Matrix3d _Erx =
 			E * Matrix3d (
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialVector)
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialMatrix)
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialTransform)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialRigidBodyInertia)
 #endif
 
 #endif /* _MATHWRAPPER_H */

src/rbdl_mathutils.cc

 	return true;
 }
 
-Matrix3d VectorCrossMatrix (const Vector3d &vector) {
-	return Matrix3d (
-			0., -vector[2], vector[1],
-			vector[2], 0., -vector[0],
-			-vector[1], vector[0], 0.
-			);
-}
-
 void SpatialMatrixSetSubmatrix(SpatialMatrix &dest, unsigned int row, unsigned int col, const Matrix3d &matrix) {
 	assert (row < 2 && col < 2);
 	

src/rbdl_mathutils.h

 bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x);
 
 /// \brief Creates the skew symmetric matrix of the cross product of a given 3D vector
-Matrix3d VectorCrossMatrix (const Vector3d &vector);
+inline Matrix3d VectorCrossMatrix (const Vector3d &vector) {
+	return Matrix3d (
+			0., -vector[2], vector[1],
+			vector[2], 0., -vector[0],
+			-vector[1], vector[0], 0.
+			);
+}
+
 // \todo write test 
 void SpatialMatrixSetSubmatrix(SpatialMatrix &dest, unsigned int row, unsigned int col, const Matrix3d &matrix);
 

tests/BodyTests.cc

 	CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), body_joined.mCenterOfMass.data(), 3, TEST_PREC);
 	CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined.mSpatialInertia.data(), 36, TEST_PREC);
 }
+
+TEST ( TestBodyConstructorSpatialRigidBodyInertiaMultiplyMotion ) {
+	Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.));
+
+	SpatialMatrix spatial_inertia = body.mSpatialInertia;
+	SpatialRigidBodyInertia rbi = SpatialRigidBodyInertia(
+				body.mMass,
+				body.mCenterOfMass,
+				body.mInertia
+				);
+
+	SpatialVector mv (1.1, 1.2, 1.3, 1.4, 1.5, 1.6);
+	SpatialVector fv_matrix = spatial_inertia * mv;
+	SpatialVector fv_rbi = rbi * mv;
+
+	CHECK_ARRAY_CLOSE (
+			fv_matrix.data(),
+			fv_rbi.data(),
+			6,
+			TEST_PREC
+			);
+}
+
+TEST ( TestBodyConstructorSpatialRigidBodyInertia ) {
+	Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.));
+
+	SpatialMatrix spatial_inertia = body.mSpatialInertia;
+	SpatialRigidBodyInertia rbi = SpatialRigidBodyInertia(
+				body.mMass,
+				body.mCenterOfMass,
+				body.mInertia
+				);
+
+//	cout << "Spatial Inertia = " << endl << spatial_inertia << endl;
+//	cout << "rbi = " << endl << rbi.toMatrix() << endl;
+//	cout << "rbi.m = " << rbi.m << endl;
+//	cout << "rbi.h = " << rbi.h.transpose() << endl;
+//	cout << "rbi.I = " << endl << rbi.I << endl;
+
+	CHECK_ARRAY_CLOSE (
+			spatial_inertia.data(),
+			rbi.toMatrix().data(),
+			36,
+			TEST_PREC
+			);
+}
+
+TEST ( TestBodyConstructorCopySpatialRigidBodyInertia ) {
+	Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.));
+
+	SpatialMatrix spatial_inertia = body.mSpatialInertia;
+
+	SpatialRigidBodyInertia rbi;
+	rbi.createFromMatrix (spatial_inertia);
+
+//	cout << "Spatial Inertia = " << endl << spatial_inertia << endl;
+//	cout << "rbi = " << endl << rbi.toMatrix() << endl;
+//	cout << "rbi.m = " << rbi.m << endl;
+//	cout << "rbi.h = " << rbi.h.transpose() << endl;
+//	cout << "rbi.I = " << endl << rbi.I << endl;
+
+	CHECK_ARRAY_CLOSE (
+			spatial_inertia.data(),
+			rbi.toMatrix().data(),
+			36,
+			TEST_PREC
+			);
+}

tests/CMakeLists.txt

File contents unchanged.

tests/CompositeRigidBodyTests.cc

 	Tau[5] = 1.3;
 
 	ForwardDynamics(*model, Q, QDot, Tau, QDDot);
+
+	ClearLogOutput();
 	CompositeRigidBodyAlgorithm (*model, Q, H);
+	// cout << LogOutput.str() << endl;
+
 	InverseDynamics (*model, Q, QDot, QDDot_zero, C);
 
 	CHECK (LinSolveGaussElimPivot (H, C * -1. + Tau, QDDot_crba));
 	Tau[11] = -0.3;
 
 	ForwardDynamics(*model, Q, QDot, Tau, QDDot);
+	ClearLogOutput();
 	CompositeRigidBodyAlgorithm (*model, Q, H);
+	// cout << LogOutput.str() << endl;
 	InverseDynamics (*model, Q, QDot, QDDot_zero, C);
 
 	CHECK (LinSolveGaussElimPivot (H, C * -1. + Tau, QDDot_crba));

tests/SpatialAlgebraTests.cc

 #include <UnitTest++.h>
 
 #include <iostream>
+#include <iomanip>
 
+#include "Body.h"
 #include "rbdl_math.h"
 #include "rbdl_mathutils.h"
 
 using namespace std;
+using namespace RigidBodyDynamics;
 using namespace RigidBodyDynamics::Math;
 
 const double TEST_PREC = 1.0e-14;
 	SpatialTransform X_rotX = Xrotx (M_PI * 0.15);
 	SpatialTransform X_rotX_axis = Xrot (M_PI * 0.15, Vector3d (1., 0., 0.));
 
-	CHECK_ARRAY_CLOSE (X_rotX.toMatrix().data(), X_rotX_axis.toMatrix().data(), 16, TEST_PREC);
+	CHECK_ARRAY_CLOSE (X_rotX.toMatrix().data(), X_rotX_axis.toMatrix().data(), 36, TEST_PREC);
 
 	// all the other axes
 	SpatialTransform X_rotX_90 = Xrotx (M_PI * 0.5);
 	SpatialTransform X_rotX_90_axis = Xrot (M_PI * 0.5, Vector3d (1., 0., 0.));
 
-	CHECK_ARRAY_CLOSE (X_rotX_90.toMatrix().data(), X_rotX_90_axis.toMatrix().data(), 16, TEST_PREC);
+	CHECK_ARRAY_CLOSE (X_rotX_90.toMatrix().data(), X_rotX_90_axis.toMatrix().data(), 36, TEST_PREC);
 
 	SpatialTransform X_rotY_90 = Xroty (M_PI * 0.5);
 	SpatialTransform X_rotY_90_axis = Xrot (M_PI * 0.5, Vector3d (0., 1., 0.));
 
-	CHECK_ARRAY_CLOSE (X_rotY_90.toMatrix().data(), X_rotY_90_axis.toMatrix().data(), 16, TEST_PREC);
+	CHECK_ARRAY_CLOSE (X_rotY_90.toMatrix().data(), X_rotY_90_axis.toMatrix().data(), 36, TEST_PREC);
 
 	SpatialTransform X_rotZ_90 = Xrotz (M_PI * 0.5);
 	SpatialTransform X_rotZ_90_axis = Xrot (M_PI * 0.5, Vector3d (0., 0., 1.));
 
-	CHECK_ARRAY_CLOSE (X_rotZ_90.toMatrix().data(), X_rotZ_90_axis.toMatrix().data(), 16, TEST_PREC);
+	CHECK_ARRAY_CLOSE (X_rotZ_90.toMatrix().data(), X_rotZ_90_axis.toMatrix().data(), 36, TEST_PREC);
+}
 
+TEST(TestSpatialTransformApplySpatialRigidBodyInertiaAdd) {
+	SpatialRigidBodyInertia rbi (
+			1.1,
+			Vector3d (1.2, 1.3, 1.4),
+			Matrix3d (
+				1.1, 0.5, 0.3,
+				0.5, 1.2, 0.4,
+				0.3, 0.4, 1.3
+				));
+
+	SpatialMatrix rbi_matrix_added = rbi.toMatrix() + rbi.toMatrix();
+	SpatialRigidBodyInertia rbi_added = rbi + rbi;
+
+	// cout << "rbi = " << endl << rbi.toMatrix() << endl;
+	// cout << "rbi_added = " << endl << rbi_added.toMatrix() << endl;
+	// cout << "rbi_matrix_added = " << endl << rbi_matrix_added << endl;
+	// cout << "diff = " << endl << 
+	//  	rbi_added.toMatrix() - rbi_matrix_added << endl;
+
+	CHECK_ARRAY_CLOSE (
+			rbi_matrix_added.data(),
+			rbi_added.toMatrix().data(),
+			36,
+			TEST_PREC
+			);
+}
+
+TEST(TestSpatialTransformApplySpatialRigidBodyInertiaFull) {
+	SpatialRigidBodyInertia rbi (
+			1.1,
+			Vector3d (1.2, 1.3, 1.4),
+			Matrix3d (
+				1.1, 0.5, 0.3,
+				0.5, 1.2, 0.4,
+				0.3, 0.4, 1.3
+				));
+
+	SpatialTransform X (
+			Xrotz (0.5) *
+			Xroty (0.9) *
+			Xrotx (0.2) *
+			Xtrans (Vector3d (1.1, 1.2, 1.3))
+		);
+
+	SpatialRigidBodyInertia rbi_transformed = X.apply (rbi);
+	SpatialMatrix rbi_matrix_transformed = X.toMatrixTranspose() * rbi.toMatrix() * X.toMatrix();
+
+	// cout << "rbi = " << endl << rbi.toMatrix() << endl;
+	// cout << "rbi_transformed = " << endl << rbi_transformed.toMatrix() << endl;
+	// cout << "rbi_matrix_transformed = " << endl << rbi_matrix_transformed << endl;
+	// cout << "diff = " << endl << 
+	// 	rbi_transformed.toMatrix() - rbi_matrix_transformed << endl;
+
+	CHECK_ARRAY_CLOSE (
+			rbi_matrix_transformed.data(),
+			rbi_transformed.toMatrix().data(),
+			36,
+			TEST_PREC
+			);
+}
+
+TEST(TestSpatialRigidBodyInertiaCreateFromMatrix) {
+	double mass = 1.1;
+	Vector3d com (0., 0., 0.);
+	Matrix3d inertia (
+				1.1, 0.5, 0.3,
+				0.5, 1.2, 0.4,
+				0.3, 0.4, 1.3
+			);
+	Body body(mass, com , inertia);
+
+	SpatialMatrix spatial_inertia = body.mSpatialInertia;
+
+	SpatialRigidBodyInertia rbi;
+	rbi.createFromMatrix (spatial_inertia);
+
+	CHECK_EQUAL (mass, rbi.m);
+	CHECK_ARRAY_EQUAL (Vector3d(mass * com).data(), rbi.h.data(), 3);
+	CHECK_ARRAY_EQUAL (inertia.data(), rbi.I.data(), 9);
 }
 
 #ifdef USE_SLOW_SPATIAL_ALGEBRA