1. Eric Cousineau
  2. rbdl

Commits

Martin Felis  committed 7afe0cf

removal of deprecated ContactInfo completed

  • Participants
  • Parent commits 6046662
  • Branches dev

Comments (0)

Files changed (7)

File src/Contacts.cc

View file
 	}
 }
 
-void ForwardDynamicsContactsOld (
-		Model &model,
-		const VectorNd &Q,
-		const VectorNd &QDot,
-		const VectorNd &Tau,
-		ConstraintSet &CS,
-		VectorNd &QDDot
-		) {
-	LOG << "-------- " << __func__ << " ------" << std::endl;
-
-//	LOG << "Q    = " << Q.transpose() << std::endl;
-//	LOG << "QDot = " << QDot.transpose() << std::endl;
-
-//	assert (ContactData.size() == 1);
-	Vector3d point_accel_t;
-
-	unsigned int ci = 0;
-	
-	// The default acceleration only needs to be computed once
-	{
-		SUPPRESS_LOGGING;
-		ForwardDynamics (model, Q, QDot, Tau, CS.QDDot_0);
-	}
-
-	// The vector f_ext_constraints might contain some values from previous
-	// computations so we need to reset it.
-	for (unsigned int fi = 0; fi < CS.f_ext_constraints.size(); fi++) {
-			CS.f_ext_constraints[fi].setZero();
-		}
-
-	// we have to compute the standard accelerations first as we use them to
-	// compute the effects of each test force
-	LOG << "=== Initial Loop Start ===" << std::endl;
-	for (ci = 0; ci < CS.size(); ci++) {
-		unsigned int body_id = CS.body[ci];
-		Vector3d point = CS.point[ci];
-		Vector3d normal = CS.normal[ci];
-		double acceleration = CS.constraint_acceleration[ci];
-
-		{
-			SUPPRESS_LOGGING;
-			UpdateKinematicsCustom (model, NULL, NULL, &CS.QDDot_0);
-			CS.point_accel_0[ci] = CalcPointAcceleration (model, Q, QDot, CS.QDDot_0, body_id, point, false);
-
-			CS.a[ci] = acceleration - normal.dot(CS.point_accel_0[ci]);
-		}
-		LOG << "point_accel_0 = " << CS.point_accel_0[ci].transpose();
-	}
-
-	// Now we can compute and apply the test forces and use their net effect
-	// to compute the inverse articlated inertia to fill K.
-	for (ci = 0; ci < CS.size(); ci++) {
-		LOG << "=== Testforce Loop Start ===" << std::endl;
-		unsigned int body_id = CS.body[ci];
-		Vector3d point = CS.point[ci];
-		Vector3d normal = CS.normal[ci];
-		double acceleration = CS.constraint_acceleration[ci];
-
-		// assemble the test force
-		LOG << "normal = " << normal.transpose() << std::endl;
-
-		Vector3d point_global = CalcBodyToBaseCoordinates (model, Q, body_id, point, false);
-		LOG << "point_global = " << point_global.transpose() << std::endl;
-
-		CS.f_t[ci].set (0., 0., 0., -normal[0], -normal[1], -normal[2]);
-		CS.f_t[ci] = spatial_adjoint(Xtrans_mat(-point_global)) * CS.f_t[ci];
-		CS.f_ext_constraints[body_id] = CS.f_t[ci];
-		LOG << "f_t[" << body_id << "] = " << CS.f_t[ci].transpose() << std::endl;
-
-		{
-//			SUPPRESS_LOGGING;
-//			ForwardDynamicsAccelerationsOnly (model, QDDot_t, &f_ext_constraints);
-			ForwardDynamics (model, Q, QDot, Tau, CS.QDDot_t, &CS.f_ext_constraints);
-			LOG << "QDDot_0 = " << CS.QDDot_0.transpose() << std::endl;
-			LOG << "QDDot_t = " << CS.QDDot_t.transpose() << std::endl;
-			LOG << "QDDot_t - QDDot_0= " << (CS.QDDot_t - CS.QDDot_0).transpose() << std::endl;
-		}
-		CS.f_ext_constraints[body_id].setZero();
-
-		// compute the resulting acceleration
-		{
-			SUPPRESS_LOGGING;
-			UpdateKinematicsCustom (model, NULL, NULL, &CS.QDDot_t);
-		}
-
-		for (unsigned int cj = 0; cj < CS.size(); cj++) {
-			{
-				SUPPRESS_LOGGING;
-
-				point_accel_t = CalcPointAcceleration (model, Q, QDot, CS.QDDot_t, CS.body[cj], CS.point[cj], false);
-			}
-
-			LOG << "point_accel_0  = " << CS.point_accel_0[ci].transpose() << std::endl;
-			CS.K(ci,cj) = CS.normal[cj].dot(point_accel_t - CS.point_accel_0[cj]);
-			LOG << "point_accel_t = " << point_accel_t.transpose() << std::endl;
-		}
-	}
-
-	LOG << "K = " << std::endl << CS.K << std::endl;
-	LOG << "a = " << std::endl << CS.a << std::endl;
-
-#ifndef RBDL_USE_SIMPLE_MATH
-	switch (CS.linear_solver) {
-		case (ConstraintSet::LinearSolverPartialPivLU) :
-			CS.constraint_force = CS.K.partialPivLu().solve(CS.a);
-			break;
-		case (ConstraintSet::LinearSolverColPivHouseholderQR) :
-			CS.constraint_force = CS.K.colPivHouseholderQr().solve(CS.a);
-			break;
-		default:
-			LOG << "Error: Invalid linear solver: " << CS.linear_solver << std::endl;
-			assert (0);
-			break;
-	}
-#else
-	bool solve_successful = LinSolveGaussElimPivot (CS.K, CS.a, CS.constraint_force);
-	assert (solve_successful);
-#endif
-
-	LOG << "f = " << CS.constraint_force.transpose() << std::endl;
-
-	for (unsigned int i = 0; i < CS.f_ext_constraints.size(); i++) {
-		CS.f_ext_constraints[i].setZero();
-	}
-
-	for (ci = 0; ci < CS.size(); ci++) {
-		unsigned int body_id = CS.body[ci];
-
-		CS.f_ext_constraints[body_id] += CS.f_t[ci] * CS.constraint_force[ci]; 
-		LOG << "f_ext[" << body_id << "] = " << CS.f_ext_constraints[body_id].transpose() << std::endl;
-	}
-
-	{
-		SUPPRESS_LOGGING;
-//		ForwardDynamicsAccelerationsOnly (model, QDDot, &f_ext_constraints);
-		ForwardDynamics (model, Q, QDot, Tau, QDDot, &CS.f_ext_constraints);
-	}
-}
-
 /** \brief Computes the effect of external forces on the generalized accelerations.
  *
  * This function is essentially similar to ForwardDynamics() except that it

File src/Contacts.h

View file
 			const char *constraint_name = NULL,
 			double acceleration = 0.);
 
+	/** \brief Copies the constraints and resets its ConstraintSet::bound
+	 * flag.
+	 */
+	ConstraintSet Copy() {
+		ConstraintSet result (*this);
+		result.bound = false;
+
+		return result;
+	}
+
 	/** \brief Specifies which method should be used for solving undelying linear systems.
 	 */
 	void SetSolver (LinearSolver solver) {

File tests/ContactsTests.cc

View file
 
 const double TEST_PREC = 1.0e-11;
 
-static void copy_contact_info (ConstraintSet &CS, std::vector<ContactInfo> &contact_info) {
-	for (unsigned int i = 0; i < contact_info.size(); i++) {
-		CS.AddConstraint(
-				contact_info[i].body_id,
-				contact_info[i].point,
-				contact_info[i].normal,
-				NULL,
-				contact_info[i].acceleration
-				);
-	}
-}
-
-static VectorNd copy_constraint_force (std::vector<ContactInfo> &contact_data) {
-	VectorNd result = VectorNd::Zero (contact_data.size());
-	for (unsigned int i = 0; i < contact_data.size(); i++) {
-		result[i] = contact_data[i].force;
-	}
-
-	return result;
-}
-
 struct FixedBase6DoF9DoF {
 	FixedBase6DoF9DoF () {
 		ClearLogOutput();
 	unsigned int contact_body_id;
 	Vector3d contact_point;
 	Vector3d contact_normal;
-	std::vector<ContactInfo> contact_data;
 	ConstraintSet constraint_set;
 };
 
 	unsigned int contact_body_id;
 	Vector3d contact_point;
 	Vector3d contact_normal;
-	std::vector<ContactInfo> contact_data;
 	ConstraintSet constraint_set;
 };
 
 	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.));
