Commits

Martin Felis committed 2e2344a

Trying to support more intuitive fixed joints, i.e. no increase of Model::dof_count

Comments (0)

Files changed (4)

 
 	SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]);
 
-	unsigned int i;
+	unsigned int i = 0;
 
 	// Copy state values from the input to the variables in model
-	assert (model.q.size() == Q.size() + 1);
-	assert (model.qdot.size() == QDot.size() + 1);
-	assert (model.qddot.size() == QDDot.size() + 1);
-	assert (model.tau.size() == Tau.size() + 1);
-
-	for (i = 0; i < Q.size(); i++) {
-		model.q[i+1] = Q[i];
-		model.qdot[i+1] = QDot[i];
-		model.qddot[i+1] = QDDot[i];
-		model.tau[i+1] = Tau[i];
-	}
+	CopyDofVectorToModelStateVector (model, model.q, Q);
+	CopyDofVectorToModelStateVector (model, model.qdot, QDot);
+	CopyDofVectorToModelStateVector (model, model.tau, Tau);
 
 	// Reset the velocity of the root body
 	model.v[0].setZero();
 
 	LOG << "qddot = " << model.qddot.transpose() << std::endl;
 
-	for (i = 1; i < model.mBodies.size(); i++) {
-		QDDot[i - 1] = model.qddot[i];
-	}
+	// copy back values
+	CopyModelStateVectorToDofVector (model, QDDot, model.qddot);
 }
 
 void ForwardDynamicsLagrangian (
 	unsigned int i;
 
 	// Copy state values from the input to the variables in model
-	assert (model.q.size() == Q.size() + 1);
-	assert (model.qdot.size() == QDot.size() + 1);
-	assert (model.qddot.size() == QDDot.size() + 1);
-	assert (model.tau.size() == Tau.size() + 1);
-
-	for (i = 0; i < Q.size(); i++) {
-		model.q[i+1] = Q[i];
-		model.qdot[i+1] = QDot[i];
-		model.qddot[i+1] = QDDot[i];
-	}
+	CopyDofVectorToModelStateVector (model, model.q, Q);
+	CopyDofVectorToModelStateVector (model, model.qdot, QDot);
+	CopyDofVectorToModelStateVector (model, model.qddot, QDDot);
 
 	// Reset the velocity of the root body
 	model.v[0].setZero();
 		}
 	}
 
-	for (i = 0; i < Tau.size(); i++) {
-		Tau[i] = model.tau[i + 1];
-	}
+	// copy back values
+	CopyModelStateVectorToDofVector (model, Tau, model.tau);
 }
 
 void CompositeRigidBodyAlgorithm (Model& model, const VectorNd &Q, MatrixNd &H, bool update_kinematics) {
 	LOG << "-------- " << __func__ << " --------" << std::endl;
 
-	if (H.rows() != Q.size() || H.cols() != Q.size()) 
-		H.resize(Q.size(), Q.size());
+	assert (H.rows() == model.dof_count && H.cols() == model.dof_count);
 
 	if (update_kinematics)
 		UpdateKinematicsCustom (model, &Q, NULL, NULL);
 	mu.push_back(std::vector<unsigned int>());
 	mu.at(parent_id).push_back(q.size());
 
-	dof_count += 1;
+	// Bodies
+	X_lambda.push_back(SpatialTransform());
+	X_base.push_back(SpatialTransform());
+	mBodies.push_back(body);
+	mBodyNames.push_back(body_name);
+
+	if (joint.mJointType != JointTypeFixed)
+		dof_count += 1;
 
 	// state information
-	q.resize (dof_count + 1);
-	qdot.resize (dof_count + 1);
-	qddot.resize (dof_count + 1);
-	tau.resize (dof_count + 1);
+	q.resize (mBodies.size());
+	qdot.resize (mBodies.size());
+	qddot.resize (mBodies.size());
+	tau.resize (mBodies.size());
 
 	v.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));
 	a.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));
 	pA.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));
 	U.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));
 
-	d = VectorNd::Zero (dof_count + 1);
-	u = VectorNd::Zero (dof_count + 1);
+	d = VectorNd::Zero (mBodies.size());
+	u = VectorNd::Zero (mBodies.size());
 
 	f.push_back (SpatialVector (0., 0., 0., 0., 0., 0.));
 	Ic.push_back (SpatialMatrixIdentity);
 
-	// Bodies
-	X_lambda.push_back(SpatialTransform());
-	X_base.push_back(SpatialTransform());
-	mBodies.push_back(body);
-	mBodyNames.push_back(body_name);
-
-	return q.size() - 1;
+	return mBodies.size() - 1;
 }
 
 unsigned int Model::AppendBody (
 	void Init ();
 };
 
+/** \brief Copies values from a DoF-vector to a model state vector while
+ * taking account for fixed joints.
+ *
+ * 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.
+ */
+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());
+}
+
+/** \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.
+ */
+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());
+}
+
 /** @} */
 
 }

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]);
+
+	ForwardDynamicsLagrangian(*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]);
+}
+
+/*
 TEST_FIXTURE(ModelFixture, TestCalcVelocitiesSimple) {
 	Body body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
 	Joint joint (
 	CHECK_EQUAL (spatial_body_velocity, model->v.at(2));
 	// std::cout << LogOutput.str() << std::endl;
 }
+*/
 
 TEST_FIXTURE ( ModelFixture, TestTransformBaseToLocal ) {
 	Body body;