Commits

Martin Felis committed 4379e5c Merge

merge default->dev

  • Participants
  • Parent commits 8b8ae9c, 90fac45
  • Branches dev

Comments (0)

Files changed (7)

 RBDL - Rigid Body Dynamics Library
-Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+Copyright (c) 2011-2013 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
 
 Introduction
 ============
 
 Recent Changes
 ==============
-
-   * 29 January 2013: added code for api_version_checking
+   * 20 February 2013: removed too specialized RigidBodyDynamics::Body constructor (API version 1.1.0)
+   * 29 January 2013: added code for api_version_checking. Current API version is 1.0.0.
    * 11 January 2013: removed Eigen3 sources and relying on an already installed Eigen3 library. Optionally RBDL can be used with the included but slower SimpleMath library.
    * 18 June 2012: added support of luamodel_introduction
    * 01 June 2012: added support of joint_models_fixed
  *
  * \section recent_changes Recent Changes :
  * <ul>
- * <li> 29. January 2013: added code for \ref api_version_checking</li>
- * <li> 11. January 2013: removed Eigen3 sources and relying on an already installed Eigen3 library. Optionally RBDL can be used with the included but slower SimpleMath library.</li>
+ * <li> 20. February 2013: removed too specialized RigidBodyDynamics::Body constructor (API version 1.1.0)</li>
+ * <li> 29. January 2013: added code for \ref api_version_checking. Current is 1.0.0.</li>
  * <li> 18. June 2012: added support of \ref luamodel_introduction</li>
  * <li> 01. June 2012: added support of \ref joint_models_fixed</li>
  * <li> 14. May 2012: fixed Body constructor as reported by Maxime Reis</li>