+	ConstraintSet constraint_set;
 
-	std::vector<ContactInfo> contact_data;
+	constraint_set.AddConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), "ground_x");
+	constraint_set.AddConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.), "ground_y");
+	constraint_set.AddConstraint (contact_body_id, contact_point, Vector3d (0., 0., 1.), "ground_z");
 
-	contact_data.push_back (ground_x);
-	contact_data.push_back (ground_y);
-	contact_data.push_back (ground_z);
-
-	ConstraintSet constraint_set;
-	copy_contact_info (constraint_set, contact_data);
 	constraint_set.Bind (model);
 
 	ClearLogOutput();
 	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.));
+	ConstraintSet constraint_set;
 
-	std::vector<ContactInfo> contact_data;
+	constraint_set.AddConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), "ground_x");
+	constraint_set.AddConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.), "ground_y");
+	constraint_set.AddConstraint (contact_body_id, contact_point, Vector3d (0., 0., 1.), "ground_z");
 
-	contact_data.push_back (ground_x);
-	contact_data.push_back (ground_y);
-	contact_data.push_back (ground_z);
-
-	ConstraintSet constraint_set;
-	copy_contact_info (constraint_set, contact_data);
 	constraint_set.Bind (model);
 
 	ClearLogOutput();
 }
  
 // 
