Commits

Martin Felis committed 58fb596

removed fixed joint handling, postponing feature

Comments (0)

Files changed (10)

addons/benchmark/benchmark.cc

 		<< " duration = " << setw(10) << duration << "(s)"
 		<< " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl;
 
-	cout << "36 DOF estimate: " << (duration / sample_count) * (36. / model->dof_count) << endl;;
-
 	return duration;
 }
 
  * \li \ref dynamics_group
  * \li \ref contacts_group
  *
- * \section KnownRestrictions Known Restrictions / Bugs
- *
- * \li Fixed joints are currently under active development and are not properly
- * tested. Use with great care.
- * \li External forces that are supplied by optional arguments f_ext e.g. to
- * RigidBodyDynamics::ForwardDynamics() are not properly tested in
- * combination with fixed joints.
- *
  * \section Licensing Licensing
  *
  * The library is published under the very permissive zlib free software
 	}
 
 	for (i = model.mBodies.size() - 1; i > 0; i--) {
-		// we can skip further processing if the joint is fixed
-		if (model.mJoints[i].mJointType == JointTypeFixed)
-			continue;
-
 		CS.d_U[i] = CS.d_IA[i] * model.S[i];
 		CS.d_d[i] = model.S[i].dot(model.U[i]);
 		CS.d_u[i] = model.tau[i] - model.S[i].dot(CS.d_pA[i]);
 			CS.d_a[i] = X_lambda.apply(CS.d_a[lambda]) + model.c[i];
 		}
 
-		// we can skip further processing if the joint type is fixed
-		if (model.mJoints[i].mJointType == JointTypeFixed) {
-			model.qddot[i] = 0.;
-			continue;
-		}
-
 		QDDot[i - 1] = (CS.d_u[i] - model.U[i].dot(CS.d_a[i])) / model.d[i];
 		LOG << "QDDot_t[" << i - 1 << "] = " << QDDot[i - 1] << std::endl;
 		CS.d_a[i] = CS.d_a[i] + model.S[i] * QDDot[i - 1];
 		if (lambda != 0) {
 			SpatialTransform X_lambda = model.X_lambda[i];
 
-			// for fixed joints we simply transform the spatial inertia and the
-			// spatial bias force to the parent body
-			if (model.mJoints[i].mJointType == JointTypeFixed) {
-				model.IA[lambda] = model.IA[lambda] + X_lambda.toMatrixTranspose() * model.IA[i] * X_lambda.toMatrix();
-				model.pA[lambda] = model.pA[lambda] + X_lambda.toMatrixTranspose() * model.pA[i];
-				continue;
-			}
-
 			SpatialMatrix Ia = model.IA[i] - model.U[i] * (model.U[i] / model.d[i]).transpose();
 			SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.U[i] * model.u[i] / model.d[i];
 
 
 		model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i];
 
-		// we can skip further processing if the joint type is fixed
-		if (model.mJoints[i].mJointType == JointTypeFixed) {
-			model.qddot[i] = 0.;
-			continue;
-		}
-
 		model.qddot[i] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i]));
 		model.a[i] = model.a[i] + model.S[i] * model.qddot[i];
 	}
 		model.f[i] = model.mBodies[i].mSpatialInertia * model.a[i] + crossf(model.v[i],model.mBodies[i].mSpatialInertia * model.v[i]);
 		if (f_ext != NULL && (*f_ext)[i] != SpatialVectorZero)
 			model.f[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i];
-
-		if (model.mJoints[i].mJointType == JointTypeFixed) {
-			model.f[lambda].setZero();
-		}
 	}
 
 	LOG << "-- first loop --" << std::endl;
 
 	H.setZero();
 
-	std::vector<unsigned int> fixed_joints_count (model.mBodies.size(), 0.);
-
 	unsigned int i;
 	for (i = 1; i < model.mBodies.size(); i++) {
-		if (model.mJoints[i].mJointType == JointTypeFixed)
-			fixed_joints_count[i] = fixed_joints_count[i - 1] + 1;
-		else
-			fixed_joints_count[i] = fixed_joints_count[i - 1];
 		model.Ic[i] = model.mBodies[i].mSpatialInertia;
 	}
 
 	for (i = 0; i < model.mBodies.size(); i++) {
 		LOG << "Ic[" << i << "] = " << std::endl << model.Ic[i] << std::endl;
 	}
