Commits

Martin Felis committed 635c6b2

fixed joints: tests for forward and inverse dynamics work

  • Participants
  • Parent commits 2e2344a
  • Branches dev

Comments (0)

Files changed (6)

File doc/Mainpage.h

  * \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

File src/Dynamics.cc

 	CopyDofVectorToModelStateVector (model, model.qdot, QDot);
 	CopyDofVectorToModelStateVector (model, model.tau, Tau);
 
+	LOG << "Q          = " << Q.transpose() << std::endl;
+	LOG << "QDot       = " << QDot.transpose() << std::endl;
+	LOG << "Tau        = " << Tau.transpose() << std::endl;
+	LOG << "---" << std::endl;
+	LOG << "model.q    = " << model.q.transpose() << std::endl;
+	LOG << "model.qdot = " << model.qdot.transpose() << std::endl;
+	LOG << "model.tau  = " << model.tau.transpose() << std::endl;
+	LOG << "---" << std::endl;
+
+
 	// Reset the velocity of the root body
 	model.v[0].setZero();
 
 	}
 
 	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "v[" << i << "]   = " << model.v[i] << std::endl;
+		LOG << "v[" << i << "]   = " << model.v[i].transpose() << std::endl;
 	}
 
 	for (i = 1; i < model.mBodies.size(); i++) {
 	}
 
 	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "pA[" << i << "]  = " << model.pA[i] << std::endl;
+		LOG << "pA[" << i << "]  = " << model.pA[i].transpose() << std::endl;
 	}
 
 	LOG << std::endl;
 
 	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;
-
 		model.U[i] = model.IA[i] * model.S[i];
 		model.d[i] = model.S[i].dot(model.U[i]);
 		model.u[i] = model.tau[i] - model.S[i].dot(model.pA[i]);
 
 		unsigned int lambda = model.lambda[i];
 		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];
-			SpatialTransform X_lambda = model.X_lambda[i];
 
 			// note: X_lambda.inverse().spatial_adjoint() = X_lambda.transpose()
 			model.IA[lambda] = model.IA[lambda] + X_lambda.toMatrixTranspose() * Ia * X_lambda.toMatrix();
 	LOG << "--- second loop ---" << std::endl;
 
 	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "U[" << i << "]   = " << model.U[i] << std::endl;
+		LOG << "U[" << i << "]   = " << model.U[i].transpose() << std::endl;
 	}
 
 	for (i = 1; i < model.mBodies.size(); i++) {
 		LOG << "IA[" << i << "]  = " << model.IA[i] << std::endl;
 	}
 	for (i = 1; i < model.mBodies.size(); i++) {
-		LOG << "pA[" << i << "]  = " << model.pA[i] << std::endl;
+		LOG << "pA[" << i << "]  = " << model.pA[i].transpose() << std::endl;
 	}
 
 	LOG << std::endl << "--- third loop ---" << std::endl;

File src/Model.cc

 		dof_count += 1;
 
 	// state information
-	q.resize (mBodies.size());
-	qdot.resize (mBodies.size());
-	qddot.resize (mBodies.size());
-	tau.resize (mBodies.size());
+	q = VectorNd::Zero (mBodies.size());
+	qdot = VectorNd::Zero (mBodies.size());
+	qddot = VectorNd::Zero (mBodies.size());
+	tau = VectorNd::Zero (mBodies.size());
 
 	v.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));
 	a.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));

File tests/DynamicsTests.cc

 #include "Kinematics.h"
 #include "Dynamics.h"
 #include "Contacts.h"
+#include "Fixtures.h"
 
 using namespace std;
 using namespace RigidBodyDynamics;
 	delete model;
 }
 
+TEST_FIXTURE(SimpleFixture, TestForwardDynamicsFixedJointSimple) {
+	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 QDDot_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;
+
+	QDot[0] = 3.1;
+	QDot[1] = -9.2;
+
+	Tau[0] = 3.2;
+	Tau[1] = -5.;
+
+	ClearLogOutput();
+	ForwardDynamics (*model, Q, QDot, Tau, QDDot);
+//	cout << LogOutput.str() << endl;
+
+	ClearLogOutput();
+	ForwardDynamics (*ref_model, Q, QDot, Tau, QDDot_ref);
+//	cout << LogOutput.str() << endl;
+
+	CHECK_ARRAY_CLOSE (QDDot_ref.data(), QDDot.data(), 2, TEST_PREC);
+
+	delete ref_model;
+}
+
+TEST_FIXTURE(SimpleFixture, TestInverseDynamicsFixedJointSimple) {
+	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;
+
+	QDot[0] = 3.1;
+	QDot[1] = -9.2;
+
+	QDDot[0] = 3.2;
+	QDDot[1] = -5.;
+
+	ClearLogOutput();
+	InverseDynamics (*model, Q, QDot, QDDot, Tau);
+//	cout << LogOutput.str() << endl;
+
+	ClearLogOutput();
+	InverseDynamics (*ref_model, Q, QDot, QDDot, Tau_ref);
+//	cout << LogOutput.str() << endl;
+
+	CHECK_ARRAY_CLOSE (Tau_ref.data(), Tau.data(), 2, TEST_PREC);
+
+	delete ref_model;
+}

File tests/Fixtures.h

 	RigidBodyDynamics::Math::VectorNd Tau;
 };
 
+struct SimpleFixture {
+	SimpleFixture () {
+		ClearLogOutput();
+		model = new RigidBodyDynamics::Model;
+		model->Init();
+		model->gravity = RigidBodyDynamics::Math::Vector3d (0., -9.81, 0.);
+	}
+	~SimpleFixture () {
+		delete model;
+	}
+	void ResizeVectors () {
+		Q = RigidBodyDynamics::Math::VectorNd::Zero (model->dof_count);
+		QDot = RigidBodyDynamics::Math::VectorNd::Zero (model->dof_count);
+		QDDot = RigidBodyDynamics::Math::VectorNd::Zero (model->dof_count);
+		Tau = RigidBodyDynamics::Math::VectorNd::Zero (model->dof_count);
+	}
 
+	RigidBodyDynamics::Model *model;
+
+	RigidBodyDynamics::Math::VectorNd Q;
+	RigidBodyDynamics::Math::VectorNd QDot;
+	RigidBodyDynamics::Math::VectorNd QDDot;
+	RigidBodyDynamics::Math::VectorNd Tau;
+};
+
+

File tests/ModelTests.cc

 	ForwardDynamics(*model, Q, QDot, Tau, QDDot);
 	CHECK_EQUAL (-1., model->q[fixed_body]);
 
-	ForwardDynamicsLagrangian(*model, Q, QDot, Tau, QDDot);
+	InverseDynamics(*model, Q, QDot, QDDot, Tau);
 	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
 }
 
 /*