-// ForwardDynamicsContactsOld
-// 
-TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOldSingleContact) {
-	contact_normal.set (0., 1., 0.);
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, contact_normal, 0.));
-
-	Vector3d point_accel_lagrangian;
-	double contact_force_lagrangian;
-
-	ConstraintSet constraint_set;
-	copy_contact_info (constraint_set, contact_data);
-	constraint_set.Bind (*model);
-		
-	ForwardDynamicsContactsLagrangian (*model, Q, QDot, Tau, constraint_set, QDDot);
-	point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point);
-	
-	ClearLogOutput();
-	ForwardDynamicsContactsOld (*model, Q, QDot, Tau, contact_data, QDDot);
-//	cout << LogOutput.str() << endl;
-
-	Vector3d point_accel_recursive;
-	double contact_force_recursive;
-	point_accel_recursive = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point, true);
-	contact_force_recursive = contact_data[0].force;
-
-	VectorNd contact_force = copy_constraint_force (contact_data);
-	CHECK_ARRAY_CLOSE (constraint_set.constraint_force.data(), contact_force.data(), contact_data.size() , TEST_PREC);
-
-	CHECK_CLOSE (0., contact_normal.dot(point_accel_recursive), TEST_PREC);
-	CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_recursive.data(), 3, TEST_PREC);
-}
-
-TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOldSingleContactRotated) {
-	contact_normal.set (0., 1., 0.);
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, contact_normal, 0.));
-
-	Q[0] = 0.6;
-	Q[3] =   M_PI * 0.6;
-
-	Vector3d point_accel_lagrangian;
-	double contact_force_lagrangian;
-
-	ConstraintSet constraint_set;
-	copy_contact_info (constraint_set, contact_data);
-	constraint_set.Bind (*model);
-		
-	ForwardDynamicsContactsLagrangian (*model, Q, QDot, Tau, constraint_set, QDDot);
-	point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point);
-	contact_force_lagrangian = contact_data[0].force;
-		
-	ClearLogOutput();
-	ForwardDynamicsContactsOld (*model, Q, QDot, Tau, contact_data, QDDot);
-//	cout << LogOutput.str() << endl;
-
-	Vector3d point_accel_recursive;
-	double contact_force_recursive;
-	point_accel_recursive = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point);
-	contact_force_recursive = contact_data[0].force;
-
-	CHECK_CLOSE (0., contact_normal.dot(point_accel_recursive), TEST_PREC);
-	CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_recursive.data(), 3, TEST_PREC);
-	VectorNd contact_force = copy_constraint_force (contact_data);
-	CHECK_ARRAY_CLOSE (constraint_set.constraint_force.data(), contact_force.data(), contact_data.size() , TEST_PREC);
-}
-
-TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOldMultipleContact) {
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (1., 0., 0.), 0.));
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (0., 1., 0.), 0.));
-
-	std::vector<ContactInfo> contact_data_lagrangian (contact_data);
-	
-
-	// we rotate the joints so that we have full mobility at the contact
-	// point:
-	//
-	//  O       X (contact point)
-	//   \     /
-	//    \   /
-	//     \ /
-	//      *      
-	//
-
-	Q[0] = M_PI * 0.25;
-	Q[3] = M_PI * 0.5;
-
-	ConstraintSet constraint_set_lagrangian;
-	copy_contact_info (constraint_set_lagrangian, contact_data);
-	constraint_set_lagrangian.Bind (*model);
-	
-	ClearLogOutput();
-	ForwardDynamicsContactsOld (*model, Q, QDot, Tau, contact_data, QDDot);
-//	cout << LogOutput.str() << endl;
-
-	Vector3d point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point);
-//	cout << "point_accel_c = " << point_accel_c.transpose() << endl;
-
-	ForwardDynamicsContactsLagrangian (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot);
-//	cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << endl;
-
-	VectorNd contact_force = copy_constraint_force (contact_data);
-	CHECK_ARRAY_CLOSE (constraint_set_lagrangian.constraint_force.data(), contact_force.data(), contact_data.size() , TEST_PREC);
-
-	CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
-	CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
-}
-
-TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOldMultipleContactRotating) {
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (1., 0., 0.), 0.));
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (0., 1., 0.), 0.));
-
-	// we rotate the joints so that we have full mobility at the contact
-	// point:
-	//
-	//  O       X (contact point)
-	//   \     /
-	//    \   /
-	//     \ /
-	//      *      
-	//
-
-	Q[0] = M_PI * 0.25;
-	Q[3] = M_PI * 0.5;
-	QDot[0] = 2.;
-	
-	ConstraintSet constraint_set_lagrangian;
-	copy_contact_info (constraint_set_lagrangian, contact_data);
-	constraint_set_lagrangian.Bind (*model);
-
-	ClearLogOutput();
-	ForwardDynamicsContactsOld (*model, Q, QDot, Tau, contact_data, QDDot);
-//	cout << LogOutput.str() << endl;
-
-	Vector3d point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point);
-//	cout << "point_accel_c = " << point_accel_c.transpose() << endl;
-
-	ForwardDynamicsContactsLagrangian (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot);
-//	cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << endl;
-
-	VectorNd contact_force = copy_constraint_force (contact_data);
-	CHECK_ARRAY_CLOSE (constraint_set_lagrangian.constraint_force.data(), contact_force.data(), contact_data.size() , TEST_PREC);
-
-	CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
-	CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
-}
-
-// 
-// Compares the results of 
-//   - ForwardDynamicsContactsLagrangian
-//   - ForwardDynamcsContacts
-// for the example model in FixedBase6DoF and a moving state (i.e. a
-// nonzero QDot)
-// 
-TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOldSingleContactRotatedMoving) {
-	Q[0] = 0.6;
-	Q[3] =   M_PI * 0.6;
-	Q[4] = 0.1;
-
-	QDot[1] = 0.1;
-	QDot[2] = -0.5;
-	QDot[3] = 0.8;
-
-	contact_normal.set (0., 1., 0.);
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, contact_normal, 0.));
-
-	ConstraintSet constraint_set_lagrangian;
-	copy_contact_info (constraint_set_lagrangian, contact_data);
-	constraint_set_lagrangian.Bind (*model);
-
-	Vector3d point_accel_lagrangian, point_accel_contacts;
-	double contact_force_lagrangian, contact_force_contacts;
-	
-	VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.);
-	VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.);
-	
-	ForwardDynamicsContactsLagrangian (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
-	ClearLogOutput();
-	ForwardDynamicsContactsOld (*model, Q, QDot, Tau, contact_data, QDDot_contacts);
-//	cout << LogOutput.str() << endl;
-
-	point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
-	point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
-
-	CHECK_CLOSE (constraint_set_lagrangian.constraint_force[0], contact_data[0].force, TEST_PREC);
-	CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC);
-	CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC);
-	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
-}
-
-TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOldMultipleContactsMultipleBodies) {
-	// we have to resize the state vectors
-
-	VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.);
-
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (1., 0., 0.), 0.));
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (0., 1., 0.), 0.));
-	contact_data.push_back (ContactInfo(child_2_rot_x_id, contact_point, Vector3d (0., 1., 0.), 0.));
-
-	ConstraintSet constraint_set_lagrangian;
-	copy_contact_info (constraint_set_lagrangian, contact_data);
-	constraint_set_lagrangian.Bind (*model);
-
-	Q[0] = 0.1;
-	Q[1] = -0.1;
-	Q[2] = 0.1;
-	Q[3] = -0.1;
-	Q[4] = 0.1;
-	Q[5] = -0.1;
-
-	ClearLogOutput();
-	ForwardDynamicsContactsOld (*model, Q, QDot, Tau, contact_data, QDDot);
-//	cout << LogOutput.str() << endl;
-
-	Vector3d point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point);
-	Vector3d point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot, child_2_rot_x_id, contact_point);
-
-//	cout << "point_accel_c = " << point_accel_c.transpose() << endl;
-
-	ForwardDynamicsContactsLagrangian (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
-//	cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << endl;
-
-	VectorNd contact_force = copy_constraint_force (contact_data);
-	CHECK_ARRAY_CLOSE (constraint_set_lagrangian.constraint_force.data(), contact_force.data(), contact_data.size() , TEST_PREC);
-
-	CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
-	CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
-	CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
-
-	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot.data(), QDDot.size(), TEST_PREC);
-}
-
-TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOldMultipleContactsMultipleBodiesMoving) {
-	VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.);
-
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (1., 0., 0.), 0.));
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (0., 1., 0.), 0.));
-	contact_data.push_back (ContactInfo(child_2_rot_x_id, contact_point, Vector3d (0., 1., 0.), 0.));
-
-	std::vector<ContactInfo> contact_data_lagrangian (contact_data);
-
-	Q[0] = 0.1;
-	Q[1] = -0.1;
-	Q[2] = 0.1;
-	Q[3] = -0.1;
-	Q[4] = -0.1;
-	Q[5] = 0.1;
-
-	QDot[0] =  1.; 
-	QDot[1] = -1.;
-	QDot[2] =  1; 
-	QDot[3] = -1.5; 
-	QDot[4] =  1.5; 
-	QDot[5] = -1.5; 
-	
-	ConstraintSet constraint_set_lagrangian;
-	copy_contact_info (constraint_set_lagrangian, contact_data);
-	constraint_set_lagrangian.Bind (*model);
-
-	ClearLogOutput();
-	ForwardDynamicsContactsOld (*model, Q, QDot, Tau, contact_data, QDDot);
-//	cout << LogOutput.str() << endl;
-
-	Vector3d point_accel_c, point_accel_2_c;
-
-	point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point);
-	point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot, child_2_rot_x_id, contact_point);
-
-//	cout << "point_accel_c = " << point_accel_c.transpose() << endl;
-
-	ForwardDynamicsContactsLagrangian (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
-//	cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << ", " << contact_data_lagrangian[2].force << endl;
-
-	VectorNd contact_force = copy_constraint_force (contact_data);
-	CHECK_ARRAY_CLOSE (constraint_set_lagrangian.constraint_force.data(), contact_force.data(), contact_data.size() , TEST_PREC);
-
-	CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
-	CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
-	CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
-
-	point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point);
-	point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, child_2_rot_x_id, contact_point);
-
-	CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
-	CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
-	CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
-
-	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot.data(), QDDot.size(), TEST_PREC);
-}
-
-
-TEST_FIXTURE (FixedBase6DoF12DoFFloatingBase, ForwardDynamicsContactsOldMultipleContactsFloatingBase) {
-	VectorNd QDDot_lagrangian = VectorNd::Constant (model->dof_count, 0.);
-
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (1., 0., 0.), 0.));
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (0., 1., 0.), 0.));
-	contact_data.push_back (ContactInfo(child_2_rot_x_id, contact_point, Vector3d (0., 1., 0.), 0.));
-
-	std::vector<ContactInfo> contact_data_lagrangian (contact_data);
-
-	Q[0] = 0.1;
-	Q[1] = -0.3;
-	Q[2] = 0.15;
-	Q[3] = -0.21;
-	Q[4] = -0.81;
-	Q[5] = 0.11;
-	Q[6] = 0.31;
-	Q[7] = -0.91;
-	Q[8] = 0.61;
-
-	QDot[0] =  1.3; 
-	QDot[1] = -1.7;
-	QDot[2] =  3; 
-	QDot[3] = -2.5; 
-	QDot[4] =  1.5; 
-	QDot[5] = -5.5; 
-	QDot[6] =  2.5; 
-	QDot[7] = -1.5; 
-	QDot[8] = -3.5; 
-
-	ConstraintSet constraint_set_lagrangian;
-	copy_contact_info (constraint_set_lagrangian, contact_data);
-	constraint_set_lagrangian.Bind (*model);
-
-	ClearLogOutput();
-	ForwardDynamicsContactsOld (*model, Q, QDot, Tau, contact_data, QDDot);
-//	cout << LogOutput.str() << endl;
-
-	Vector3d point_accel_c, point_accel_2_c;
-
-	point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point);
-	point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot, child_2_rot_x_id, contact_point);
-
-//	cout << "point_accel_c = " << point_accel_c.transpose() << endl;
-
-	ClearLogOutput();
-	ForwardDynamicsContactsLagrangian (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
-//	cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << ", " << contact_data_lagrangian[2].force << endl;
-//	cout << LogOutput.str() << endl;
-
-	VectorNd contact_force = copy_constraint_force (contact_data);
-	CHECK_ARRAY_CLOSE (constraint_set_lagrangian.constraint_force.data(), contact_force.data(), contact_data.size() , TEST_PREC);
-
-	CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
-	CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
-	CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
-
-	point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point);
-	point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, child_2_rot_x_id, contact_point);
-
-	CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
-	CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
-	CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
-
-	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot.data(), QDDot.size(), TEST_PREC);
-}
-
-// 
 // ForwardDynamicsContacts
 // 
 TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContact) {
 	contact_normal.set (0., 1., 0.);
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, contact_normal, 0.));
+	constraint_set.AddConstraint (contact_body_id, contact_point, contact_normal);
+	ConstraintSet constraint_set_lagrangian = constraint_set.Copy();
 
