1. Eric Cousineau
  2. rbdl-backup

Commits

Martin Felis  committed b58f220

found and fixed error in Base <-> Body coordinate transformations

  • Participants
  • Parent commits 863760e
  • Branches dev

Comments (0)

Files changed (4)

File src/Kinematics.cc

View file
 
 	if (body_id >= model.fixed_body_discriminator) {
 		unsigned int fbody_id = body_id - model.fixed_body_discriminator;
-		model.mFixedBodies[fbody_id].mBaseTransform = model.X_base[model.mFixedBodies[fbody_id].mMovableParent] * model.mFixedBodies[fbody_id].mParentTransform;
+		unsigned int parent_id = model.mFixedBodies[fbody_id].mMovableParent;
 
-		Matrix3d body_rotation = model.mFixedBodies[fbody_id].mBaseTransform.E.transpose();
-		Vector3d body_position = model.mFixedBodies[fbody_id].mBaseTransform.r;
+		Matrix3d fixed_rotation = model.mFixedBodies[fbody_id].mParentTransform.E.transpose();
+		Vector3d fixed_position = model.mFixedBodies[fbody_id].mParentTransform.r;
 
-		return body_position + body_rotation * point_body_coordinates;
+		Matrix3d parent_body_rotation = model.X_base[parent_id].E.transpose();
+		Vector3d parent_body_position = model.X_base[parent_id].r;
+
+		return parent_body_position + parent_body_rotation * (fixed_position + fixed_rotation * (point_body_coordinates));
 	}
 
 	Matrix3d body_rotation = model.X_base[body_id].E.transpose();
 		unsigned int body_id,
 		const Vector3d &point_base_coordinates,
 		bool update_kinematics) {
-	// update the Kinematics if necessary
 	if (update_kinematics) {
 		UpdateKinematicsCustom (model, &Q, NULL, NULL);
 	}
 
 	if (body_id >= model.fixed_body_discriminator) {
 		unsigned int fbody_id = body_id - model.fixed_body_discriminator;
-		model.mFixedBodies[fbody_id].mBaseTransform = model.X_base[model.mFixedBodies[fbody_id].mMovableParent] * model.mFixedBodies[fbody_id].mParentTransform;
+		unsigned int parent_id = model.mFixedBodies[fbody_id].mMovableParent;
 
-		Matrix3d body_rotation = model.mFixedBodies[fbody_id].mBaseTransform.E.transpose();
-		Vector3d body_position = model.mFixedBodies[fbody_id].mBaseTransform.r;
+		Matrix3d fixed_rotation = model.mFixedBodies[fbody_id].mParentTransform.E;
+		Vector3d fixed_position = model.mFixedBodies[fbody_id].mParentTransform.r;
 
-		return body_rotation * point_base_coordinates - body_rotation * body_position;
+		Matrix3d parent_body_rotation = model.X_base[parent_id].E;
+		Vector3d parent_body_position = model.X_base[parent_id].r;
+
+		return fixed_rotation * ( - fixed_position - parent_body_rotation * (parent_body_position - point_base_coordinates));
 	}
 
 	Matrix3d body_rotation = model.X_base[body_id].E;
 	Vector3d body_position = model.X_base[body_id].r;
 
-	return body_rotation * point_base_coordinates - body_rotation * body_position;
+	return body_rotation * (point_base_coordinates - body_position);
 }
 
 Matrix3d CalcBodyWorldOrientation (
 		bool update_kinematics
 	) {
 	LOG << "-------- " << __func__ << " --------" << std::endl;
-	unsigned int i;
-
-	assert (body_id > 0 && body_id < model.mBodies.size());
+	assert (model.IsBodyId(body_id));
 	assert (model.mBodies.size() == Q.size() + 1);
 	assert (model.mBodies.size() == QDot.size() + 1);
 
 
 	Vector3d point_abs_pos = CalcBodyToBaseCoordinates (model, Q, body_id, point_position, false); 
 
+	unsigned int reference_body_id = body_id;
+
+	if (model.IsFixedBodyId(body_id)) {
+		unsigned int fbody_id = body_id - model.fixed_body_discriminator;
+		reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
+	}
+
 	LOG << "body_index     = " << body_id << std::endl;
-	LOG << "point_pos      = " << point_position << std::endl;
+	LOG << "point_pos      = " << point_position.transpose() << std::endl;
 //	LOG << "global_velo    = " << global_velocities.at(body_id) << std::endl;
-	LOG << "body_transf    = " << model.X_base[body_id].toMatrix() << std::endl;
-	LOG << "point_abs_ps   = " << point_abs_pos << std::endl;
-	LOG << "X   = " << Xtrans_mat (point_abs_pos) * spatial_inverse(model.X_base[body_id].toMatrix()) << std::endl;
-	LOG << "v   = " << model.v[body_id] << std::endl;
+	LOG << "body_transf    = " << std::endl << model.X_base[body_id].toMatrix() << std::endl;
+	LOG << "point_abs_ps   = " << point_abs_pos.transpose() << std::endl;
+	LOG << "X   = " << std::endl << Xtrans_mat (point_abs_pos) * spatial_inverse(model.X_base[reference_body_id].toMatrix()) << std::endl;
+	LOG << "v   = " << model.v[reference_body_id].transpose() << std::endl;
 
 	// Now we can compute the spatial velocity at the given point
 //	SpatialVector body_global_velocity (global_velocities.at(body_id));
-	SpatialVector point_spatial_velocity = Xtrans_mat (point_abs_pos) * spatial_inverse(model.X_base[body_id].toMatrix()) * model.v[body_id];
+	SpatialVector point_spatial_velocity = Xtrans_mat (point_abs_pos) * spatial_inverse(model.X_base[reference_body_id].toMatrix()) * model.v[reference_body_id];
 
 	LOG << "point_velocity = " <<	Vector3d (
 			point_spatial_velocity[3],

File src/Model.h

View file
 	 */
 	unsigned int GetBodyId (const char *id) const;
 
+
+	bool IsFixedBodyId (unsigned int body_id) {
+		if (body_id >= fixed_body_discriminator 
+				&& body_id < std::numeric_limits<unsigned int>::max() 
+				&& body_id - fixed_body_discriminator < mFixedBodies.size()) {
+			return true;
+		}
+		return false;
+	}
+
+	bool IsBodyId (unsigned int id) {
+		if (id > 0 && id < mBodies.size())
+			return true;
+		if (id >= fixed_body_discriminator && id < std::numeric_limits<unsigned int>::max()) {
+			if (id - fixed_body_discriminator < mFixedBodies.size())
+				return true;
+		}
+		return false;
+	}
 	/// \brief Initializes the helper values for the dynamics algorithm
 	void Init ();
 };

File tests/CalcVelocitiesTests.cc

View file
 	CHECK_CLOSE( 1., point_velocity[1], TEST_PREC);
 	CHECK_CLOSE( 0., point_velocity[2], TEST_PREC);
 }
