Commits

Martin Felis committed 80e32fe

removed unneeded code

Comments (0)

Files changed (1)

 	LOG << "apply qddot = " << QDDot << std::endl;
 }
 
-/*
-void ComputeContactImpulses (
-		Model &model,
-		const VectorNd &Q,
-		const VectorNd &QDotPre,
-		const std::vector<ContactInfo> &ContactData,
-		VectorNd &QDotPost
-		) {
-	std::vector<ContactInfo> ContactImpulseInfo;
-	VectorNd QDotZero (QDotPre.size());
-	QDotZero.setZero();
-	ContactInfo contact_info;
-	Vector3d point_velocity;
-
-	unsigned int i;
-	for (i = 0; i < ContactData.size(); i++) {
-		contact_info = ContactData[i];
-		CalcPointVelocity (model, Q, QDotPre, contact_info.body_id, contact_info.point, point_velocity);
-
-		Vector3d point_acceleration;
-		{
-			SUPPRESS_LOGGING;
-			CalcPointAcceleration (model, Q, QDotPre, QDotPre, contact_info.body_id, contact_info.point, point_acceleration);
-		}
-		LOG << "point_acceleration = " << point_acceleration << std::endl;
-
-		ContactInfo x_velocity_info (contact_info);
-		x_velocity_info.normal.set (1., 0., 0.);
-		x_velocity_info.acceleration = 0. - point_velocity[0];
-		ContactImpulseInfo.push_back (x_velocity_info);
-
-		ContactInfo y_velocity_info (contact_info);
-		y_velocity_info.normal.set (0., 1., 0.);
-		y_velocity_info.acceleration = 0. - point_velocity[1];
-		ContactImpulseInfo.push_back (y_velocity_info);
-
-		ContactInfo z_velocity_info (contact_info);
-		z_velocity_info.normal.set (0., 0., 1.);
-		z_velocity_info.acceleration = 0. - point_velocity[2];
-		ContactImpulseInfo.push_back (z_velocity_info);
-	}
-
-	std::vector<SpatialVector> contact_f_ext;
-	VectorNd QDDotFext (QDotPre);
-	QDDotFext.setZero();
-	VectorNd Tau_zero (QDDotFext);
-
-	// for debugging
-	VectorNd QDDotZeroFext (QDotPre.size());
-	{
-		SUPPRESS_LOGGING;
-		ForwardDynamics (model, Q, QDotPre, Tau_zero, QDDotZeroFext);
-	}
-
-	contact_info = ContactData[0]; 
-	{
-		SUPPRESS_LOGGING;
-		ComputeContactForces (model, Q, QDotPre, Tau_zero, ContactImpulseInfo, contact_f_ext);
-	}
-
-	LOG << "-------- APPLY_EXT ------" << std::endl;
-	for (i = 0; i < model.f_ext.size(); i++) {
-		model.f_ext[i] = contact_f_ext[i];
-		LOG << "f_ext[" << i << "] = " << model.f_ext[i] << std::endl;
-	}
-
-	{
-		SUPPRESS_LOGGING;
-		ForwardDynamics (model, Q, QDotPre, Tau_zero, QDDotFext);
-	}
-	LOG << "QDotPre       = " << QDotPre << std::endl;
-	LOG << "QDDotZeroFext = " << QDDotZeroFext << std::endl;
-	LOG << "QDDotFext     = " << QDDotFext << std::endl;
-
-	VectorNd humans_impulse (QDotPre.size());
-	humans_impulse[0] = 0.;
-	humans_impulse[1] = -0.05;
-	humans_impulse[2] = 0.1;
-	humans_impulse[3] = 0.;
-	humans_impulse[4] = -0.15;
-	humans_impulse[5] = -0.15;
-
-	humans_impulse = humans_impulse - QDotPre;
-	LOG << "humans impulse= " << humans_impulse << std::endl;
-
-	Vector3d point_accel;
-	{
-		SUPPRESS_LOGGING;
-		CalcPointAcceleration (model, Q, QDotPre, QDDotFext, contact_info.body_id, contact_info.point, point_accel);
-	}
-	LOG << "Point Accel = " << point_accel << std::endl;
-	{
-		SUPPRESS_LOGGING;
-		CalcPointAcceleration (model, Q, QDotPre, humans_impulse, contact_info.body_id, contact_info.point, point_accel);
-	}
-	LOG << "humans Point Accel = " << point_accel << std::endl;
-
-	{
-		SUPPRESS_LOGGING;
-		CalcPointVelocity (model, Q, QDDotFext, contact_info.body_id, contact_info.point, point_velocity);
-	}
-	LOG << "Point Veloc = " << point_velocity << std::endl;
-
-	{
-		SUPPRESS_LOGGING;
-		CalcPointVelocity (model, Q, humans_impulse, contact_info.body_id, contact_info.point, point_velocity);
-	}
-	LOG << "humans Point Veloc = " << point_velocity << std::endl;
-
-	LOG << "------ SCND ITER ------" << std::endl;
-	LOG << "QDotPre = " << QDotPre << std::endl;
-
-	{
-		SUPPRESS_LOGGING;
-		CalcPointVelocity (model, Q, QDotPre, contact_info.body_id, contact_info.point, point_velocity);
-	}
-	LOG << "QDotPre Vel = " << point_velocity << std::endl;
-	// Reset f_ext
-	for (i = 0; i < model.f_ext.size(); i++) {
-		model.f_ext[i] = SpatialVector (0., 0., 0., 0., 0., 0.);
-	}
-
-	ContactImpulseInfo.clear();
-
-	for (i = 0; i < ContactData.size(); i++) {
-		ContactInfo contact_info (ContactData[i]);
-		Vector3d point_velocity_impulse;
-		{
-			SUPPRESS_LOGGING;
-			CalcPointVelocity (model, Q, QDotPre, contact_info.body_id, contact_info.point, point_velocity);
-			CalcPointVelocity (model, Q, QDDotFext, contact_info.body_id, contact_info.point, point_velocity_impulse);
-		}
-
-		LOG << "pv = " << point_velocity << std::endl;
-		LOG << "pv_imp = " << point_velocity_impulse << std::endl;
-		LOG << "accdest = " << (point_velocity) * (-2) - point_velocity_impulse << std::endl;
-
-		ContactInfo x_velocity_info (contact_info);
-		x_velocity_info.normal.set (1., 0., 0.);
-		x_velocity_info.acceleration = - point_velocity[0] * 2  - point_velocity_impulse[0];
-		ContactImpulseInfo.push_back (x_velocity_info);
-
-		ContactInfo y_velocity_info (contact_info);
-		y_velocity_info.normal.set (0., 1., 0.);
-		y_velocity_info.acceleration = - point_velocity[1] * 2 - point_velocity_impulse[1];
-		ContactImpulseInfo.push_back (y_velocity_info);
-
-		ContactInfo z_velocity_info (contact_info);
-		z_velocity_info.normal.set (0., 0., 1.);
-		z_velocity_info.acceleration = - point_velocity[2] * 2 - point_velocity_impulse[2];
-		ContactImpulseInfo.push_back (z_velocity_info);
-	}
-
-	{
-		SUPPRESS_LOGGING;
-		ComputeContactForces (model, Q, QDotPre, Tau_zero, ContactImpulseInfo, contact_f_ext);
-	}
-	
-	LOG << "-------- APPLY_EXT ------" << std::endl;
-	for (i = 0; i < model.f_ext.size(); i++) {
-		model.f_ext[i] = contact_f_ext[i];
-		LOG << "f_ext[" << i << "] = " << model.f_ext[i] << std::endl;
-	}
-
-	// Compute the joint space acceleration
-	{
-		SUPPRESS_LOGGING;
-		ForwardDynamics (model, Q, QDotPre, Tau_zero, QDDotFext);
-	}
-
-	// Compute the cartesian acceleration
-	{
-		SUPPRESS_LOGGING;
-		CalcPointAcceleration (model, Q, QDotPre, QDDotFext, contact_info.body_id, contact_info.point, point_accel);
-	}
-
-	// Compute the cartesian velocity
-	{
-		SUPPRESS_LOGGING;
-		CalcPointVelocity (model, Q, QDDotFext, contact_info.body_id, contact_info.point, point_velocity);
-	}
-
-	LOG << "QDDotFext2 = " << QDDotFext << std::endl;
-	LOG << "accelera 2 = " << point_accel << std::endl;
-	LOG << "velo     2 = " << point_velocity << std::endl;
-	QDotPost = QDotPre + QDDotFext;
-}
-*/
 } /* namespace Experimental */
 
 } /* namespace RigidBodyDynamics */