-	ConstraintSet constraint_set_lagrangian;
-	copy_contact_info (constraint_set_lagrangian, contact_data);
-	copy_contact_info (constraint_set, contact_data);
 	constraint_set_lagrangian.Bind (*model);
 	constraint_set.Bind (*model);
 	
-	Vector3d point_accel_lagrangian, point_accel_contacts, point_accel_contacts_old;
-	double contact_force_lagrangian, contact_force_contacts, contact_force_contacts_old;
+	Vector3d point_accel_lagrangian, point_accel_contacts;
+	double contact_force_lagrangian, contact_force_contacts;
 	
 	ClearLogOutput();
 
 	VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.);
 	VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.);
-	VectorNd QDDot_contacts_old = VectorNd::Constant (model->mBodies.size() - 1, 0.);
 	
 	ClearLogOutput();
 	ForwardDynamicsContactsLagrangian (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
 	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot_contacts);
 //	cout << LogOutput.str() << endl;
 	ClearLogOutput();
-	ForwardDynamicsContactsOld (*model, Q, QDot, Tau, contact_data, QDDot_contacts_old);
 
 	point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
 	point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
-	point_accel_contacts_old = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts_old, contact_body_id, contact_point, true);
 
-	CHECK_CLOSE (constraint_set_lagrangian.constraint_force[0], contact_data[0].force, TEST_PREC);
 	CHECK_CLOSE (constraint_set_lagrangian.constraint_force[0], constraint_set.constraint_force[0], TEST_PREC);
-
 	CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC);
-	CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts_old), TEST_PREC);
-
 	CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC);
-	CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts_old.data(), 3, TEST_PREC);
-
 	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
-	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts_old.data(), QDDot_lagrangian.size(), TEST_PREC);
 }
 
 TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotated) {
 	Q[4] = 0.1;
 
 	contact_normal.set (0., 1., 0.);
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, contact_normal, 0.));
-
-	ConstraintSet constraint_set_lagrangian;
-	copy_contact_info (constraint_set_lagrangian, contact_data);
-	copy_contact_info (constraint_set, contact_data);
+	
+	constraint_set.AddConstraint (contact_body_id, contact_point, contact_normal);
+	ConstraintSet constraint_set_lagrangian = constraint_set.Copy();
+	
 	constraint_set_lagrangian.Bind (*model);
 	constraint_set.Bind (*model);
 	
 	
 	ClearLogOutput();
 	ForwardDynamicsContactsLagrangian (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
-	ForwardDynamicsContactsOld (*model, Q, QDot, Tau, contact_data, QDDot_contacts);
 	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot_contacts_opt);
 
 	point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
-	point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
 	point_accel_contacts_opt = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts_opt, contact_body_id, contact_point, true);
 
-	CHECK_CLOSE (constraint_set_lagrangian.constraint_force[0], contact_data[0].force, TEST_PREC);
 	CHECK_CLOSE (constraint_set_lagrangian.constraint_force[0], constraint_set.constraint_force[0], TEST_PREC);
-
-	CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC);
 	CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts_opt), TEST_PREC);
-
-	CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC);
 	CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts_opt.data(), 3, TEST_PREC);
