Commits

Martin Felis committed c7cd036

found problematic test case for fixed joints and inverse dynamics

Comments (0)

Files changed (2)

 		unsigned int lambda = model.lambda[i];
 
 		jcalc (model, i, X_J, model.S[i], v_J, c_J, model.q[i], model.qdot[i]);
-		LOG << "X_T (" << i << "):" << std::endl << model.X_T[i] << std::endl;
 
 		model.X_lambda[i] = X_J * model.X_T[i];
 
 			model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.S[i] * model.qddot[i] + model.c[i];
 		}
 
-		LOG << "X_J (" << i << "):" << std::endl << X_J << std::endl;
-		LOG << "v (" << i << "):" << std::endl << v_J << std::endl;
-		LOG << "a (" << i << "):" << std::endl << v_J << std::endl;
-
 		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) {
+		}
+	}
+
+	LOG << "-- first loop --" << std::endl;
+	for (i = 0; i < model.mBodies.size(); i++) {
+		LOG << "X_base[" << i << "] = " << std::endl << model.X_base[i] << std::endl;
+	}
+	for (i = 0; i < model.mBodies.size(); i++) {
+		LOG << "v[" << i << "] = " << model.v[i].transpose() << std::endl;
+	}
+	for (i = 0; i < model.mBodies.size(); i++) {
+		LOG << "a[" << i << "] = " << model.a[i].transpose() << std::endl;
+	}
+	for (i = 0; i < model.mBodies.size(); i++) {
+		LOG << "f[" << i << "] = " << model.f[i].transpose() << std::endl;
 	}
 
 	for (i = model.mBodies.size() - 1; i > 0; i--) {
 		}
 	}
 
+	LOG << "-- second loop" << std::endl;
+	LOG << "tau = " << model.tau.transpose() << std::endl;
+	for (i = 0; i < model.mBodies.size(); i++) {
+		LOG << "f[" << i << "] = " << model.f[i].transpose() << std::endl;
+	}
+	for (i = 0; i < model.mBodies.size(); i++) {
+		LOG << "S[" << i << "] = " << model.S[i].transpose() << std::endl;
+	}
+
+
 	// copy back values
 	CopyModelStateVectorToDofVector (model, Tau, model.tau);
 }

tests/DynamicsTests.cc

 
 	delete ref_model;
 }
+
+TEST_FIXTURE(SimpleFixture, TestInverseDynamicsFixedJointNonNullBody) {
+	Body body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
+	Joint joint (
+			JointTypeRevolute,
+			Vector3d (0., 0., 1.)
+			);
+
+	Body null_body (1., Vector3d (0., 1., 0.), Vector3d (0., 0., 0.));
+	Joint fixed_joint (
+			JointTypeFixed,
+			Vector3d (0., 0., 0.)
+			);
+
+	model->AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint, null_body);
+	model->AppendBody(Xtrans(Vector3d(0., 0., 0.)), fixed_joint, body);
+	model->AppendBody(Xtrans(Vector3d(1., 0., 0.)), joint, null_body);
+	model->AppendBody(Xtrans(Vector3d(0., 0., 0.)), fixed_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 (*ref_model, Q, QDot, QDDot, Tau_ref);
+//	cout << LogOutput.str() << endl;
+
+	ClearLogOutput();
+	InverseDynamics (*model, Q, QDot, QDDot, Tau);
+//	cout << LogOutput.str() << endl;
+
+	CHECK_ARRAY_CLOSE (Tau_ref.data(), Tau.data(), 2, TEST_PREC);
+
+	delete ref_model;
+}