Commits

Martin Felis committed 58f43fd

wip: contact for floating base models

  • Participants
  • Parent commits e3b6522

Comments (0)

Files changed (2)

 
 		test_forces[cj] = Xtrans (contact_point_position * -1.).adjoint() * test_force;
 		LOG << "body_id         = " << contact_info.body_id << std::endl;
-		LOG << "test_force_base = " << test_forces[cj] << std::endl;
 		// LOG << "test_force_body = " << Xtrans (model.GetBodyOrigin(contact_info.body_id)).adjoint() * test_force_base << std::endl;
 
 		// apply the test force
 		model.f_ext[contact_info.body_id] = test_forces[cj];
 		cmlVector QDDot_test_ext (QDot);
 
-		LOG << "-------- TEST_EXT ------" << std::endl;
+		LOG << "-------- TEST_EXT -------" << std::endl;
+		LOG << "test_force_base = " << test_forces[cj] << std::endl;
 		{
 			SUPPRESS_LOGGING;
 			ForwardDynamics (model, Q, QDot, Tau, QDDot_test_ext);
 
 	LOG << "Ae = " << std::endl << Ae << std::endl;
 	LOG << "C0 = " << C0 << std::endl;
-	LinSolveGaussElim (Ae, C0, u);
+	LinSolveGaussElimPivot (Ae, C0, u);
 
 	LOG << "u = " << u << std::endl;
 

tests/ContactsTests.cc

 	float_model->Init();
 	float_model->gravity.set (0., -9.81, 0.);
 
-	Body base_body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.));
+	Body base_body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.));
 
 	float_model->SetFloatingBaseBody(base_body);
 
 
 	contact_data.push_back (ground_x);
 	contact_data.push_back (ground_y);
-//	contact_data.push_back (ground_z);
+	contact_data.push_back (ground_z);
+
+	Q[1] = 1.;
 
 	// We want the body to rotate around its contact point which is located
 	// at (0, 0, 0). There it should have a negative unit rotation around the
 
 	// This has now to be shuffled such that it complies with the ordering of
 	// the DoF in the generalized velocity vector.
-	QDot[0] = velocity_body[5];
+	QDot[0] = velocity_body[3];
 	QDot[1] = velocity_body[4];
-	QDot[2] = velocity_body[3];
+	QDot[2] = velocity_body[5];
 	QDot[3] = velocity_body[2];
 	QDot[4] = velocity_body[1];
 	QDot[5] = velocity_body[0];
 	cout << "velocity_body = " << velocity_body << std::endl;
 	cout << "QDot = " << QDot << std::endl;
 
+	{
+		SUPPRESS_LOGGING;
+		ForwardDynamics (*float_model, Q, QDot, Tau, QDDot);
+	}
+
+	Vector3d test_point;
+	{
+		SUPPRESS_LOGGING;
+		test_point = float_model->GetBodyPointPosition(contact_data[0].body_id, contact_data[0].point);
+	}
+	cout << "test_point = " << test_point << std::endl;
+	
+	Vector3d test_velocity;
+	{
+		SUPPRESS_LOGGING;
+		CalcPointVelocity (*float_model, Q, QDot, contact_data[0].body_id, contact_data[0].point, test_velocity);
+	}
+	cout << "test_velocity = " << test_velocity << std::endl;
+	
+	QDDot.zero();
+	QDDot[0] = 0.;
+	Vector3d test_accel;
+	{
+		SUPPRESS_LOGGING;
+		CalcPointAcceleration (*float_model, Q, QDot, QDDot, contact_data[0].body_id, contact_data[0].point, test_accel);
+	}
+
+	cout << "test_accel = " << test_accel << endl;
+
 	ForwardDynamicsContacts (*float_model, Q, QDot, Tau, contact_data, QDDot);
 
 	cout << LogOutput.str() << endl;