include/rbdl/Body.h

 	/** \brief Constructs a body from mass, center of mass and radii of gyration 
 	 * 
 	 * This constructor eases the construction of a new body as all the
-	 * required parameters can simply be specified as parameters to the
+	 * required parameters can be specified as parameters to the
 	 * constructor. These are then used to generate the spatial inertia
 	 * matrix which is expressed at the origin.
 	 * 
 			Math::Matrix3d parallel_axis;
 			parallel_axis = mass * com_cross * com_cross.transpose();
 
-			Math::Vector3d gr (gyration_radii);
+			mInertia = Math::Matrix3d (
+					gyration_radii[0], 0., 0.,
+					0., gyration_radii[1], 0.,
+					0., 0., gyration_radii[2]
+					);
+
 			Math::Matrix3d pa (parallel_axis);
 			Math::Matrix3d mcc = mass * com_cross;
 			Math::Matrix3d mccT = mcc.transpose();
 
+			Math::Matrix3d inertia_O = mInertia + pa;
+
 			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),
-					pa(2, 0), pa(2, 1), gr[2] + pa(2, 2), mcc(2, 0), mcc(2, 1), mcc(2, 2),
+					inertia_O(0,0), inertia_O(0,1), inertia_O(0,2), mcc(0, 0), mcc(0, 1), mcc(0, 2),
+					inertia_O(1,0), inertia_O(1,1), inertia_O(1,2), mcc(1, 0), mcc(1, 1), mcc(1, 2),
+					inertia_O(2,0), inertia_O(2,1), inertia_O(2,2), mcc(2, 0), mcc(2, 1), mcc(2, 2),
 					mccT(0, 0), mccT(0, 1), mccT(0, 2), mass, 0., 0.,
 					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 
 					mccT(2, 0), mccT(2, 1), mccT(2, 2), 0., 0., mass
 					);
 		}
-
-	/** \brief Constructs a body out of the given parameters
-	 * 
-	 * This constructor eases the construction of a new body as all the
-	 * required parameters can simply be specified as parameters to the
-	 * constructor. These are then used to generate the spatial inertia
-	 * matrix which is expressed at the origin.
-	 * 
-	 * \param mass the mass of the body
-	 * \param com  the position of the center of mass in the bodies coordinates
-	 * \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,
-			const Math::Vector3d &com,
-			const double &length,
-			const Math::Vector3d &gyration_radii) :
-		mMass (mass),
-		mCenterOfMass(com) {
-			Math::Matrix3d com_cross (
-					0., -com[2],  com[1],
-					com[2],      0., -com[0],
-					-com[1],  com[0],      0.
-					);
-			Math::Matrix3d parallel_axis;
-			parallel_axis = mass * com_cross * com_cross.transpose();
-
-			LOG << "parrallel axis = " << parallel_axis << std::endl;
-
-			Math::Vector3d gr = mass * Math::Vector3d(
-					gyration_radii[0] * gyration_radii[0] * length * length,
-					gyration_radii[1] * gyration_radii[1] * length * length,
-					gyration_radii[2] * gyration_radii[2] * length * length
-					);
-			Math::Matrix3d pa (parallel_axis);
-			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),
-					pa(2, 0), pa(2, 1), gr[2] + pa(2, 2), mcc(2, 0), mcc(2, 1), mcc(2, 2),
-					mccT(0, 0), mccT(0, 1), mccT(0, 2), mass, 0., 0.,
-					mccT(1, 0), mccT(1, 1), mccT(1, 2), 0., mass, 0.,
-					mccT(2, 0), mccT(2, 1), mccT(2, 2), 0., 0., mass
-					);
-
-			LOG << "spatial inertia = " << mSpatialInertia << std::endl;
-		}
-		
+	
 	/** \brief Joins inertial parameters of two bodies to create a composite
 	 * body.
 	 *

include/rbdl/SpatialAlgebraOperators.h

 		return result;
 	}
 
+	/// Mass
 	double m;
+	/// Coordinates of the center of mass
 	Vector3d h;
+	/// Inertia expressed at the origin
 	Matrix3d I;
 };
 

include/rbdl/rbdl_config.h.cmake

File contents unchanged.

tests/BodyTests.cc

 TEST ( TestComputeSpatialInertiaFromAbsoluteRadiiGyration ) {
 	Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.));
 
+	Matrix3d inertia_C (
+			1.4, 0., 0.,
+			0., 2., 0., 
+			0., 0., 3.);
+
 	SpatialMatrix reference_inertia (
 			4.843, -1.98, -2.145, 0, -1.43, 1.32,
 			-1.98, 6.334, -1.716, 1.43, 0, -1.65,
 
 //	cout << LogOutput.str() << endl;
 
+	Body com_body(1.1, Vector3d (0., 0., 0.), Vector3d (1.4, 2., 3.));
+
 	CHECK_ARRAY_CLOSE (reference_inertia.data(), body.mSpatialInertia.data(), 36, TEST_PREC);
+	CHECK_ARRAY_CLOSE (inertia_C.data(), body.mInertia.data(), 9, TEST_PREC);
 }
 
 TEST ( TestBodyConstructorMassComInertia ) {
 			);
 
 	CHECK_ARRAY_CLOSE (reference_inertia.data(), body.mSpatialInertia.data(), 36, TEST_PREC);
+	CHECK_ARRAY_CLOSE (inertia_C.data(), body.mInertia.data(), 9, TEST_PREC);
 }
 
 TEST ( TestBodyJoinNullbody ) {
 	SpatialRigidBodyInertia rbi = SpatialRigidBodyInertia(
 				body.mMass,
 				body.mCenterOfMass,
-				body.mInertia
+				body.mSpatialInertia.block<3,3>(0,0)
 				);
 
 	SpatialVector mv (1.1, 1.2, 1.3, 1.4, 1.5, 1.6);
 	SpatialRigidBodyInertia rbi = SpatialRigidBodyInertia(
 				body.mMass,
 				body.mCenterOfMass,
-				body.mInertia
+				body.mSpatialInertia.block<3,3>(0,0)
 				);
 
 //	cout << "Spatial Inertia = " << endl << spatial_inertia << endl;

tests/DynamicsTests.cc

 
 	model.gravity = Vector3d (0., -9.81, 0.);
 
-	Body null_body (0., Vector3d(0., 0., 0.), 1., Vector3d (0., 0., 0.));
-	Body base_body (1., Vector3d(0., 0.5, 0.), 1., Vector3d (1., 1., 1.));
+	Body null_body (0., Vector3d(0., 0., 0.), Vector3d (0., 0., 0.));
+	Body base_body (1., Vector3d(0., 0.5, 0.), Vector3d (1., 1., 1.));
 
 	Joint joint_rot_z (JointTypeRevolute, Vector3d (0., 0., 1.));
 	Joint joint_rot_y (JointTypeRevolute, Vector3d (0., 1., 0.));
 
 	model.gravity = Vector3d (0., -9.81, 0.);
 
-	Body null_body (0., Vector3d(0., 0., 0.), 1., Vector3d (0., 0., 0.));
-	Body base_body (1., Vector3d(0., 0.5, 0.), 1., Vector3d (1., 1., 1.));
+	Body null_body (0., Vector3d(0., 0., 0.), Vector3d (0., 0., 0.));
+	Body base_body (1., Vector3d(0., 0.5, 0.), Vector3d (1., 1., 1.));
 
 	Joint joint_rot_z (JointTypeRevolute, Vector3d (0., 0., 1.));
 	Joint joint_rot_y (JointTypeRevolute, Vector3d (0., 1., 0.));