-
-	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
-	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts_opt.data(), QDDot_lagrangian.size(), TEST_PREC);}
+	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts_opt.data(), QDDot_lagrangian.size(), TEST_PREC);
+}
 
 // 
 // Similiar to the previous test, this test compares the results of 
 	QDot[3] = 0.8;
 
 	contact_normal.set (0., 1., 0.);
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, contact_normal, 0.));
-
-	ConstraintSet constraint_set_lagrangian;
-	copy_contact_info (constraint_set_lagrangian, contact_data);
-	copy_contact_info (constraint_set, contact_data);
+	constraint_set.AddConstraint (contact_body_id, contact_point, contact_normal);
+	ConstraintSet constraint_set_lagrangian = constraint_set.Copy();
+	
 	constraint_set_lagrangian.Bind (*model);
 	constraint_set.Bind (*model);
 	
-	Vector3d point_accel_lagrangian, point_accel_contacts_opt;
-	double contact_force_lagrangian, contact_force_contacts_opt;
+	Vector3d point_accel_lagrangian, point_accel_contacts;
+	double contact_force_lagrangian, contact_force_contacts;
 	
 	VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.);
-	VectorNd QDDot_contacts_opt = VectorNd::Constant (model->mBodies.size() - 1, 0.);
+	VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.);
 	
 	ClearLogOutput();
-	ForwardDynamicsContactsOld (*model, Q, QDot, Tau, contact_data, QDDot_lagrangian);
+	ForwardDynamicsContactsLagrangian (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
 //	cout << LogOutput.str() << endl;
 	ClearLogOutput();
-	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot_contacts_opt);
+	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot_contacts);
 //	cout << LogOutput.str() << endl;
 
 	point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
-	point_accel_contacts_opt = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts_opt, contact_body_id, contact_point, true);
+	point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
 
 	// check whether FDContactsLagrangian and FDContactsOld match
-	CHECK_CLOSE (constraint_set.constraint_force[0], contact_data[0].force, TEST_PREC);
+	CHECK_CLOSE (constraint_set_lagrangian.constraint_force[0], constraint_set.constraint_force[0], TEST_PREC);
 
-	CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts_opt), TEST_PREC);
-	CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts_opt.data(), 3, TEST_PREC);
-	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts_opt.data(), QDDot_lagrangian.size(), TEST_PREC);
+	CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC);
+	CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC);
+	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
 }
 
 TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptDoubleContact) {
-	contact_data.push_back (ContactInfo(contact_body_id, Vector3d (1., 0., 0.), contact_normal, 0.));
-	contact_data.push_back (ContactInfo(contact_body_id, Vector3d (0., 1., 0.), contact_normal, 0.));
+	ConstraintSet constraint_set_lagrangian;
 
-	ConstraintSet constraint_set_lagrangian;
-	copy_contact_info (constraint_set_lagrangian, contact_data);
-	copy_contact_info (constraint_set, contact_data);
+	constraint_set.AddConstraint (contact_body_id, Vector3d (1., 0., 0.), contact_normal);
+	constraint_set.AddConstraint (contact_body_id, Vector3d (0., 1., 0.), contact_normal);
+	
+	constraint_set_lagrangian = constraint_set.Copy();
 	constraint_set_lagrangian.Bind (*model);
 	constraint_set.Bind (*model);
 	
-	Vector3d point_accel_lagrangian, point_accel_contacts, point_accel_contacts_opt;
-	double contact_force_lagrangian, contact_force_contacts, contact_force_contacts_opt;
+	Vector3d point_accel_lagrangian, point_accel_contacts;
+	double contact_force_lagrangian, contact_force_contacts;
 	
 	ClearLogOutput();
 
 	VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.);
 	VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.);
-	VectorNd QDDot_contacts_opt = VectorNd::Constant (model->mBodies.size() - 1, 0.);
 	
 	ClearLogOutput();
 
 	ForwardDynamicsContactsLagrangian (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
-	ForwardDynamicsContactsOld (*model, Q, QDot, Tau, contact_data, QDDot_contacts);
-	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot_contacts_opt);
+	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot_contacts);
 
 	point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
 	point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
-	point_accel_contacts_opt = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts_opt, contact_body_id, contact_point, true);
 
 	// check whether FDContactsLagrangian and FDContacts match
 	CHECK_ARRAY_CLOSE (
 			constraint_set.size(), TEST_PREC
 			);
 
-	// check whether FDContactsLagrangian and FDContactsOld match
-	CHECK_CLOSE (constraint_set_lagrangian.constraint_force[0], contact_data[0].force, TEST_PREC);
-	CHECK_CLOSE (constraint_set_lagrangian.constraint_force[1], contact_data[1].force, TEST_PREC);
-
 	// check whether the point accelerations match
 	CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC);
-	CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts_opt.data(), 3, TEST_PREC);
 
 	// check whether the generalized accelerations match
 	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
-	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts_opt.data(), QDDot_lagrangian.size(), TEST_PREC);
 }
 
 TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptDoubleContactRepeated) {
 	// makes sure that all variables in the constraint set gets reset
 	// properly when making repeated calls to ForwardDynamicsContacts.
-	contact_data.push_back (ContactInfo(contact_body_id, Vector3d (1., 0., 0.), contact_normal, 0.));
-	contact_data.push_back (ContactInfo(contact_body_id, Vector3d (0., 1., 0.), contact_normal, 0.));
+	ConstraintSet constraint_set_lagrangian;
 
-	ConstraintSet constraint_set_lagrangian;
-	copy_contact_info (constraint_set_lagrangian, contact_data);
-	copy_contact_info (constraint_set, contact_data);
+	constraint_set.AddConstraint (contact_body_id, Vector3d (1., 0., 0.), contact_normal);
+	constraint_set.AddConstraint (contact_body_id, Vector3d (0., 1., 0.), contact_normal);
+	
+	constraint_set_lagrangian = constraint_set.Copy();
 	constraint_set_lagrangian.Bind (*model);
 	constraint_set.Bind (*model);
 	
-	Vector3d point_accel_lagrangian, point_accel_contacts, point_accel_contacts_opt;
-	double contact_force_lagrangian, contact_force_contacts, contact_force_contacts_opt;
+	Vector3d point_accel_lagrangian, point_accel_contacts;
+	double contact_force_lagrangian, contact_force_contacts;
 	
 	ClearLogOutput();
 
 	VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.);
 	VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.);
-	VectorNd QDDot_contacts_opt = VectorNd::Constant (model->mBodies.size() - 1, 0.);
 	
 	ClearLogOutput();
 
 	ForwardDynamicsContactsLagrangian (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
-	ForwardDynamicsContactsOld (*model, Q, QDot, Tau, contact_data, QDDot_contacts);
 	// Call ForwardDynamicsContacts multiple times such that old values might
 	// be re-used and thus cause erroneus values.
-	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot_contacts_opt);
-	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot_contacts_opt);
-	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot_contacts_opt);
+	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot_contacts);
+	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot_contacts);
+	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot_contacts);
 
 	point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
 	point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
