Source

rbdl / generator / main.cc

#include <iostream>
#include <ginac/ginac.h>
#include "mathutils.h"
#include <vector>
#include <sstream>

using namespace std;
using namespace GiNaC;

typedef SpatialAlgebra::Templates::SpatialVector<ex> SpatialGinVector;

void q_print_func(const ex& t1, const ex& t2, const print_context &pc) {
	stringstream out;
	out << t2;	
	int index;
	out >> index;

	cout << "q[" << index << "]";
}

void qdot_print_func(const ex& t1, const ex& t2, const print_context &pc) {
	stringstream out;
	out << t2;	
	int index;
	out >> index;

	cout << "qdot[" << index << "]";
}

DECLARE_FUNCTION_2P(qdotfunc);
static ex qdotfunc_eval (const ex &t, const ex &index) {
	return qdotfunc(t, index).hold();
}
REGISTER_FUNCTION(qdotfunc,eval_func(qdotfunc_eval).
		print_func<print_csrc>(qdot_print_func));


DECLARE_FUNCTION_2P(qfunc);
static ex qfunc_eval (const ex &t, const ex &index) {
	return qfunc(t, index).hold();
}

static ex qfunc_deriv (const ex &t, const ex &index, unsigned deriv_param) {
	GINAC_ASSERT(deriv_param == 0);
	return qdotfunc(t, index);
}

REGISTER_FUNCTION(qfunc, 
		eval_func(qfunc_eval).
		derivative_func(qfunc_deriv).
		print_func<print_csrc>(q_print_func));

std::vector<ex> CreateQFunctions (size_t num, symbol t) {
	std::vector<ex> result;

	unsigned int i;
	for (i = 0; i < num; i++) {
		result.push_back (qfunc(t, i));
	}

	return result;
}

std::vector<ex> CreateQDotFunctions (size_t num, symbol t) {
	std::vector<ex> result;

	unsigned int i;
	for (i = 0; i < num; i++) {
		result.push_back (qdotfunc(t, i));
	}

	return result;
}

int main() {
	symbol t("t");

	cout << csrc;

	std::vector<ex> q = CreateQFunctions (3, t);
	std::vector<ex> qdot = CreateQDotFunctions (3, t);
	int i;
	for (i = 0; i < q.size(); i++) {
		cout << "q[" << i << "] = " << q[i] << endl;
	}

	for (i = 0; i < q.size(); i++) {
		cout << "diff(q[" << i << "],t) = " << q[i].diff(t,1) << endl;
	}

	for (i = 0; i < qdot.size(); i++) {
		cout << "qdot[" << i << "] = " << qdot[i] << endl;
	}
			
	return 0;
}