Commits

Martin Felis committed afd79cb Draft

GiNaC code seems to work for velocity and forward kinematics computation (wow)

  • Participants
  • Parent commits e00db09

Comments (0)

Files changed (9)

CMake/FindGINAC.cmake

+# - Try to find ginac
+#
+#
+
+SET (GINAC_FOUND FALSE)
+
+FIND_PATH (GINAC_INCLUDE_DIR ginac.h /usr/include/ginac /usr/local/include/ginac $ENV{GINAC_PATH}/src $ENV{GINAC_INCLUDE_PATH})
+
+FIND_LIBRARY (GINAC_LIBRARY NAMES ginac PATHS /usr/lib /usr/local/lib $ENV{GINAC_PATH} ENV{GINAC_LIBRARY_PATH})
+
+IF (GINAC_INCLUDE_DIR AND GINAC_LIBRARY)
+	SET (GINAC_FOUND TRUE)
+ENDIF (GINAC_INCLUDE_DIR AND GINAC_LIBRARY)
+
+IF (GINAC_FOUND)
+   IF (NOT ginac_FIND_QUIETLY)
+      MESSAGE(STATUS "Found GiNaC: ${GINAC_LIBRARY}")
+   ENDIF (NOT ginac_FIND_QUIETLY)
+ELSE (GINAC_FOUND)
+   IF (ginac_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find GiNaC")
+   ENDIF (ginac_FIND_REQUIRED)
+ENDIF (GINAC_FOUND)
+
+MARK_AS_ADVANCED (
+	GINAC_INCLUDE_DIR
+	GINAC_LIBRARY
+	)
 SET (CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
 SET (CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 
+FIND_PACKAGE (GINAC REQUIRED)
+
 ADD_SUBDIRECTORY ( generator )
-ADD_SUBDIRECTORY ( gui )
-ADD_SUBDIRECTORY ( tests )
+# ADD_SUBDIRECTORY ( gui )
+# ADD_SUBDIRECTORY ( tests )
 
 SET ( RBDL_SOURCES 
 	src/mathutils.cc
 ADD_LIBRARY ( rbdl SHARED ${RBDL_SOURCES} )
 ADD_LIBRARY ( rbdl-static STATIC ${RBDL_SOURCES} )
 
+INCLUDE_DIRECTORIES (${GINAC_INCLUDE_DIR})
+
+TARGET_LINK_LIBRARIES ( rbdl
+	${GINAC_LIBRARY}
+)
+
+TARGET_LINK_LIBRARIES ( rbdl-static
+	${GINAC_LIBRARY}
+)
+
 SET_TARGET_PROPERTIES ( rbdl-static PROPERTIES OUTPUT_NAME "rbdl")
 SET_TARGET_PROPERTIES ( rbdl-static PROPERTIES PREFIX "lib")
 

generator/main.cc

 #include <iostream>
 #include <ginac/ginac.h>
+#include "rbdl.h"
 #include "mathutils.h"
 #include <vector>
 #include <sstream>
 
 using namespace std;
 using namespace GiNaC;
+using namespace RigidBodyDynamics;
+using namespace SpatialAlgebra;
 
 typedef SpatialAlgebra::Templates::SpatialVector<ex> SpatialGinVector;
 
 		derivative_func(qfunc_deriv).
 		print_func<print_csrc>(q_print_func));
 
-std::vector<ex> CreateQFunctions (size_t num, symbol t) {
-	std::vector<ex> result;
+VectorNd CreateQFunctions (size_t num, symbol t) {
+	VectorNd result (num);
 
 	unsigned int i;
 	for (i = 0; i < num; i++) {
-		result.push_back (qfunc(t, i));
+		result[i] = qfunc(t, i);
 	}
 
 	return result;
 }
 
-std::vector<ex> CreateQDotFunctions (size_t num, symbol t) {
-	std::vector<ex> result;
+VectorNd CreateQDotFunctions (size_t num, symbol t) {
+	VectorNd result (num);
 
 	unsigned int i;
 	for (i = 0; i < num; i++) {
-		result.push_back (qdotfunc(t, i));
+		result[i] = qdotfunc(t, i);
 	}
 
 	return result;
 }
 
+void calc_point_velocity (double *q, double *qdot, double velocity_out[3]) {
+}
+
+void simplify (ex &expression, const VectorNd &q, const VectorNd &qdot) {
+	unsigned int i;
+	VectorNd s_q (q.size());
+	VectorNd s_cq (q.size());
+	VectorNd s_sq (q.size());
+
+	for (i = 0; i < q.size(); i++) {
+		stringstream index_stream;
+		index_stream << i;
+
+		std::string index (index_stream.str());
+
+		s_q[i] = symbol  (std::string ("q") + index);
+		s_cq[i] = symbol (std::string ("cq") + index);
+		s_sq[i] = symbol (std::string ("sq") + index);
+	}
+
+	VectorNd s_qdot (qdot.size());
+	VectorNd s_cqdot (qdot.size());
+	VectorNd s_sqdot (qdot.size());
+
+	for (i = 0; i < qdot.size(); i++) {
+		stringstream index_stream;
+		index_stream << i;
+
+		std::string index (index_stream.str());
+
+		s_qdot[i] = symbol  (std::string ("qdot") + index);
+		s_cqdot[i] = symbol (std::string ("cqdot") + index);
+		s_sqdot[i] = symbol (std::string ("sqdot") + index);
+	}
+
+	for (i = 0; i < q.size(); i++) {
+		expression = expression.subs (cos(q[i])==s_cq[i]);
+		expression = expression.subs (sin(q[i])==s_sq[i]);
+		expression = expression.subs (q[i]==s_q[i]);
+	}
+
+	for (i = 0; i < qdot.size(); i++) {
+		expression = expression.subs (cos(qdot[i])==s_cqdot[i]);
+		expression = expression.subs (sin(qdot[i])==s_sqdot[i]);
+		expression = expression.subs (qdot[i]==s_qdot[i]);
+	}
+}
+
 int main() {
 	symbol t("t");
 
 	cout << csrc;
 
-	std::vector<ex> q = CreateQFunctions (3, t);
-	std::vector<ex> qdot = CreateQDotFunctions (3, t);
+	Model *model = new Model();
+	model->Init();
+	model->gravity.set (0., -9.81, 0.);
+	Body base_body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.));
+	unsigned int base_id = model->SetFloatingBaseBody(base_body);
+
+	VectorNd q = CreateQFunctions (model->dof_count, t);
+	VectorNd qdot = CreateQDotFunctions (model->dof_count, t);
 	int i;
 	for (i = 0; i < q.size(); i++) {
 		cout << "q[" << i << "] = " << q[i] << endl;
 	for (i = 0; i < qdot.size(); i++) {
 		cout << "qdot[" << i << "] = " << qdot[i] << endl;
 	}
-			
+
+	Vector3d point_velocity;
+	point_velocity = CalcPointVelocity (*model, q, qdot, base_id, Vector3d (1.1, 0.2, -0.3));
+
+	Vector3d point_position;
+	point_position = model->CalcBodyToBaseCoordinates(base_id, Vector3d (1.1, 1.2, 1.3));
+
+	simplify(point_velocity[0], q, qdot);
+
+	simplify(point_position[0], q, qdot);
+	simplify(point_position[1], q, qdot);
+	simplify(point_position[2], q, qdot);
+
+	cout << "out[0] = " << point_position[0] << ";" << endl;
+	cout << "out[1] = " << point_position[1] << ";" << endl;
+	cout << "out[2] = " << point_position[2] << ";" << endl;
+
+		/*
+	point_velocity[0] = point_velocity[0].subs(q[0]==q0);
+	point_velocity[0] = point_velocity[0].subs(cos(q[0])==cq0);
+	point_velocity[0] = point_velocity[0].subs(sin(q[0])==sq0);
+
+	point_velocity[0] = point_velocity[0].subs(q[3]==q3);
+	point_velocity[0] = point_velocity[0].subs(cos(q[3])==cq3);
+	point_velocity[0] = point_velocity[0].subs(sin(q[3])==sq3);
+	*/
+//	e = e.subs(cos(wild())==sqrt(1-pow(sin(wild()),2)));
+
+//	cout << point_velocity[0] << endl;
+
+	double q_values[6];
+	double qdot_values[6];
+	double vel_values[3];
+
+	qdot_values[0] = 1.;
+	qdot_values[1] = 2.;
+	qdot_values[2] = 3.;
+
+	calc_point_velocity (q_values, qdot_values, vel_values);
+
+	cout << vel_values[0] << endl;
+	cout << vel_values[1] << endl;
+	cout << vel_values[2] << endl;
+
 	return 0;
 }
 
 		}
 
 		// evaluate a0 and C0
-		double a0i = cml::dot(contact_info.normal,point_accel);
+		val_type a0i = cml::dot(contact_info.normal,point_accel);
 
 		a0[ci] = a0i;
 		C0[ci] = - (a0i - contact_info.acceleration);
 			}
 
 			// acceleration due to the test force
-			double a1j_i = cml::dot(test_contact_info.normal, point_test_accel);
+			val_type a1j_i = cml::dot(test_contact_info.normal, point_test_accel);
 			LOG << "test_accel a1j_i = " << a1j_i << std::endl;
 			LOG << "a0[ci] = " << a0[ci] << std::endl;
 			Ae(ci,cj) = a1j_i - a0[ci];
 	Fext = zero_f_ext;
 
 	for (ci = 0; ci < contact_count; ci++) {
-		ContactData[ci].force = u[ci];
+		// \todo disable force computation in GiNaC mode!
+		// ContactData[ci].force = u[ci];
 
 		test_forces[ci] = test_forces[ci] * u[ci];
 		// it is important to *add* the constraint force as multiple forces
 		SpatialVector &S,
 		SpatialVector &v_J,
 		SpatialVector &c_J,
-		const double &q,
-		const double &qdot
+		const val_type &q,
+		const val_type &qdot
 		) {
 	// exception if we calculate it for the root body
 	assert (joint_id > 0);
 #include "Model.h"
 #include "mathutils.h"
 
+#include "ginac/ginac.h"
+
 namespace RigidBodyDynamics {
 
 class Model;
 
 		if (joint_type == JointTypeRevolute) {
 			// make sure we have a unit axis
-			assert (joint_axis.length() == 1.);
+
+//			assert (joint_axis.length() == 1.);
 
 			assert ( joint_axis == Vector3d (1., 0., 0.)
 					|| joint_axis == Vector3d (0., 1., 0.)
 			mJointAxis.set (0., 0., 0., 0., 0., 0.);
 		} else if (joint_type == JointTypePrismatic) {
 			// make sure we have a unit axis
-			assert (joint_axis.length() == 1.);
+//			assert (joint_axis.length() == 1.);
 
 			mJointAxis.set (
 					0., 0., 0.,
 		SpatialAlgebra::SpatialVector &S,
 		SpatialAlgebra::SpatialVector &v_J,
 		SpatialAlgebra::SpatialVector &c_J,
-		const double &q,
-		const double &qdot
+		const val_type &q,
+		const val_type &qdot
 		);
 
 }
 #ifndef _CMLWRAPPER_H
 #define _CMLWRAPPER_H
 
+#include "ginac/ginac.h"
+
+using namespace GiNaC;
+typedef GiNaC::ex val_type;
+
 #include "cml/cml_config.h"
 #include "cml/cml.h"
 
-typedef cml::vector<double, cml::fixed<3> > Vector3d;
-typedef cml::matrix<double, cml::fixed<3,3> > Matrix3d;
+typedef cml::vector<val_type, cml::fixed<3> > Vector3d;
+typedef cml::matrix<val_type, cml::fixed<3,3> > Matrix3d;
 
-typedef cml::vector<double, cml::dynamic<> > VectorNd;
-typedef cml::matrix<double, cml::dynamic<> > MatrixNd;
+typedef cml::vector<val_type, cml::dynamic<> > VectorNd;
+typedef cml::matrix<val_type, cml::dynamic<> > MatrixNd;
 
 #include "SpatialAlgebra.h"
 
 		class SpatialMatrix;
 }
 
-typedef Templates::SpatialVector<double> SpatialVector;
-typedef Templates::SpatialMatrix<double> SpatialMatrix;
+typedef Templates::SpatialVector<val_type> SpatialVector;
+typedef Templates::SpatialMatrix<val_type> SpatialMatrix;
 
 }
 
 	unsigned int i,j;
 	for (j = 0; j < n; j++) {
 		for (i = j + 1; i < n; i++) {
-			if (fabs(A(j,j)) <= std::numeric_limits<double>::epsilon()) {
+			/*
+			if (abs(A(j,j)) <= std::numeric_limits<double>::epsilon()) {
 				std::cout << LogOutput.str() << std::endl;
 			}
-			assert (fabs(A(j,j)) > std::numeric_limits<double>::epsilon());
-			double d = A(i,j)/A(j,j);
+			assert (abs(A(j,j)) > std::numeric_limits<double>::epsilon());
+			*/
+			ex d = A(i,j)/A(j,j);
 
 			b[i] -= b[j] * d;
 
 
 	for (j = 0; j < n; j++) {
 		pi = j;
-		double pv = fabs (A(j,pivot[j]));
+		ex pv = abs (A(j,pivot[j]));
 
 		// LOG << "j = " << j << " pv = " << pv << std::endl;
 		// find the pivot
 		for (k = j; k < n; k++) {
-			double pt = fabs (A(j,pivot[k]));
+			ex pt = abs (A(j,pivot[k]));
 			if (pt > pv) {
 				pv = pt;
 				pi = k;
 		}
 
 		for (i = j + 1; i < n; i++) {
-			if (fabs(A(j,pivot[j])) <= std::numeric_limits<double>::epsilon()) {
+			if (abs(A(j,pivot[j])) <= std::numeric_limits<double>::epsilon()) {
 				LOG << "Pivoting failed for matrix A = " << std::endl;
 				LOG << "A = " << std::endl << A << std::endl;
 //				LOG << "b = " << b << std::endl;
 				std::cout << LogOutput.str() << std::endl;
 			}
-			assert (fabs(A(j,pivot[j])) > std::numeric_limits<double>::epsilon());
-			double d = A(i,pivot[j])/A(j,pivot[j]);
+			assert (abs(A(j,pivot[j])) > std::numeric_limits<double>::epsilon());
+			ex d = A(i,pivot[j])/A(j,pivot[j]);
 
 			b[i] -= b[j] * d;
 
 
 	for (i = 0; i < 6; i++) {
 		for (j = 0; j < 6; j++) {
-			if (fabs(matrix_a(i,j) - matrix_b(i,j)) >= epsilon) {
+			if (abs(matrix_a(i,j) - matrix_b(i,j)) >= epsilon) {
 				std::cerr << "Expected:" 
 					<< std::endl << matrix_a << std::endl
 					<< "but was" << std::endl 
 	unsigned int i;
 
 	for (i = 0; i < 6; i++) {
-		if (fabs(vector_a[i] - vector_b[i]) >= epsilon) {
+		if (abs(vector_a[i] - vector_b[i]) >= epsilon) {
 			std::cerr << "Expected:" 
 				<< std::endl << vector_a << std::endl
 				<< "but was" << std::endl 
 			);
 }
 
-SpatialMatrix Xrotx (const double &xrot) {
-	double s, c;
+SpatialMatrix Xrotx (const ex &xrot) {
+	ex s, c;
 	s = sin (xrot);
 	c = cos (xrot);
 
 			);
 }
 
-SpatialMatrix Xroty (const double &yrot) {
-	double s, c;
+SpatialMatrix Xroty (const ex &yrot) {
+	ex s, c;
 	s = sin (yrot);
 	c = cos (yrot);
 
 			);
 }
 
-SpatialMatrix Xrotz (const double &zrot) {
-	double s, c;
+SpatialMatrix Xrotz (const ex &zrot) {
+	ex s, c;
 	s = sin (zrot);
 	c = cos (zrot);
 
 void SpatialMatrixSetSubmatrix(SpatialAlgebra::SpatialMatrix &dest, unsigned int row, unsigned int col, const Matrix3d &matrix);
 
 bool SpatialMatrixCompareEpsilon (const SpatialAlgebra::SpatialMatrix &matrix_a,
-		const SpatialAlgebra::SpatialMatrix &matrix_b, double epsilon);
+		const SpatialAlgebra::SpatialMatrix &matrix_b, val_type epsilon);
 bool SpatialVectorCompareEpsilon (const SpatialAlgebra::SpatialVector &vector_a,
-		const SpatialAlgebra::SpatialVector &vector_b, double epsilon);
+		const SpatialAlgebra::SpatialVector &vector_b, val_type epsilon);
 
 /** \brief Creates a transformation of a linear displacement
  *
  *
  * \param zrot Rotation angle in radians.
  */
-SpatialAlgebra::SpatialMatrix Xrotz (const double &zrot);
+SpatialAlgebra::SpatialMatrix Xrotz (const val_type &zrot);
 
 /** \brief Creates a rotational transformation around the Y-axis
  *
  *
  * \param yrot Rotation angle in radians.
  */
-SpatialAlgebra::SpatialMatrix Xroty (const double &yrot);
+SpatialAlgebra::SpatialMatrix Xroty (const val_type &yrot);
 
 /** \brief Creates a rotational transformation around the X-axis
  *
  *
  * \param xrot Rotation angle in radians.
  */
-SpatialAlgebra::SpatialMatrix Xrotx (const double &xrot);
+SpatialAlgebra::SpatialMatrix Xrotx (const val_type &xrot);
 
 /** \brief Creates a spatial transformation for given parameters 
  *
  */
 SpatialAlgebra::SpatialMatrix XtransRotZYXEuler (const Vector3d &displacement, const Vector3d &zyx_euler);
 
-inline std::ostream& operator<<(std::ostream& output, const std::vector<double> &val) {
+inline std::ostream& operator<<(std::ostream& output, const std::vector<val_type> &val) {
 	unsigned int i;
 	for (i = 0; i < val.size(); i++)
 		output << val.at(i) << " ";