Commits

Martin Felis  committed 936a878 Merge

merged dev back to default

  • Participants
  • Parent commits 8a84dc2, 9a1f5d5

Comments (0)

Files changed (7)

File src/Dynamics.cc

 	assert (solve_successful);
 #endif
 
+	LOG << "x = " << std::endl << x << std::endl;
+
 	// Copy back QDDot
 	for (i = 0; i < model.dof_count; i++)
 		QDDot[i] = x[i];

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 src/SimpleMath.h

 
 	unsigned int i,j;
 	for (i = 0; i < blockrows; i++) {
-		output << "[ ";
 		for (j = 0; j < blockcols; j++) {
+			output.width(12);
 			output << block(i,j) << " ";
 		}
-		output << "]" << std::endl;
 	}
 
 	return output;
 inline std::ostream& operator<<(std::ostream& output, const Matrix<val_type> &matrix) {
 	output << std::endl;
 	for (unsigned int i = 0; i < matrix.rows(); i++) {
-		output << "[ ";
 		for (unsigned int j = 0; j < matrix.cols(); j++) {
+			output.width(12);
 			output << matrix(i,j) << " ";
 		}
-		output << "]" << std::endl;
+		output << std::endl;
 	}
 	return output;
 }

File src/main.cc

-#include <iostream>
-
-#include "featherstone.h"
-
-using namespace std;
-
-int main (int argc, char* argv[]) {
-	vec3d translation;
-	
-	translation.x = 0;
-	translation.y = -1;
-	translation.z = 0;
-
-	vec3d axis;
-	axis.x = 1.;
-	axis.y = 0.;
-	axis.z = 0.;
-	
-	Body* thigh = RootBody.CreateChild (RevoluteJoint, axis);
-	thigh->mOriginTranslation[0] = 0.;
-	thigh->mOriginTranslation[1] = 0.;
-	thigh->mOriginTranslation[2] = 0.;
-
-	Body* shank = thigh->CreateChild (RevoluteJoint, axis);
-	shank->mOriginTranslation[0] = 0.;
-	shank->mOriginTranslation[1] = 1.;
-	shank->mOriginTranslation[2] = 0.;
-
-	RootBody.Print ("Root");
-	thigh->Print ("Thigh");
-	shank->Print ("Shank");
-
-	return 0;
-}

File tests/CMakeLists.txt

 	CompositeRigidBodyTests.cc
 	ContactsTests.cc
 	ImpulsesTests.cc
+	TwolegModelTests.cc
 	)
 
 FIND_PACKAGE (UnitTest++ REQUIRED)

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

