Commits

Martin Felis  committed 9a1f5d5

fixed evil jacobian computation bug

  • Participants
  • Parent commits c7aa961
  • Branches dev

Comments (0)

Files changed (3)

File src/Kinematics.cc

 #include <iostream>
 #include <limits>
+#include <cstring>
 #include <assert.h>
 
 #include "mathutils.h"
 
 	assert (G.rows() == 3 && G.cols() == model.dof_count );
 
-	unsigned int j;
+	G.setZero();
+
+	// we have to make sure that only the joints that contribute to the
+	// bodies motion also get non-zero columns in the jacobian.
+	//VectorNd e = VectorNd::Zero(Q.size() + 1);
+
+	unsigned int j = body_id;
+
+	char e[(Q.size() + 1)];
+	memset (&e[0], 0, Q.size() + 1);
+
+	// e will contain 
+	while (j != 0) {
+		e[j] = 1;
+		j = model.lambda[j];
+	}
+
 	for (j = 1; j < model.mBodies.size(); j++) {
-		SpatialVector S_base;
-		S_base = point_trans * spatial_inverse(model.X_base[j]) * model.S[j];
+		if (e[j] == 1) {
+			SpatialVector S_base;
+			S_base = point_trans * spatial_inverse(model.X_base[j]) * model.S[j];
 
-		G(0, j - 1) = S_base[3];
-		G(1, j - 1) = S_base[4];
-		G(2, j - 1) = S_base[5];
+			G(0, j - 1) = S_base[3];
+			G(1, j - 1) = S_base[4];
+			G(2, j - 1) = S_base[5];
+		}
 	}
 }
 

File tests/ContactsTests.cc

 	cout << QDDot << endl;
 	*/
 }
+
+TEST ( TestForwardDynamicsContactsLagrangianMoving ) {
+	Model model;
+	model.Init();
+	model.gravity = Vector3d  (0., -9.81, 0.);
+	Body base_body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.));
+	unsigned int base_body_id = model.SetFloatingBaseBody(base_body);
+
+	VectorNd Q = VectorNd::Constant ((size_t) model.dof_count, 0.);
+	VectorNd QDot = VectorNd::Constant ((size_t) model.dof_count, 0.);
+	VectorNd QDDot = VectorNd::Constant  ((size_t) model.dof_count, 0.);
+	VectorNd Tau = VectorNd::Constant ((size_t) model.dof_count, 0.);
+
+	Q[0] = 0.1;
+	Q[1] = 0.2;
+	Q[2] = 0.3;
+	Q[3] = 0.4;
+	Q[4] = 0.5;
+	Q[5] = 0.6;
+	QDot[0] = 1.1;
+	QDot[1] = 1.2;
+	QDot[2] = 1.3;
+	QDot[3] = -1.4;
+	QDot[4] = -1.5;
+	QDot[5] = -1.6;
+
+	unsigned int contact_body_id = base_body_id;
+	Vector3d contact_point ( 0., -1., 0.);
+
+	ContactInfo ground_x (contact_body_id, contact_point, Vector3d (1., 0., 0.));
+	ContactInfo ground_y (contact_body_id, contact_point, Vector3d (0., 1., 0.));
+	ContactInfo ground_z (contact_body_id, contact_point, Vector3d (0., 0., 1.));
+
+	std::vector<ContactInfo> contact_data;
+
+	contact_data.push_back (ground_x);
+	contact_data.push_back (ground_y);
+	contact_data.push_back (ground_z);
+
+	ClearLogOutput();
+
+	ForwardDynamicsContactsLagrangian (model, Q, QDot, Tau, contact_data, QDDot);
+
+	Vector3d point_acceleration = CalcPointAcceleration (model, Q, QDot, QDDot, contact_body_id, contact_point);
+
+	CHECK_ARRAY_CLOSE (
+			Vector3d (0., 0., 0.).data(),
+			point_acceleration.data(),
+			3,
+			TEST_PREC
+			);
+
+	// cout << "LagrangianSimple Logoutput Start" << endl;
+	// cout << LogOutput.str() << endl;
+	// cout << "LagrangianSimple Logoutput End" << endl;
+
+	/*
+	unsigned int i;
+	for (i = 0; i < contact_data.size(); i++) {
+		cout << "cf[" << i << "] = " << contact_data[i].force << endl;
+	}
+
+	cout << QDDot << endl;
+	*/
+}

File tests/TwolegModelTests.cc

 using namespace RigidBodyDynamics;
 using namespace RigidBodyDynamics::Experimental;
 
-const double TEST_PREC = 1.0e-14;
+const double TEST_PREC = 1.0e-13;
 
 Model *model = NULL;
 
 	temp_id = model->AddBody (temp_id, Xtrans (joint_location[JointLocationAnkle]), joint_rot_z, foot_left_body);
 	foot_left_id = temp_id;
 
-	cerr << "--- model created (" << model->dof_count << " DOF) ---" << endl;
+//	cerr << "--- model created (" << model->dof_count << " DOF) ---" << endl;
 
 	// contact data
 
 	ForwardDynamics (*model, Q, QDot, Tau, QDDot_aba);
 	ForwardDynamicsLagrangian (*model, Q, QDot, Tau, QDDot_lag);
 
-	cout << "QDDot_aba = " << QDDot_aba.transpose() << endl;
-	cout << "QDDot_lag = " << QDDot_lag.transpose() << endl;
+//	cout << "QDDot_aba = " << QDDot_aba.transpose() << endl;
+//	cout << "QDDot_lag = " << QDDot_lag.transpose() << endl;
+
+	unsigned int body_id = contact_data_left[0].body_id;
+	Vector3d contact_point = contact_data_left[0].point;
+
+	MatrixNd G (3, Q.size());
+	CalcPointJacobian (*model, Q, body_id, contact_point, G, true);
+
+//	cout << G << endl;
 
 	ClearLogOutput();
 
 	ForwardDynamicsContactsLagrangian (*model, Q, QDot, Tau, contact_data_left, QDDot);
 
-	cout << "C0: " << contact_data_left[0].body_id << ", " << contact_data_left[0].point.transpose() << endl;
-	cout << "C1: " << contact_data_left[1].body_id << ", " << contact_data_left[1].point.transpose() << endl;
-	cout << "td: " << foot_left_id << ", " << heel_point.transpose() << endl;
+//	cout << "C0: " << contact_data_left[0].body_id << ", " << contact_data_left[0].point.transpose() << endl;
+//	cout << "C1: " << contact_data_left[1].body_id << ", " << contact_data_left[1].point.transpose() << endl;
+//	cout << "td: " << foot_left_id << ", " << heel_point.transpose() << endl;
 
 	contact_force[0] = contact_data_left[0].force;
 	contact_force[1] = contact_data_left[1].force;
 
-	cout << LogOutput.str() << endl;
+	CHECK_EQUAL (body_id, foot_left_id);
+	CHECK_EQUAL (contact_point, heel_point);
+
+//	cout << LogOutput.str() << endl;
 	contact_accel_left = CalcPointAcceleration (*model, Q, QDot, QDDot, foot_left_id, heel_point);
 	contact_vel_left = CalcPointVelocity (*model, Q, QDot, foot_left_id, heel_point);
-	cout << contact_force << endl;
-	cout << contact_accel_left << endl;
+//	cout << contact_force << endl;
+//	cout << contact_accel_left << endl;
 
 	CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), contact_accel_left.data(), 3, TEST_PREC);