+
+TEST ( FixedJointCalcPointVelocity ) {
+	// the standard modeling using a null body
+	Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
+	Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
+
+	Model model;
+	model.Init();
+
+	Joint joint_rot_z (
+			JointTypeRevolute,
+			Vector3d(0., 0., 1.)
+			);
+	model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body);
+
+	SpatialTransform transform = Xtrans (Vector3d (1., 0., 0.));
+	unsigned int fixed_body_id = model.AppendBody (transform, Joint(JointTypeFixed), fixed_body, "fixed_body");
+
+	VectorNd Q = VectorNd::Zero (model.dof_count);
+	VectorNd QDot = VectorNd::Zero (model.dof_count);
+
+	QDot[0] = 1.;
+
+	ClearLogOutput();
+	Vector3d point0_velocity = CalcPointVelocity (model, Q, QDot, fixed_body_id, Vector3d (0., 0., 0.));
+	// cout << LogOutput.str() << endl;
+	Vector3d point1_velocity = CalcPointVelocity (model, Q, QDot, fixed_body_id, Vector3d (1., 0., 0.));
+
+	CHECK_ARRAY_CLOSE (Vector3d (0., 1., 0.).data(), point0_velocity.data(), 3, TEST_PREC);
+	CHECK_ARRAY_CLOSE (Vector3d (0., 2., 0.).data(), point1_velocity.data(), 3, TEST_PREC);
+}
+
+/*
+TEST ( FixedJointCalcPointVelocityRotated ) {
+	// the standard modeling using a null body
+	Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
+	Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
+
+	Model model;
+	model.Init();
+
+	Joint joint_rot_z (
+			JointTypeRevolute,
+			Vector3d(0., 0., 1.)
+			);
+	model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body);
+
+	SpatialTransform transform = Xtrans (Vector3d (1., 0., 0.));
+	unsigned int fixed_body_id = model.AppendBody (transform, Joint(JointTypeFixed), fixed_body, "fixed_body");
+
+	VectorNd Q = VectorNd::Zero (model.dof_count);
+	VectorNd QDot = VectorNd::Zero (model.dof_count);
+
+	Q[0] = M_PI * 0.5;
+	QDot[0] = 1.;
+
+	ClearLogOutput();
+	Vector3d point0_velocity = CalcPointVelocity (model, Q, QDot, fixed_body_id, Vector3d (0., 0., 0.));
+	cout << LogOutput.str() << endl;
+	Vector3d point1_velocity = CalcPointVelocity (model, Q, QDot, fixed_body_id, Vector3d (1., 0., 0.));
+
+	CHECK_ARRAY_CLOSE (Vector3d (-1., 0., 0.).data(), point0_velocity.data(), 3, TEST_PREC);
+	CHECK_ARRAY_CLOSE (Vector3d (-2., 0., 0.).data(), point1_velocity.data(), 3, TEST_PREC);
+}
+*/