+#include <UnitTest++.h>
+
+#include <iostream>
+
+#include "mathutils.h"
+#include "Logging.h"
+
+#include "Model.h"
+#include "Contacts.h"
+#include "Dynamics.h"
+#include "Kinematics.h"
+
+using namespace std;
+using namespace SpatialAlgebra;
+using namespace RigidBodyDynamics;
+using namespace RigidBodyDynamics::Experimental;
+
+const double TEST_PREC = 1.0e-13;
+
+Model *model = NULL;
+
+unsigned int hip_id,
+		 upper_leg_right_id,
+		 lower_leg_right_id,
+		 foot_right_id,
+		 upper_leg_left_id,
+		 lower_leg_left_id,
+		 foot_left_id;
+
+Body hip_body,
+		 upper_leg_right_body,
+		 lower_leg_right_body,
+		 foot_right_body,
+		 upper_leg_left_body,
+		 lower_leg_left_body,
+		 foot_left_body;
+
+Joint joint_rot_z, joint_rot_y, joint_rot_x;
+Joint joint_trans_z, joint_trans_y, joint_trans_x;
+
+VectorNd Q;
+VectorNd QDot;
+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;
+
+enum ParamNames {
+	ParamSteplength = 0,
+	ParamNameLast
+};
+
+enum PosNames {
+	PosHipPosX,
+	PosHipPosY,
+	PosHipRotZ,
+	PosRightThighRotZ,
+	PosRightShankRotZ,
+	PosRightAnkleRotZ,
+	PosLeftThighRotZ,
+	PosLeftShankRotZ,
+	PosLeftAnkleRotZ,
+	PosNameLast
+};
+
+enum StateNames {
+	StateHipPosX,
+	StateHipPosY,
+	StateHipRotZ,
+	StateRightThighRotZ,
+	StateRightShankRotZ,
+	StateRightAnkleRotZ,
+	StateLeftThighRotZ,
+	StateLeftShankRotZ,
+	StateLeftAnkleRotZ,
+	StateHipVelX,
+	StateHipVelY,
+	StateHipRotVelZ,
+	StateRightThighRotVelZ,
+	StateRightShankRotVelZ,
+	StateRightAnkleRotVelZ,
+	StateLeftThighRotVelZ,
+	StateLeftShankRotVelZ,
+	StateLeftAnkleRotVelZ,
+	StateNameLast
+};
+
+enum ControlNames {
+	ControlRightThighRotZ,
+	ControlRightKneeRotZ,
+	ControlRightAnkleRotZ,
+	ControlLeftThighRotZ,
+	ControlLeftKneeRotZ,
+	ControlLeftAnkleRotZ,
+	ControlNameLast
+};
+
+enum SegmentLengthsNames {
+	SegmentLengthsHip = 0,
+	SegmentLengthsThigh,
+	SegmentLengthsShank,
+	SegmentLengthsFootHeight,
+	SegmentLengthsFoot,
+	SegmentLengthsNameLast
+};
+
+const double ModelMass = 73.;
+const double ModelHeight = 1.741;
+
+// absolute lengths!
+double segment_lengths[SegmentLengthsNameLast] = {
+	0.4346,
+	0.4222,
+	0.4340,
+	0.0317,
+	0.2581
+};
+
+enum JointLocations {
+	JointLocationHip = 0,
+	JointLocationKnee,
+	JointLocationAnkle,
+	JointLocationLast
+};
+
+Vector3d joint_location[JointLocationLast] = {
+	Vector3d (0., 0., 0.),
+	Vector3d (0., - 0.2425 * ModelHeight, 0.),
+	Vector3d (0., - 0.2529 * ModelHeight, 0.)
+};
+
+enum SegmentMassNames {
+	SegmentMassHip,
+	SegmentMassThigh,
+	SegmentMassShank,
+	SegmentMassFoot,
+	SegmentMassLast
+};
+
+double segment_mass[SegmentMassLast] = {
+	0.4346 * ModelMass,
+	0.1416 * ModelMass,
+	0.0433 * ModelMass,
+	0.0137 * ModelMass
+};
+
+enum COMNames {
+	COMHip,
+	COMThigh,
+	COMShank,
+	COMFoot,
+	COMNameLast
+};
+
+Vector3d com_position[COMNameLast] = {
+	Vector3d (0., 0.3469 * ModelHeight, 0.),
+	Vector3d (0., 0.2425 * ModelHeight, 0.),
+	Vector3d (0., 0.2529 * ModelHeight, 0.),
+	Vector3d (0.0182 * ModelHeight, 0., 0.)
+};
+
+enum RGyrationNames {
+	RGyrationHip,
+	RGyrationThigh,
+	RGyrationShank,
+	RGyrationFoot,
+	RGyrationLast
+};
+
+Vector3d rgyration[RGyrationLast] = {
+	Vector3d (0.1981, 0.1021, 0.1848),
+	Vector3d (0.1389, 0.0629, 0.1389),
+	Vector3d (0.1123, 0.0454, 0.1096),
+	Vector3d (0.0081, 0.0039, 0.0078)
+};
+
+Vector3d heel_point (0., 0., 0.);
+Vector3d medial_point (0., 0., 0.);
+
+void init_model () {
+	assert (model);
+
+	model->Init();
+
+	model->gravity = Vector3d (0., -9.81, 0.);
+
+	joint_rot_z = Joint (JointTypeRevolute, Vector3d (0., 0., 1.));
+	joint_rot_y = Joint (JointTypeRevolute, Vector3d (0., 1., 0.));
+	joint_rot_x = Joint (JointTypeRevolute, Vector3d (1., 0., 0.));
+
+	joint_trans_z = Joint (JointTypePrismatic, Vector3d (0., 0., 1.));
+	joint_trans_y = Joint (JointTypePrismatic, Vector3d (0., 1., 0.));
+	joint_trans_x = Joint (JointTypePrismatic, Vector3d (1., 0., 0.));
+
+	Body null_body (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
+
+	// hip
+	hip_body = Body (segment_mass[SegmentMassHip], com_position[COMHip], rgyration[RGyrationHip]);
+
+	// lateral right
+	upper_leg_right_body = Body (segment_mass[SegmentMassThigh], com_position[COMThigh], rgyration[RGyrationThigh]);
+	lower_leg_right_body = Body (segment_mass[SegmentMassShank], com_position[COMShank], rgyration[RGyrationShank]);
+	foot_right_body = Body (segment_mass[SegmentMassFoot], com_position[COMFoot], rgyration[RGyrationFoot]);
+
+	// lateral left
+	upper_leg_left_body = Body (segment_mass[SegmentMassThigh], com_position[COMThigh], rgyration[RGyrationThigh]);
+	lower_leg_left_body = Body (segment_mass[SegmentMassShank], com_position[COMShank], rgyration[RGyrationShank]);
+	foot_left_body = Body (segment_mass[SegmentMassFoot], com_position[COMFoot], rgyration[RGyrationFoot]);
+
+	// temporary value to store most recent body id
+	unsigned int temp_id;
+
+	// add hip to the model (planar, 3 DOF)
+	temp_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_trans_x, null_body);
+ 	temp_id = model->AddBody (temp_id, Xtrans (Vector3d (0., 0., 0.)), joint_trans_y, null_body);
+	hip_id = model->AddBody (temp_id, Xtrans (Vector3d (0., 0., 0.)), joint_rot_z, hip_body);
+
+	//
+	// right leg
+	//
+	
+	// add right upper leg
+	temp_id = model->AddBody (hip_id, Xtrans (Vector3d(0., 0., 0.)), joint_rot_z, upper_leg_right_body);
+	upper_leg_right_id = temp_id;
+
+	// add the right lower leg (only one DOF)
+	temp_id = model->AddBody (temp_id, Xtrans (joint_location[JointLocationKnee]), joint_rot_z, lower_leg_right_body);
+	lower_leg_right_id = temp_id;
+
+	// add the right foot (1 DOF)
+	temp_id = model->AddBody (temp_id, Xtrans (joint_location[JointLocationAnkle]), joint_rot_z, foot_right_body);
+	foot_right_id = temp_id;
+
+	//
+	// left leg
+	//
+
+	// add left upper leg
+	temp_id = model->AddBody (hip_id, Xtrans (Vector3d(0., 0., 0.)), joint_rot_z, upper_leg_left_body);
+	upper_leg_left_id = temp_id;
+
+	// add the left lower leg (only one DOF)
+	temp_id = model->AddBody (temp_id, Xtrans (joint_location[JointLocationKnee]), joint_rot_z, lower_leg_left_body);
+	lower_leg_left_id = temp_id;
+
+	// add the left foot (1 DOF)
+	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;
+
+	// contact data
+
+	// 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.));
+
+	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.));
+
+	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.));
+
+	// 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);
+}
+
+template <typename T>
+void copy_values (T *dest, const T *src, size_t count) {
+	memcpy (dest, src, count * sizeof (T));
+}
+
+TEST ( TestForwardDynamicsContactsLagrangianFootmodel ) {
+	model = new Model;
+
+	init_model();
+
+	Q.resize(model->dof_count);
+	QDot.resize(model->dof_count);
+	QDDot.resize(model->dof_count);
+	Tau.resize(model->dof_count);
+
+	Q[0] = -0.2;
+	Q[1] = 0.9;
+	Q[2] = 0;
+	Q[3] = -0.15;
+	Q[4] = -0.15;
+	Q[5] = 0.1;
+	Q[6] = 0.15;
+	Q[7] = -0.15;
+	Q[8] = 0;
+
+	QDot.setZero();
+
+	Tau[0] = 0;
+	Tau[1] = 0;
+	Tau[2] = 0;
+	Tau[3] = 1;
+	Tau[4] = 1;
+	Tau[5] = 1;
+	Tau[6] = 1;
+	Tau[7] = 1;
+	Tau[8] = 1;
+
+	Vector3d contact_accel_left;
+	Vector3d contact_vel_left;
+	Vector3d contact_force = Vector3d::Zero();
+
+	VectorNd QDDot_aba (QDDot);
+	VectorNd QDDot_lag (QDDot);
+	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;
+
+	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;
+
+	contact_force[0] = contact_data_left[0].force;
+	contact_force[1] = contact_data_left[1].force;
+
+	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;
+
+	CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), contact_accel_left.data(), 3, TEST_PREC);
+
+	delete model;
+}