Commits

Martin Felis  committed 87cb6cd

CRBA with fixed joints works for simple test case

  • Participants
  • Parent commits 635c6b2
  • Branches dev

Comments (0)

Files changed (5)

File src/Dynamics.cc

 
 	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;
 	}
 
+	LOG << "-- initialization --" << std::endl;
+	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--) {
 		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();
 		}
 
+		if (model.mJoints[i].mJointType == JointTypeFixed) {
+			continue;
+		}
+
+		dof_i = i - model.mFixedJointCount[i] - 1;
+
 		SpatialVector F = model.Ic[i] * model.S[i];
-		H(i - 1, i - 1) = model.S[i].dot(F);
+		H(dof_i, dof_i) = model.S[i].dot(F);
+
 		unsigned int j = i;
+		unsigned int dof_j = dof_i;
 
 		while (model.lambda[j] != 0) {
 			F = model.X_lambda[j].toMatrixTranspose() * F;
 			j = model.lambda[j];
-			H(i - 1,j - 1) = F.dot(model.S[j]);
-			H(j - 1,i - 1) = H(i - 1,j - 1);
+
+			if (model.mJoints[j].mJointType == JointTypeFixed) {
+				continue;
+			}
+
+			dof_j = j - model.mFixedJointCount[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);
 		}
 	}
 }

File src/Kinematics.cc

 
 	unsigned int i;
 
-	assert (model.q.size() == Q.size() + 1);
-	assert (model.qdot.size() == QDot.size() + 1);
-	assert (model.qddot.size() == QDDot.size() + 1);
-
 	if (model.experimental_floating_base) {
 		assert (0 && !"UpdateKinematics not supported yet for experimental floating bases");
 	}
 	
-	// positions
-	for (i = 0; i < model.dof_count; i++) {
-		model.q[i + 1] = Q[i];
-	}
-
-	// velocities
-	for (i = 0; i < model.dof_count; i++) {
-		model.qdot[i + 1] = QDot[i];
-	}
-
-	// accelerations
-	for (i = 0; i < model.dof_count; i++) {
-		model.qddot[i + 1] = QDDot[i];
-	}
+	// Copy positions, velocities, and accelerations while taking account for
+	// fixed joints
+	CopyDofVectorToModelStateVector (model, model.q, Q);
+	CopyDofVectorToModelStateVector (model, model.qdot, QDot);
+	CopyDofVectorToModelStateVector (model, model.qddot, QDDot);
 
 	SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]);
 
 
 	unsigned int i;
 
-	if (Q)
-	
-	if (QDot)
-
-	if (QDDot)
-		assert (model.qddot.size() == QDDot->size() + 1);
-
 	if (model.experimental_floating_base) {
 		assert (0 && !"UpdateKinematics not supported yet for experimental floating bases");
 	}
 
 	if (Q) {
-		assert (model.q.size() == Q->size() + 1);
-		// positions
-		for (i = 0; i < model.dof_count; i++) {
-			model.q[i + 1] = (*Q)[i];
-		}
+		CopyDofVectorToModelStateVector (model, model.q, *Q);
 	}
 
 	if (QDot) {
-		assert (model.qdot.size() == QDot->size() + 1);
-		// velocities
-		for (i = 0; i < model.dof_count; i++) {
-			model.qdot[i + 1] = (*QDot)[i];
-		}
+		CopyDofVectorToModelStateVector (model, model.qdot, *QDot);
 	}
 
 	if (QDDot) {
-		assert (model.qddot.size() == QDDot->size() + 1);
-		// accelerations
-		for (i = 0; i < model.dof_count; i++) {
-			model.qddot[i + 1] = (*QDDot)[i];
-		}
+		CopyDofVectorToModelStateVector (model, model.qddot, *QDDot);
 	}
 
 	if (Q) {

File src/Model.cc

 	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);
 	// 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);
  *
  * An important note is that body 0 is the root body and the moving bodies
  * start at index 1. Additionally the vectors for the states q, qdot, etc.
- * have \#Model::dof_count + 1 entries where always the first entry (e.g. q[0])
- * contains the value for the base (or "root" body). Thus the numbering might be
- * confusing as q[1] holds the position variable of the first degree of
- * freedom. This numbering scheme is very beneficial in terms of readability
- * of the code as the resulting code is very similar to the pseudo-code in
- * the RBDA book.
+ * have \#Model::mBodies + 1 entries where always the first entry (e.g.
+ * q[0]) contains the value for the base (or "root" body). Thus the
+ * numbering might be confusing as q[1] holds the position variable of the
+ * first added joint. This numbering scheme is very beneficial in terms of
+ * readability of the code as the resulting code is very similar to the
+ * pseudo-code in the RBDA book.
+ *
+ * \note The dimensions of q, qdot, qddot, and tau are increased whenever
+ * a body is added. This is also true for bodies that are added with a
+ * fixed joint. To query the actual number of degrees of freedom use
+ * Model::dof_count.
  */
 struct Model {
 	// Structural information
 	/** \brief The joint position
 	 * 
 	 * Warning: to have an easier numbering in the algorithm the state vector
-	 * has NDOF + 1 elements. However element with index 0 is not used!
+	 * has #Bodies + 1 elements. However element with index 0 is not used!
 	 * 
 	 * q[0] - unused <br>
 	 * q[1] - joint 1 <br>
 	 * q[2] - joint 2 <br>
 	 * ... <br>
-	 * q[NDOF] - joint NDOF <br>
+	 * q[#Bodies] - joint #Bodies <br>
 	 *
+	 * \note The dimensions of q, qdot, qddot, and tau are increased whenever
+	 * a body is added. This is also true for bodies that are added with a
+	 * fixed joint. To query the actual number of degrees of freedom use
+	 * Model::dof_count.
 	 */
 	Math::VectorNd q;
 	/// \brief The joint velocity
 	std::vector<Math::SpatialVector> S;
 	/// \brief Transformations from the parent body to the frame of the joint
 	std::vector<Math::SpatialTransform> X_T;
+	/// \brief The number of fixed joints that have been declared before each joint.
+	std::vector<unsigned int> mFixedJointCount;
 
 	////////////////////////////////////
 	// Dynamics variables

File 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;
+}