-	for (i = 0; i < model.mBodies.size(); i++) {
-		LOG << "mFixedJointCount[" << i << "] = " << std::endl << model.mFixedJointCount[i] << std::endl;
-	}
-
 	unsigned int dof_i = model.dof_count;
 
 	for (i = model.mBodies.size() - 1; i > 0; i--) {
 			model.Ic[lambda] = model.Ic[lambda] + model.X_lambda[i].toMatrixTranspose() * model.Ic[i] * model.X_lambda[i].toMatrix();
 		}
 
-		if (model.mJoints[i].mJointType == JointTypeFixed) {
-			continue;
-		}
-
-		dof_i = i - model.mFixedJointCount[i] - 1;
+		dof_i = i - 1;
 
 		SpatialVector F = model.Ic[i] * model.S[i];
 		H(dof_i, dof_i) = model.S[i].dot(F);
 			F = model.X_lambda[j].toMatrixTranspose() * F;
 			j = model.lambda[j];
 
-			if (model.mJoints[j].mJointType == JointTypeFixed) {
-				continue;
-			}
-
-			dof_j = j - model.mFixedJointCount[j] - 1;
+			dof_j = j - 1;
 
 			LOG << "i,j         = " << i << ", " << j << std::endl;
 			LOG << "dof_i,dof_j = " << dof_i << ", " << dof_j << std::endl;
 	// constraints (see RBDA, p. 55)
 	c_J.setZero();
 
