Martin  Felis avatar Martin Felis committed f304194

replaced cmlMatrix and cmlVector by MatrixNd and VectorNd

Comments (0)

Files changed (15)

gui/src/modelstate.cc

 Joint joint_base_rot_z, joint_base_rot_y, joint_base_rot_x,
 	joint_child_rot_z, joint_child_rot_y, joint_child_rot_x;
 
-cmlVector Q;
-cmlVector QDot;
-cmlVector QDDot;
-cmlVector Tau;
+VectorNd Q;
+VectorNd QDot;
+VectorNd QDDot;
+VectorNd Tau;
 
 unsigned int contact_body_id;
 Vector3d contact_point;
 
 std::vector<ContactInfo> contact_data;
 
-typedef cmlVector (rhs_func) (double, const cmlVector&);
+typedef VectorNd (rhs_func) (double, const VectorNd&);
 
-cmlVector rk45_integrator (double t0, double tf, cmlVector &y0, rhs_func func, double error) {
-	cmlVector k1 (y0.size());
-	cmlVector k2 (y0.size());
-	cmlVector k3 (y0.size());
-	cmlVector k4 (y0.size());
-	cmlVector k5 (y0.size());
-	cmlVector k6 (y0.size());
-	cmlVector rk4 (y0.size());
-	cmlVector rk5 (y0.size());
+VectorNd rk45_integrator (double t0, double tf, VectorNd &y0, rhs_func func, double error) {
+	VectorNd k1 (y0.size());
+	VectorNd k2 (y0.size());
+	VectorNd k3 (y0.size());
+	VectorNd k4 (y0.size());
+	VectorNd k5 (y0.size());
+	VectorNd k6 (y0.size());
+	VectorNd rk4 (y0.size());
+	VectorNd rk5 (y0.size());
 
 	double s = 1.;
 	double t = t0;
 	double h0 = tf - t0;
 	double h = h0;
 	double h_min = 1.0e5;
-	cmlVector h_min_y (y0);
-	cmlVector h_min_rk5 (y0);
-	cmlVector y (y0);
+	VectorNd h_min_y (y0);
+	VectorNd h_min_rk5 (y0);
+	VectorNd y (y0);
 	int stepcount = 0;
 
 	while (t < tf) {
 //	return y0 + h * (k1 + 2 * k2 + 2 * k3 + k4) / 6.; 
 }
 
-cmlVector rk4_integrator (double t0, double tf, cmlVector &y0, rhs_func func, double stepsize) {
-	cmlVector k1 (y0.size());
-	cmlVector k2 (y0.size());
-	cmlVector k3 (y0.size());
-	cmlVector k4 (y0.size());
-	cmlVector y (y0);
+VectorNd rk4_integrator (double t0, double tf, VectorNd &y0, rhs_func func, double stepsize) {
+	VectorNd k1 (y0.size());
+	VectorNd k2 (y0.size());
+	VectorNd k3 (y0.size());
+	VectorNd k4 (y0.size());
+	VectorNd y (y0);
 
 	double t = t0;
 	double h = stepsize;
 	return y;
 }
 
-cmlVector euler_integrator (double t0, double tf, cmlVector &y0, rhs_func func, double stepsize) {
-	cmlVector y (y0);
+VectorNd euler_integrator (double t0, double tf, VectorNd &y0, rhs_func func, double stepsize) {
+	VectorNd y (y0);
 
-	cmlVector ydot (y0);
+	VectorNd ydot (y0);
 	ydot = func (tf, y);
 
 	y = y0 + (tf - t0) * ydot;
 
 	unsigned int base_body_id = model->SetFloatingBaseBody(base);
 
-	Q = cmlVector(model->dof_count);
-	QDot = cmlVector(model->dof_count);
-	QDDot = cmlVector(model->dof_count);
-	Tau = cmlVector(model->dof_count);
+	Q = VectorNd(model->dof_count);
+	QDot = VectorNd(model->dof_count);
+	QDDot = VectorNd(model->dof_count);
+	Tau = VectorNd(model->dof_count);
 
 	Q.zero();
 	QDot.zero();
 	ForwardDynamics (*model, Q, QDot, Tau, QDDot);
 }
 
-cmlVector rhs_contact (double t, const cmlVector &y) {
+VectorNd rhs_contact (double t, const VectorNd &y) {
 	unsigned int i;
 	unsigned int size = Q.size();
 
-	cmlVector q (size);
-	cmlVector qdot (size);
-	cmlVector qddot (size);
+	VectorNd q (size);
+	VectorNd qdot (size);
+	VectorNd qddot (size);
 
 	std::vector<ContactInfo> contact_data;
 	contact_point = model->CalcBaseToBodyCoordinates (contact_body_id, Vector3d (Q[0], 0., Q[2]));
 	contact_point_world_acc = CalcPointAcceleration (*model, q, qdot, qddot, contact_body_id, contact_point);
 	cout << "\tCPacc = " << contact_point_world_acc;
 
-	cmlVector res (size * 2);
+	VectorNd res (size * 2);
 	for (i = 0; i < size; i++) {
 		res[i] = qdot[i];
 		res[i + size] = qddot[i];
 	return res;
 }
 
-cmlVector rhs_normal (double t, const cmlVector &y) {
+VectorNd rhs_normal (double t, const VectorNd &y) {
 	unsigned int i;
 	unsigned int size = Q.size();
 
-	cmlVector q (size);
-	cmlVector qdot (size);
-	cmlVector qddot (size);
+	VectorNd q (size);
+	VectorNd qdot (size);
+	VectorNd qddot (size);
 
 	for (i = 0; i < size; i++) {
 		q[i] = y[i];
 	contact_point_world_acc = CalcPointAcceleration (*model, q, qdot, qddot, contact_body_id, contact_point);
 	cout << "\tCPacc = " << contact_point_world_acc << endl;
 
-	cmlVector res (size * 2);
+	VectorNd res (size * 2);
 	for (i = 0; i < size; i++) {
 		res[i] = qdot[i];
 		res[i + size] = qddot[i];
 	unsigned int size = Q.size();
 	unsigned int i;
 	
-	cmlVector y (size * 2);
+	VectorNd y (size * 2);
 
 	for (i = 0; i < size; i++) {
 		y[i] = Q[i];
 		y[i + size] = QDot[i];
 	}
 
-	cmlVector ynew (size * 2);
+	VectorNd ynew (size * 2);
 //	delta_time = 0.02;
 //	ynew = rk45_integrator (0., delta_time, y, rhs_normal, 1.0e-3);
 //	ynew = rk4_integrator (0., delta_time, y, rhs_contact, 5.0e-2);
  */
 void ForwardDynamicsFloatingBase (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDot,
-		const cmlVector &Tau,
-		cmlVector &QDDot
+		const VectorNd &Q,
+		const VectorNd &QDot,
+		const VectorNd &Tau,
+		VectorNd &QDDot
 		) {
 	LOG << "-------- " << __func__ << " --------" << std::endl;
-	cmlVector q_expl (Q.size() - 6);
-	cmlVector qdot_expl (QDot.size() - 6);
-	cmlVector tau_expl (Tau.size() - 6);
-	cmlVector qddot_expl (QDDot.size() - 6);
+	VectorNd q_expl (Q.size() - 6);
+	VectorNd qdot_expl (QDot.size() - 6);
+	VectorNd tau_expl (Tau.size() - 6);
+	VectorNd qddot_expl (QDDot.size() - 6);
 
 	LOG << "Q = " << Q << std::endl;
 	LOG << "QDot = " << QDot << std::endl;
 
 void ForwardDynamics (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDot,
-		const cmlVector &Tau,
-		cmlVector &QDDot
+		const VectorNd &Q,
+		const VectorNd &QDot,
+		const VectorNd &Tau,
+		VectorNd &QDDot
 		) {
 	if (model.experimental_floating_base) {
-		cmlVector q (Q.size());
-		cmlVector qdot (QDot.size());
-		cmlVector qddot (QDDot.size());
-		cmlVector tau (Tau.size());
+		VectorNd q (Q.size());
+		VectorNd qdot (QDot.size());
+		VectorNd qddot (QDDot.size());
+		VectorNd tau (Tau.size());
 
 		unsigned int i;
 		
 
 void ForwardDynamicsFloatingBaseExpl (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDot,
-		const cmlVector &Tau,
+		const VectorNd &Q,
+		const VectorNd &QDot,
+		const VectorNd &Tau,
 		const SpatialMatrix &X_B,
 		const SpatialVector &v_B,
 		const SpatialVector &f_B,
 		SpatialVector &a_B,
-		cmlVector &QDDot
+		VectorNd &QDDot
 		)
 {
 	SpatialVector result;
 
 void ComputeContactForces (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDot,
-		const cmlVector &Tau,
+		const VectorNd &Q,
+		const VectorNd &QDot,
+		const VectorNd &Tau,
 		std::vector<ContactInfo> &ContactData,
 		std::vector<SpatialAlgebra::SpatialVector> &Fext
 		) {
 	
 	// save current external forces:
 
-	cmlMatrix Ae;
+	MatrixNd Ae;
 	Ae.resize(contact_count, contact_count);
-	cmlVector C0 (contact_count);
-	cmlVector a0 (contact_count);
+	VectorNd C0 (contact_count);
+	VectorNd a0 (contact_count);
 
 	std::vector<SpatialVector> current_f_ext (model.f_ext);
 	std::vector<SpatialVector> zero_f_ext (model.f_ext.size(), SpatialVector (0., 0., 0., 0., 0., 0.));
 	model.f_ext = zero_f_ext;
 
 	LOG << "-------- ZERO_EXT ------" << std::endl;
-	cmlVector QDDot_zero_ext (QDot);
+	VectorNd QDDot_zero_ext (QDot);
 	{
 		SUPPRESS_LOGGING;
 		ForwardDynamics (model, Q, QDot, Tau, QDDot_zero_ext);
 
 		// apply the test force
 		model.f_ext[contact_info.body_id] = test_forces[cj];
-		cmlVector QDDot_test_ext (QDot);
+		VectorNd QDDot_test_ext (QDot);
 
 		LOG << "-------- TEST_EXT -------" << std::endl;
 		LOG << "test_force_body = " << Xtrans (model.GetBodyOrigin(contact_info.body_id) - contact_point_position).adjoint() * test_forces[cj] << std::endl;
 	}
 	
 	// solve the system!!!
-	cmlVector u (contact_count);
+	VectorNd u (contact_count);
 
 	LOG << "Ae = " << std::endl << Ae << std::endl;
 	LOG << "C0 = " << C0 << std::endl;
 
 void ForwardDynamicsContacts (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDot,
-		const cmlVector &Tau,
+		const VectorNd &Q,
+		const VectorNd &QDot,
+		const VectorNd &Tau,
 		std::vector<ContactInfo> &ContactData,
-		cmlVector &QDDot
+		VectorNd &QDDot
 		) {
 	LOG << "-------- ForwardDynamicsContacts ------" << std::endl;
 
 /*
 void ComputeContactImpulses (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDotPre,
+		const VectorNd &Q,
+		const VectorNd &QDotPre,
 		const std::vector<ContactInfo> &ContactData,
-		cmlVector &QDotPost
+		VectorNd &QDotPost
 		) {
 	std::vector<ContactInfo> ContactImpulseInfo;
-	cmlVector QDotZero (QDotPre.size());
+	VectorNd QDotZero (QDotPre.size());
 	QDotZero.zero();
 	ContactInfo contact_info;
 	Vector3d point_velocity;
 	}
 
 	std::vector<SpatialVector> contact_f_ext;
-	cmlVector QDDotFext (QDotPre);
+	VectorNd QDDotFext (QDotPre);
 	QDDotFext.zero();
-	cmlVector Tau_zero (QDDotFext);
+	VectorNd Tau_zero (QDDotFext);
 
 	// for debugging
-	cmlVector QDDotZeroFext (QDotPre.size());
+	VectorNd QDDotZeroFext (QDotPre.size());
 	{
 		SUPPRESS_LOGGING;
 		ForwardDynamics (model, Q, QDotPre, Tau_zero, QDDotZeroFext);
 	LOG << "QDDotZeroFext = " << QDDotZeroFext << std::endl;
 	LOG << "QDDotFext     = " << QDDotFext << std::endl;
 
-	cmlVector humans_impulse (QDotPre.size());
+	VectorNd humans_impulse (QDotPre.size());
 	humans_impulse[0] = 0.;
 	humans_impulse[1] = -0.05;
 	humans_impulse[2] = 0.1;
  */
 void ForwardDynamics (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDot,
-		const cmlVector &Tau,
-		cmlVector &QDDot
+		const VectorNd &Q,
+		const VectorNd &QDot,
+		const VectorNd &Tau,
+		VectorNd &QDDot
 		);
 
 struct ContactInfo;
  */
 void ComputeContactForces (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDot,
-		const cmlVector &Tau,
+		const VectorNd &Q,
+		const VectorNd &QDot,
+		const VectorNd &Tau,
 		std::vector<ContactInfo> &ContactData,
 		const std::vector<SpatialAlgebra::SpatialVector> &Fext
 		);
  */
 void ForwardDynamicsContacts (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDot,
-		const cmlVector &Tau,
+		const VectorNd &Q,
+		const VectorNd &QDot,
+		const VectorNd &Tau,
 		std::vector<ContactInfo> &ContactData,
-		cmlVector &QDDot
+		VectorNd &QDDot
 		);
 
 /** \brief Computes the change of the generalized velocity due to collisions
  */
 void ComputeContactImpulses (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDotPre,
+		const VectorNd &Q,
+		const VectorNd &QDotPre,
 		const std::vector<ContactInfo> &ContactData,
-		cmlVector &QDotPost
+		VectorNd &QDotPost
 		);
 }
 

src/Dynamics_experimental.h

  */
 void ForwardDynamicsFloatingBaseExpl (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDot,
-		const cmlVector &Tau,
+		const VectorNd &Q,
+		const VectorNd &QDot,
+		const VectorNd &Tau,
 		const SpatialAlgebra::SpatialMatrix &X_B,
 		const SpatialAlgebra::SpatialVector &v_B,
 		const SpatialAlgebra::SpatialVector &f_B,
 		SpatialAlgebra::SpatialVector &a_B,
-		cmlVector &QDDot
+		VectorNd &QDDot
 		);
 
 /** \brief Computes the change of the generalized velocity due to collisions
  */
 void ComputeContactImpulses (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDotPre,
+		const VectorNd &Q,
+		const VectorNd &QDotPre,
 		const std::vector<ContactInfo> &ContactData,
-		cmlVector &QDotPost
+		VectorNd &QDotPost
 		);
 }
 

src/Kinematics.cc

 
 Vector3d CalcPointVelocity (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDot,
+		const VectorNd &Q,
+		const VectorNd &QDot,
 		unsigned int body_id,
 		const Vector3d &point_position
 	) {
 
 Vector3d CalcPointAcceleration (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDot,
-		const cmlVector &QDDot,
+		const VectorNd &Q,
+		const VectorNd &QDot,
+		const VectorNd &QDDot,
 		unsigned int body_id,
 		const Vector3d &point_position
 		)
  */
 Vector3d CalcPointVelocity (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDot,
+		const VectorNd &Q,
+		const VectorNd &QDot,
 		unsigned int body_id,
 		const Vector3d &point_position
 		);
  */
 Vector3d CalcPointAcceleration (
 		Model &model,
-		const cmlVector &Q,
-		const cmlVector &QDot,
-		const cmlVector &QDDot,
+		const VectorNd &Q,
+		const VectorNd &QDot,
+		const VectorNd &QDDot,
 		unsigned int body_id,
 		const Vector3d &point_position
 		);
 	}
 };
 
-/** \brief Computes the joint variables 
- *
- * \param model    the rigid body model
- * \param joint_id the id of the joint we are interested in (output)
- * \param XJ       the joint transformation (output)
- * \param S        motion subspace of the joint (output)
- * \param v_J      joint velocity (output)
- * \param c_J      joint acceleration for rhenomic joints (output)
- * \param q        joint state variable
- * \param qdot     joint velocity variable
- */
-void jcalc (
-		const Model &model,
-		const unsigned int &joint_id,
-		SpatialAlgebra::SpatialMatrix &XJ,
-		SpatialAlgebra::SpatialVector &S,
-		SpatialAlgebra::SpatialVector &v_J,
-		SpatialAlgebra::SpatialVector &c_J,
-		const double &q,
-		const double &qdot
-		);
-
 }
 
 #endif /* _MODEL_H */

src/SpatialAlgebra.h

 
 // forward declaration as it is needed for the SpatialLinSolveFunction
 // \todo proper forward declaration of linsolve
-bool LinSolveGaussElimPivot (cmlMatrix A, cmlVector b, cmlVector &x);
+bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x);
 
 /** \brief Namespace for all the spatial algebra quantities
  */
  */
 inline SpatialVector SpatialLinSolve (SpatialMatrix A, SpatialVector b) {
 	SpatialVector x(0., 0., 0., 0., 0., 0.);
-	cmlMatrix cmlA;
-	cmlVector cmlb(6);
-	cmlVector cmlx(6);
+	MatrixNd cmlA;
+	VectorNd cmlb(6);
+	VectorNd cmlx(6);
 
 	cmlA.resize(6,6);
 	cmlb.resize(6);
 #include "cml/cml_config.h"
 #include "cml/cml.h"
 
-typedef cml::vector<double, cml::dynamic<> > cmlVector;
-typedef cml::matrix<double, cml::dynamic<> > cmlMatrix;
-
 typedef cml::vector<double, cml::fixed<3> > Vector3d;
 typedef cml::matrix<double, cml::fixed<3,3> > Matrix3d;
 
 typedef cml::vector<double, cml::dynamic<> > VectorNd;
 typedef cml::matrix<double, cml::dynamic<> > MatrixNd;
 
-typedef cml::quaternion<double, cml::fixed<>, cml::vector_first, cml::positive_cross> Quaternion;
-
 #include "SpatialAlgebra.h"
 
-/*
-typedef SpatialAlgebra::SpatialVector SpatialVector;
-typedef SpatialAlgebra::SpatialMatrix SpatialMatrix;
-*/
-
-//typedef cml::vector<double, cml::fixed<6> > SpatialVector;
-//typedef cml::matrix<double, cml::fixed<6,6> > SpatialMatrix;
-
 #endif /* _CMLWRAPPER_H */
 			-vector[1],  vector[0], 0);
 }
 
-bool LinSolveGaussElim (cmlMatrix A, cmlVector b, cmlVector &x) {
+bool LinSolveGaussElim (MatrixNd A, VectorNd b, VectorNd &x) {
 	x.zero();
 
 	// We can only solve quadratic systems
 	return true;
 }
 
-bool LinSolveGaussElimPivot (cmlMatrix A, cmlVector b, cmlVector &x) {
+bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x) {
 	x.zero();
 
 	// We can only solve quadratic systems
 	size_t *pivot = new size_t[n];
 
 	// temporary result vector which contains the pivoted result
-	cmlVector px(x);
+	VectorNd px(x);
 	
 	unsigned int i,j,k;
 
 
 /** \brief Solves a linear system using gaussian elimination
  */
-bool LinSolveGaussElim (cmlMatrix A, cmlVector b, cmlVector &x);
+bool LinSolveGaussElim (MatrixNd A, VectorNd b, VectorNd &x);
 
 /** \brief Solves a linear system using gaussian elimination with pivoting
  */
-bool LinSolveGaussElimPivot (cmlMatrix A, cmlVector b, cmlVector &x);
+bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x);
 
 /** \brief Creates the matrix to the cross product of a given 3D vector
  *  

tests/CMakeLists.txt

 	CalcVelocitiesTests.cc
 	CalcAccelerationsTests.cc
 	KinematicsTests.cc
-	ContactsTests.cc
+	# ContactsTests.cc
 	# ImpulsesTests.cc
 	)
 

tests/FloatingBaseTests.cc

 	Body base;
 	unsigned int base_body_id;
 
-	cmlVector q, qdot, qddot, tau;
+	VectorNd q, qdot, qddot, tau;
 };
 
 TEST_FIXTURE ( FloatingBaseFixture, TestCalcPointTransformation ) {
 	// floating base
 	base_body_id = model->SetFloatingBaseBody(base);
 
-	cmlVector Q;
-	cmlVector QDot;
-	cmlVector QDDot;
-	cmlVector Tau;
+	VectorNd Q;
+	VectorNd QDot;
+	VectorNd QDDot;
+	VectorNd Tau;
 
 	Q.resize(6);
 	QDot.resize(6);

tests/MathTests.cc

 TEST (GaussElimPivot) {
 	ClearLogOutput();
 
-	cmlMatrix A;
+	MatrixNd A;
 	A.resize(3,3);
-	cmlVector b(3);
-	cmlVector x(3);
+	VectorNd b(3);
+	VectorNd x(3);
 
 	A.set(0, 2, 1,
 			1, 1, 5,
 			0, 0, 1);
 	b.set(1,2,3);
 
-	cmlVector test_result (3);
+	VectorNd test_result (3);
 	test_result.set (-12, -1, 3);
 
 	LinSolveGaussElimPivot (A, b, x);

tests/ModelTests.cc

 
 	unsigned int body_id = model->SetFloatingBaseBody (body);
 
-	cmlVector q (model->dof_count);
-	cmlVector qdot (model->dof_count);
-	cmlVector qddot (model->dof_count);
-	cmlVector tau (model->dof_count);
+	VectorNd q (model->dof_count);
+	VectorNd qdot (model->dof_count);
+	VectorNd qddot (model->dof_count);
+	VectorNd tau (model->dof_count);
 
 	Vector3d base_coords;
 	Vector3d body_coords;
 
 	unsigned int body_id = model->SetFloatingBaseBody (body);
 
-	cmlVector q (model->dof_count);
-	cmlVector qdot (model->dof_count);
-	cmlVector qddot (model->dof_count);
-	cmlVector tau (model->dof_count);
+	VectorNd q (model->dof_count);
+	VectorNd qdot (model->dof_count);
+	VectorNd qddot (model->dof_count);
+	VectorNd tau (model->dof_count);
 
-	cmlVector input (model->dof_count);
-	cmlVector reference (model->dof_count + 1);
+	VectorNd input (model->dof_count);
+	VectorNd reference (model->dof_count + 1);
 
 	reference[0] = 0;
 	reference[1] = 0.;
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.