-	point_accel_contacts_opt = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts_opt, contact_body_id, contact_point, true);
 
 	// check whether FDContactsLagrangian and FDContacts match
 	CHECK_ARRAY_CLOSE (
 			constraint_set.size(), TEST_PREC
 			);
 
-	// check whether FDContactsLagrangian and FDContactsOld match
-	CHECK_CLOSE (constraint_set_lagrangian.constraint_force[0], contact_data[0].force, TEST_PREC);
-	CHECK_CLOSE (constraint_set_lagrangian.constraint_force[1], contact_data[1].force, TEST_PREC);
-
 	// check whether the point accelerations match
 	CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC);
-	CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts_opt.data(), 3, TEST_PREC);
 
 	// check whether the generalized accelerations match
 	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
-	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts_opt.data(), QDDot_lagrangian.size(), TEST_PREC);
 }
 
 TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptMultipleContact) {
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (1., 0., 0.), 0.));
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (0., 1., 0.), 0.));
+	ConstraintSet constraint_set_lagrangian;
+
+	constraint_set.AddConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.));
+	constraint_set.AddConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.));
+	
+	constraint_set_lagrangian = constraint_set.Copy();
+	constraint_set_lagrangian.Bind (*model);
+	constraint_set.Bind (*model);
 
 	// we rotate the joints so that we have full mobility at the contact
 	// point:
 	Q[1] = 0.2;
 	Q[3] = M_PI * 0.5;
 
-	ConstraintSet constraint_set_lagrangian;
-	copy_contact_info (constraint_set_lagrangian, contact_data);
-	copy_contact_info (constraint_set, contact_data);
-	constraint_set_lagrangian.Bind (*model);
-	constraint_set.Bind (*model);
-	
 	VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.);
 	VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.);
-	VectorNd QDDot_contacts_opt = VectorNd::Constant (model->mBodies.size() - 1, 0.);
 	
 	ClearLogOutput();
 	ForwardDynamicsContactsLagrangian (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
-	ForwardDynamicsContactsOld (*model, Q, QDot, Tau, contact_data, QDDot_contacts);
-	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot_contacts_opt);
+	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot_contacts);
 
 //	cout << LogOutput.str() << endl;
 
 
 //	cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << endl;
 
-	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts_opt.data(), QDDot_lagrangian.size(), TEST_PREC);
+	CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
 
 	CHECK_ARRAY_CLOSE (
 			constraint_set_lagrangian.constraint_force.data(),
 			constraint_set.size(), TEST_PREC
 			);
 
-	CHECK_CLOSE (constraint_set_lagrangian.constraint_force[0], contact_data[0].force, TEST_PREC);
-	CHECK_CLOSE (constraint_set_lagrangian.constraint_force[1], contact_data[1].force, TEST_PREC);
-
 	CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
 	CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
 }
 
 TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMoving) {
-	VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.);
+	ConstraintSet constraint_set_lagrangian;
 
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (1., 0., 0.), 0.));
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (0., 1., 0.), 0.));
-	contact_data.push_back (ContactInfo(child_2_rot_x_id, contact_point, Vector3d (0., 1., 0.), 0.));
-
+	constraint_set.AddConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.));
+	constraint_set.AddConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.));
+	constraint_set.AddConstraint (child_2_rot_x_id, contact_point, Vector3d (0., 1., 0.));
+	
+	constraint_set_lagrangian = constraint_set.Copy();
+	constraint_set_lagrangian.Bind (*model);
+	constraint_set.Bind (*model);
+	
 	Q[0] = 0.1;
 	Q[1] = -0.1;
 	Q[2] = 0.1;
 	QDot[4] =  1.5; 
 	QDot[5] = -1.5; 
 
-	ConstraintSet constraint_set_lagrangian;
-	copy_contact_info (constraint_set_lagrangian, contact_data);
-	copy_contact_info (constraint_set, contact_data);
-	constraint_set_lagrangian.Bind (*model);
-	constraint_set.Bind (*model);
-	
+	VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.);
+
 	ClearLogOutput();
 	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot);
 //	cout << LogOutput.str() << endl;
 }
 
 TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMovingAlternate) {
-	VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.);
+	ConstraintSet constraint_set_lagrangian;
 
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (1., 0., 0.), 0.));
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (0., 1., 0.), 0.));
-	contact_data.push_back (ContactInfo(child_2_rot_x_id, contact_point, Vector3d (0., 1., 0.), 0.));
+	constraint_set.AddConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.));
+	constraint_set.AddConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.));
+	constraint_set.AddConstraint (child_2_rot_x_id, contact_point, Vector3d (0., 1., 0.));
+	
+	constraint_set_lagrangian = constraint_set.Copy();
+	constraint_set_lagrangian.Bind (*model);
+	constraint_set.Bind (*model);
 
 	Q[0] = 0.1;
 	Q[1] = -0.3;
 	QDot[7] = -1.5; 
 	QDot[8] = -3.5; 
 
-	ConstraintSet constraint_set_lagrangian;
-	copy_contact_info (constraint_set_lagrangian, contact_data);
-	copy_contact_info (constraint_set, contact_data);
-	constraint_set_lagrangian.Bind (*model);
-	constraint_set.Bind (*model);
-	
+	VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.);
+
 	ClearLogOutput();
 	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot);
 //	cout << LogOutput.str() << endl;
 }
 
 TEST_FIXTURE (FixedBase6DoF12DoFFloatingBase, ForwardDynamicsContactsMultipleContactsFloatingBase) {
+	ConstraintSet constraint_set_lagrangian;
+
+	constraint_set.AddConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.));
+	constraint_set.AddConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.));
+	constraint_set.AddConstraint (child_2_rot_x_id, contact_point, Vector3d (0., 1., 0.));
+	
+	constraint_set_lagrangian = constraint_set.Copy();
+	constraint_set_lagrangian.Bind (*model);
+	constraint_set.Bind (*model);
+
 	VectorNd QDDot_lagrangian = VectorNd::Constant (model->dof_count, 0.);
 
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (1., 0., 0.), 0.));
-	contact_data.push_back (ContactInfo(contact_body_id, contact_point, Vector3d (0., 1., 0.), 0.));
-	contact_data.push_back (ContactInfo(child_2_rot_x_id, contact_point, Vector3d (0., 1., 0.), 0.));
-
 	Q[0] = 0.1;
 	Q[1] = -0.3;
 	Q[2] = 0.15;
 	QDot[7] = -1.5; 
 	QDot[8] = -3.5; 
 