-	if (model.mJoints[joint_id].mJointType == JointTypeFixed) {
-		XJ = SpatialTransform();
-		v_J.setZero();
-
-		return;
-	} else if (model.mJoints[joint_id].mJointType == JointTypeRevolute) {
+	if (model.mJoints[joint_id].mJointType == JointTypeRevolute) {
 		XJ = Xrot (q, Vector3d (
 					model.mJoints[joint_id].mJointAxes[0][0],
 					model.mJoints[joint_id].mJointAxes[0][1],
  */
 enum JointType {
 	JointTypeUndefined = 0,
-	JointTypeFixed,
 	JointTypeRevolute,
 	JointTypePrismatic,
 
 		// Some assertions, as we concentrate on simple cases
 	
 		// Only rotation around the Z-axis
-		assert ( joint_type == JointTypeRevolute || joint_type == JointTypeFixed || joint_type == JointTypePrismatic );
+		assert ( joint_type == JointTypeRevolute || joint_type == JointTypePrismatic );
 
 		mJointType = joint_type;
 
 					0., 0., 0.
 					);
 
-		} else if (joint_type == JointTypeFixed) {
-			mJointAxes[0].set (
-					joint_axis[0],
-					joint_axis[1], 
-					joint_axis[2], 
-					0., 0., 0.
-					);
-			mJointAxes[0].set (0., 0., 0., 0., 0., 0.);
 		} else if (joint_type == JointTypePrismatic) {
 			// make sure we have a unit axis
 			assert (joint_axis.squaredNorm() == 1.);
 	mJoints.push_back(root_joint);
 	S.push_back (zero_spatial);
 	X_T.push_back(SpatialTransform());
-	mFixedJointCount = std::vector<unsigned int> (1, 0);
 	
 	// Dynamic variables
 	c.push_back(zero_spatial);
 	// Here we emulate multi DoF joints by simply adding nullbodies. This
 	// may be changed in the future, but so far it is reasonably fast.
 	if (joint.mJointType != JointTypePrismatic 
-			&& joint.mJointType != JointTypeRevolute
-			&& joint.mJointType != JointTypeFixed) {
+			&& joint.mJointType != JointTypeRevolute) {
 		unsigned int joint_count;
 		if (joint.mJointType == JointType1DoF)
 			joint_count = 1;
 	mBodies.push_back(body);
 	mBodyNames.push_back(body_name);
 
-	if (joint.mJointType != JointTypeFixed)
-		dof_count += 1;
+	dof_count++;
 
 	// state information
 	q = VectorNd::Zero (mBodies.size());
 	// child bodies perspective.
 	X_T.push_back(joint_frame);
 
-	// compute the proper fixed joint count
-	unsigned int fixed_joint_count = mFixedJointCount[mBodies.size() - 2];
-
-	if (joint.mJointType == JointTypeFixed)
-		fixed_joint_count++;
-
-	mFixedJointCount.push_back (fixed_joint_count);
-
 	// Dynamic variables
 	c.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));
 	IA.push_back(body.mSpatialInertia);
 #include <assert.h>
 #include <iostream>
 #include <limits>
+#include <cstring>
 
 #include "Logging.h"
 #include "Joint.h"
 	void Init ();
 };
 
-/** \brief Copies values from a DoF-vector to a model state vector while
- * taking account for fixed joints.
+/** \brief Copies values from a DoF-vector to a model state vector.
  *
- * Fixed joints do not have a DoF and must therefore be handled
- * differently. This functions copies values from a vector with
- * Model::dof_count variables into a vector of Model::mBodies.size()
- * (such as Model::q, Model::qdot, Model::qddot, and Model::tau) and
- * always skips the values which correspond to a fixed joint.
+ * All values for the generalized vectors such as Q, QDot, QDDot, and Tau
+ * are copied to the values within the model structure. As the model
+ * structure uses a different numbering, all values have to be shifted by
+ * one.
  */
 inline void CopyDofVectorToModelStateVector (const Model &model, Math::VectorNd &dest_model_state, const Math::VectorNd &dof_src) {
-	unsigned int body_index = 1;
-	unsigned int dof_index = 0;
-
 	assert (dest_model_state.size() == model.mBodies.size());
 	assert (dof_src.size() == model.dof_count);
 
-	do {
-		// bodies that are connected by fixed joints are skipped
-		if (model.mJoints[body_index].mJointType == JointTypeFixed) {
-			body_index++;
-			continue;
-		}
-
-		dest_model_state[body_index] = dof_src[dof_index];
-
-		dof_index++;
-		body_index++;
-	} while (body_index < model.mBodies.size());
+	for (unsigned int i = 0; i < model.dof_count; i++)
+		dest_model_state[i + 1] = dof_src[i];
 }
 
-/** \brief Inverse operation to CopyDofVectorToModelStateVector()
- * 
- * Fixed joints do not have a DoF and must therefore be handled
- * differently. This functions copies values from a vector with
- * Model::mBodies.size() variables (such as Model::q, Model::qdot,
- * Model::qddot, and Model::tau) into a vector of Model::dof_count
- * variables and skips the values which correspond to a fixed
- * joint.
- */
+/** \brief Inverse operation to CopyDofVectorToModelStateVector() */
 inline void CopyModelStateVectorToDofVector (const Model &model, Math::VectorNd &dof_dest, const Math::VectorNd &src_model_state) {
-	unsigned int body_index = 1;
-	unsigned int dof_index = 0;
-
 	assert (dof_dest.size() == model.dof_count);
 	assert (src_model_state.size() == model.mBodies.size());
 
-	do {
-		// bodies that are connected by fixed joints are skipped
-		if (model.mJoints[body_index].mJointType == JointTypeFixed) {
-			body_index++;
-			continue;
-		}
-
-		dof_dest[dof_index] = src_model_state[body_index];
-
-		dof_index++;
-		body_index++;
-	} while (body_index < model.mBodies.size());
+	for (unsigned int i = 0; i < model.dof_count; i++)
+		dof_dest[i] = src_model_state[i + 1];	
 }
 
 /** @} */

tests/CompositeRigidBodyTests.cc

 
 	CHECK_ARRAY_CLOSE (H_crba.data(), H_id.data(), model->dof_count * model->dof_count, TEST_PREC);
 }
-
-TEST_FIXTURE(SimpleFixture, TestCompositeRigidBodyFixedJointSimple) {
-	Body body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
-	Joint joint (
-			JointTypeRevolute,
-			Vector3d (0., 0., 1.)
-			);
-
-	Body null_body (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
-	Joint fixed_joint (
-			JointTypeFixed,
-			Vector3d (0., 0., 0.)
-			);
-
-	model->AddBody(0, Xtrans(Vector3d(1., 0., 0.)), fixed_joint, null_body);
-	model->AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint, body);
-	model->AppendBody(Xtrans(Vector3d(1., 0., 0.)), fixed_joint, null_body);
-	model->AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint, body);
-
-	// proper initialization of Q, QDot, QDDot, Tau
-	ResizeVectors();
-
-	// create a reference model
-	Model *ref_model = new Model;
-	ref_model->gravity = Vector3d (0., -9.81, 0.);
-	ref_model->Init();
-
-	ref_model->AddBody(0,Xtrans(Vector3d(1., 0., 0.)), joint, body);
-	ref_model->AppendBody(Xtrans(Vector3d(1., 0., 0.)), joint, body);
-
-	VectorNd Tau_ref (ref_model->dof_count);
-
-	Q.setZero();
-	QDot.setZero();
-	QDDot.setZero();
-	Tau.setZero();
-
-	// make sure the models are equivalent in terms of their dynamics
-	Q[0] = 1.1;
-	Q[1] = 2.3;
-
-	MatrixNd H (model->dof_count, model->dof_count);
-	MatrixNd H_ref (ref_model->dof_count, ref_model->dof_count);
-
-	ClearLogOutput();
-	CompositeRigidBodyAlgorithm (*ref_model, Q, H_ref);
-//	cout << LogOutput.str() << endl;
-
-	ClearLogOutput();
-	CompositeRigidBodyAlgorithm (*model, Q, H);
-//	cout << LogOutput.str() << endl;
-
-	CHECK_ARRAY_CLOSE (H_ref.data(), H.data(), H.size(), TEST_PREC);
-
-	delete ref_model;
-}