File tests/KinematicsTests.cc

View file
 	CHECK_ARRAY_CLOSE (Vector3d (1., 2., 0.1).data(), base_coords.data(), 3, TEST_PREC);
 }
 
+TEST ( FixedJointBodyCalcBodyToBaseRotated ) {
+	// the standard modeling using a null body
+	Body null_body;
+	Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
+	Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
+
+	Model model;
+	model.Init();
+
+	Joint joint_rot_z (
+			JointTypeRevolute,
+			Vector3d(0., 0., 1.)
+			);
+	model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body);
+	unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(1., 0., 0.)), Joint(JointTypeFixed), fixed_body);
+
+	VectorNd Q = VectorNd::Zero (model.dof_count);
+
+	ClearLogOutput();
+	Q[0] = M_PI * 0.5;
+	Vector3d base_coords = CalcBodyToBaseCoordinates (model, Q, fixed_body_id, Vector3d (1., 0., 0.));
+//	cout << LogOutput.str() << endl;	
+
+	CHECK_ARRAY_CLOSE (Vector3d (0., 2., 0.).data(), base_coords.data(), 3, TEST_PREC);
+}
+
 TEST ( FixedJointBodyCalcBaseToBody ) {
 	// the standard modeling using a null body
 	Body null_body;
 	CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.1).data(), base_coords.data(), 3, TEST_PREC);
 }
 
+TEST ( FixedJointBodyCalcBaseToBodyRotated ) {
+	// the standard modeling using a null body
+	Body null_body;
+	Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
+	Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
+
+	Model model;
+	model.Init();
+
+	Joint joint_rot_z (
+			JointTypeRevolute,
+			Vector3d(0., 0., 1.)
+			);
+	model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body);
+	unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(1., 0., 0.)), Joint(JointTypeFixed), fixed_body);
+
+	VectorNd Q = VectorNd::Zero (model.dof_count);
+
+	ClearLogOutput();
+	Q[0] = M_PI * 0.5;
+	Vector3d base_coords = CalcBaseToBodyCoordinates (model, Q, fixed_body_id, Vector3d (0., 2., 0.));
+	// cout << LogOutput.str() << endl;	
+
+	CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.).data(), base_coords.data(), 3, TEST_PREC);
+}
+
 TEST ( FixedJointBodyWorldOrientation ) {
 	// the standard modeling using a null body
 	Body null_body;