-	ConstraintSet constraint_set_lagrangian;
-	copy_contact_info (constraint_set_lagrangian, contact_data);
-	copy_contact_info (constraint_set, contact_data);
-	constraint_set_lagrangian.Bind (*model);
-	constraint_set.Bind (*model);
-	
 	ClearLogOutput();
 	ForwardDynamicsContacts (*model, Q, QDot, Tau, constraint_set, QDDot);
 //	cout << LogOutput.str() << endl;

File tests/DynamicsTests.cc

View file
 	Joint joint_rot_z, joint_rot_y, joint_rot_x;
 	Joint joint_trans_z, joint_trans_y, joint_trans_x;
 
-	std::vector<ContactInfo> contact_data_right;
-	std::vector<ContactInfo> contact_data_left;
-	std::vector<ContactInfo> contact_data_both;
+	ConstraintSet CS_right;
+	ConstraintSet CS_left;
+	ConstraintSet CS_both;
 
 	model = new Model();
 
 	LOG << "--- model created (" << model->dof_count << " DOF) ---" << endl;
 	
 	// contact data
-	ContactInfo right_x (foot_right_id, Vector3d (0., 0., 0.), Vector3d (1., 0., 0.));
-	ContactInfo right_y (foot_right_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.));
-	ContactInfo right_z (foot_right_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.));
+	CS_right.AddConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (1., 0., 0.), "foot_right_x");
+	CS_right.AddConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.), "foot_right_y");
+	CS_right.AddConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.), "foot_right_z");
 
-	ContactInfo left_x (foot_left_id, Vector3d (0., 0., 0.), Vector3d (1., 0., 0.));
-	ContactInfo left_y (foot_left_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.));
-	ContactInfo left_z (foot_left_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.));
+	CS_left.AddConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (1., 0., 0.), "foot_left_x");
+	CS_left.AddConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.), "foot_left_y");
+	CS_left.AddConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.), "foot_left_z");
+	
+	CS_both.AddConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (1., 0., 0.), "foot_right_x");
+	CS_both.AddConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.), "foot_right_y");
+	CS_both.AddConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.), "foot_right_z");
+	CS_both.AddConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (1., 0., 0.), "foot_left_x");
+	CS_both.AddConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.), "foot_left_y");
+	CS_both.AddConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.), "foot_left_z");
 
-	// right contact
-	contact_data_right.push_back (right_x);
-	contact_data_right.push_back (right_y);
-	//	contact_data_right.push_back (right_z);
-
-	// left contact
-	contact_data_left.push_back (left_x);
-	contact_data_left.push_back (left_y);
-	//	contact_data_left.push_back (left_z);
-
-	// both contact
-	contact_data_both.push_back (right_x);
-	contact_data_both.push_back (right_y);
-	contact_data_both.push_back (right_z);
-
-	contact_data_both.push_back (left_x);
-	contact_data_both.push_back (left_y);
-	contact_data_both.push_back (left_z);
+	CS_right.Bind(*model);
+	CS_left.Bind(*model);
+	CS_both.Bind(*model);
 
 	VectorNd Q(model->dof_count);
 	VectorNd QDot(model->dof_count);

File tests/Fixtures.h

View file
 	unsigned int contact_body_id;
 	RigidBodyDynamics::Math::Vector3d contact_point;
 	RigidBodyDynamics::Math::Vector3d contact_normal;
-	std::vector<RigidBodyDynamics::ContactInfo> contact_data;
 	RigidBodyDynamics::ConstraintSet constraint_set;
 };
 

File tests/ImpulsesTests.cc

View file
 
 const double TEST_PREC = 1.0e-14;
 
-static void copy_contact_info (ConstraintSet &CS, std::vector<ContactInfo> &contact_info) {
-	for (unsigned int i = 0; i < contact_info.size(); i++) {
-		CS.AddConstraint(
-				contact_info[i].body_id,
-				contact_info[i].point,
-				contact_info[i].normal,
-				NULL,
-				contact_info[i].acceleration
-				);
-	}
-}
-
 struct ImpulsesFixture {
 	ImpulsesFixture () {
 		ClearLogOutput();
 	Vector3d contact_point;
 	Vector3d contact_normal;
 	ConstraintSet constraint_set;
-	std::vector<ContactInfo> contact_data;
 };
 
 TEST_FIXTURE(ImpulsesFixture, TestContactImpulse) {
-	contact_data.push_back (ContactInfo (contact_body_id, contact_point, Vector3d (1., 0., 0.), 0.));
-	contact_data.push_back (ContactInfo (contact_body_id, contact_point, Vector3d (0., 1., 0.), 0.));
-	contact_data.push_back (ContactInfo (contact_body_id, contact_point, Vector3d (0., 0., 1.), 0.));
+	constraint_set.AddConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 0.); 
+	constraint_set.AddConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 0.); 
+	constraint_set.AddConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 0.); 
+
+	constraint_set.Bind (*model);
 
 	QDot[0] = 0.1;
 	QDot[1] = -0.2;
 
 	// cout << "Point Velocity = " << point_velocity << endl;
 
-	copy_contact_info (constraint_set, contact_data);
-	constraint_set.Bind(*model);
-
 	VectorNd qdot_post (QDot.size());
 	ComputeContactImpulsesLagrangian (*model, Q, QDot, constraint_set, qdot_post);
 	// cout << LogOutput.str() << endl;
 }
 
 TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRotated) {
-	contact_data.push_back (ContactInfo (contact_body_id, contact_point, Vector3d (1., 0., 0.), 0.));
-	contact_data.push_back (ContactInfo (contact_body_id, contact_point, Vector3d (0., 1., 0.), 0.));
-	contact_data.push_back (ContactInfo (contact_body_id, contact_point, Vector3d (0., 0., 1.), 0.));
+	constraint_set.AddConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 0.); 
+	constraint_set.AddConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 0.); 
+	constraint_set.AddConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 0.); 
+
+	constraint_set.Bind (*model);
 
 	Q[0] = 0.2;
 	Q[1] = -0.5;
 	}
 
 	// cout << "Point Velocity = " << point_velocity << endl;