tests/ModelTests.cc

 	CHECK_EQUAL (test_joint_axis, S);
 }
 
-/** Checks that the model internal position variable does not get
- * updated during a call to ForwardDynamics
- */
-/*
-TEST_FIXTURE(ModelFixture, TestAddFixedJoint) {
-	Body body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
-	Joint joint (
-			JointTypeRevolute,
-			Vector3d (0., 0., 1.)
-			);
-
-	Body endeffector (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
-	Joint fixed_joint (
-			JointTypeFixed,
-			Vector3d (0., 0., 0.)
-			);
-
-	model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body);
-	unsigned int fixed_body = model->AddBody(1, Xtrans(Vector3d(1., 0., 0.)), fixed_joint, endeffector);
-	model->AddBody(fixed_body, Xtrans(Vector3d(0., 0., 0.)), joint, body);
-
-	CHECK_EQUAL (2, model->dof_count);
-
-	// Initialization of the input vectors
-	VectorNd Q = VectorNd::Zero ((size_t) model->dof_count);
-	VectorNd QDot = VectorNd::Zero ((size_t) model->dof_count);
-	VectorNd QDDot = VectorNd::Zero ((size_t) model->dof_count);
-	VectorNd Tau = VectorNd::Zero ((size_t) model->dof_count);
-
-	// set all position variables to -1. After FD the position variable of the
-	// fixed body should keep this value.
-	for (int i = 0; i < model->mBodies.size(); i++) {
-		model->q[i] = -1.;
-	}
-
-	QDot[0] = 1.;
-
-	ForwardDynamics(*model, Q, QDot, Tau, QDDot);
-	CHECK_EQUAL (-1., model->q[fixed_body]);
-
-	InverseDynamics(*model, Q, QDot, QDDot, Tau);
-	CHECK_EQUAL (-1., model->q[fixed_body]);
-
-	// Note: checking of the lagrangian formulation and the CRBA is done in
-	// tests/CompositeRigidBodyTests.cc
-}
-*/
-
-TEST_FIXTURE(ModelFixture, TestCalcVelocitiesSimple) {
-	Body body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
-	Joint joint (
-			JointTypeRevolute,
-			Vector3d (0., 0., 1.)
-			);
-
-	Body endeffector (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
-	Joint fixed_joint (
-			JointTypeFixed,
-			Vector3d (0., 0., 0.)
-			);
-
-	model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body);
-	model->AddBody(1, Xtrans(Vector3d(1., 0., 0.)), fixed_joint, endeffector);
-
-	// Initialization of the input vectors
-	VectorNd Q = VectorNd::Zero ((size_t) model->dof_count);
-	VectorNd QDot = VectorNd::Zero ((size_t) model->dof_count);
-	VectorNd QDDot = VectorNd::Zero ((size_t) model->dof_count);
-	VectorNd Tau = VectorNd::Zero ((size_t) model->dof_count);
-
-	QDot[0] = 1.;
-	ForwardDynamics(*model, Q, QDot, Tau, QDDot);
-
-	SpatialVector spatial_body_velocity (0., 0., 1., 0., 1., 0.);
-	CHECK_EQUAL (spatial_body_velocity, model->v.at(2));
-	// std::cout << LogOutput.str() << std::endl;
-	ClearLogOutput();
-
-	QDot[0] = -1.;
-	ForwardDynamics(*model, Q, QDot, Tau, QDDot);
-
-	spatial_body_velocity.set (0., 0., -1., 0., -1., 0.);
-	CHECK_EQUAL (spatial_body_velocity, model->v.at(2));
-	// std::cout << LogOutput.str() << std::endl;
-}
-
 TEST_FIXTURE ( ModelFixture, TestTransformBaseToLocal ) {
 	Body body;
 	unsigned int body_id = model->SetFloatingBaseBody (body);