-	copy_contact_info (constraint_set, contact_data);
-	constraint_set.Bind(*model);
-
 	VectorNd qdot_post (QDot.size());
 	ComputeContactImpulsesLagrangian (*model, Q, QDot, constraint_set, qdot_post);
 	// cout << LogOutput.str() << endl;
 }
 
 TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRotatedCollisionVelocity) {
-	contact_data.push_back (ContactInfo (contact_body_id, contact_point, Vector3d (1., 0., 0.), 1.));
-	contact_data.push_back (ContactInfo (contact_body_id, contact_point, Vector3d (0., 1., 0.), 2.));
-	contact_data.push_back (ContactInfo (contact_body_id, contact_point, Vector3d (0., 0., 1.), 3.));
+	constraint_set.AddConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 1.); 
+	constraint_set.AddConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 2.); 
+	constraint_set.AddConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 3.); 
+
+	constraint_set.Bind (*model);
 
 	Q[0] = 0.2;
 	Q[1] = -0.5;
 	}
 
 	// cout << "Point Velocity = " << point_velocity << endl;
-	copy_contact_info (constraint_set, contact_data);
-	constraint_set.Bind(*model);
 
 	VectorNd qdot_post (QDot.size());
 	ComputeContactImpulsesLagrangian (*model, Q, QDot, constraint_set, qdot_post);

File tests/TwolegModelTests.cc

View file
 VectorNd QDDot;
 VectorNd Tau;
 
-std::vector<ContactInfo> contact_data_right;
-std::vector<ContactInfo> contact_data_left;
-std::vector<ContactInfo> contact_data_left_flat;
-std::vector<ContactInfo> contact_data_both;
-
 ConstraintSet constraint_set_right;
 ConstraintSet constraint_set_left;
 ConstraintSet constraint_set_left_flat;
 ConstraintSet constraint_set_both;
 
-static void copy_contact_info (ConstraintSet &CS, std::vector<ContactInfo> &contact_info) {
-	for (unsigned int i = 0; i < contact_info.size(); i++) {
-		CS.AddConstraint(
-				contact_info[i].body_id,
-				contact_info[i].point,
-				contact_info[i].normal,
-				NULL,
-				contact_info[i].acceleration
-				);
-	}
-}
-
 enum ParamNames {
 	ParamSteplength = 0,
 	ParamNameLast
 	// the contact points for heel and toe
 	heel_point.set (-0.05, -0.0317, 0.);
 	medial_point.set  (-0.05, -0.0317 + segment_lengths[SegmentLengthsFoot], 0.);
-	
-	ContactInfo right_heel_x (foot_right_id, heel_point, Vector3d (1., 0., 0.));
-	ContactInfo right_heel_y (foot_right_id, heel_point, Vector3d (0., 1., 0.));
-	ContactInfo right_heel_z (foot_right_id, heel_point, Vector3d (0., 0., 1.));
 
-	ContactInfo left_heel_x (foot_left_id, heel_point, Vector3d (1., 0., 0.));
-	ContactInfo left_heel_y (foot_left_id, heel_point, Vector3d (0., 1., 0.));
-	ContactInfo left_heel_z (foot_left_id, heel_point, Vector3d (0., 0., 1.));
+	constraint_set_right.AddConstraint(foot_right_id, heel_point, Vector3d (1., 0., 0.), "right_heel_x");
+	constraint_set_right.AddConstraint(foot_right_id, heel_point, Vector3d (0., 1., 0.), "right_heel_y");
 
-	ContactInfo right_medial_x (foot_right_id, medial_point, Vector3d (1., 0., 0.));
-	ContactInfo right_medial_y (foot_right_id, medial_point, Vector3d (0., 1., 0.));
-	ContactInfo right_medial_z (foot_right_id, medial_point, Vector3d (0., 0., 1.));
+	constraint_set_left.AddConstraint(foot_left_id, heel_point, Vector3d (1., 0., 0.), "left_heel_x");
+	constraint_set_left.AddConstraint(foot_left_id, heel_point, Vector3d (0., 1., 0.), "left_heel_y");
 
-	ContactInfo left_medial_x (foot_left_id, medial_point, Vector3d (1., 0., 0.));
-	ContactInfo left_medial_y (foot_left_id, medial_point, Vector3d (0., 1., 0.));
-	ContactInfo left_medial_z (foot_left_id, medial_point, Vector3d (0., 0., 1.));
+	constraint_set_both.AddConstraint(foot_right_id, heel_point, Vector3d (1., 0., 0.), "right_heel_x");
+	constraint_set_both.AddConstraint(foot_right_id, heel_point, Vector3d (0., 1., 0.), "right_heel_y");
+	constraint_set_both.AddConstraint(foot_right_id, heel_point, Vector3d (0., 0., 1.), "right_heel_z");
 
-	// right contact
-	contact_data_right.push_back (right_heel_x);
-	contact_data_right.push_back (right_heel_y);
-//	contact_data_right.push_back (right_heel_z);
-
-	// left contact
-	contact_data_left.push_back (left_heel_x);
-	contact_data_left.push_back (left_heel_y);
-//	contact_data_left.push_back (left_heel_z);
-
-	// left flat
-	contact_data_left_flat.push_back (left_heel_x);
-	contact_data_left_flat.push_back (left_heel_y);
-//	contact_data_left_flat.push_back (left_medial_y);
-
-	// both contact
-	contact_data_both.push_back (right_heel_x);
-	contact_data_both.push_back (right_heel_y);
-	contact_data_both.push_back (right_heel_z);
-
-	contact_data_both.push_back (left_heel_x);
-	contact_data_both.push_back (left_heel_y);
-	contact_data_both.push_back (left_heel_z);
-
-	copy_contact_info (constraint_set_right, contact_data_right);
-	copy_contact_info (constraint_set_left, contact_data_left);
-	copy_contact_info (constraint_set_both, contact_data_both);
+	constraint_set_both.AddConstraint(foot_left_id, heel_point, Vector3d (1., 0., 0.), "left_heel_x");
+	constraint_set_both.AddConstraint(foot_left_id, heel_point, Vector3d (0., 1., 0.), "left_heel_y");
+	constraint_set_both.AddConstraint(foot_left_id, heel_point, Vector3d (0., 0., 1.), "left_heel_z");
 
 	constraint_set_right.Bind (*model);
 	constraint_set_left.Bind (*model);
 //	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;
+	unsigned int body_id = constraint_set_left.body[0];
+	Vector3d contact_point = constraint_set_left.point[0];
 
 	MatrixNd G (3, Q.size());
 	CalcPointJacobian (*model, Q, body_id, contact_point, G, true);
 //	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;
+	contact_force[0] = constraint_set_left.constraint_force[0];
+	contact_force[1] = constraint_set_left.constraint_force[1];
 
 	CHECK_EQUAL (body_id, foot_left_id);
 	CHECK_EQUAL (contact_point, heel_point);