returnValue DiscreteTimeExport::setDifferentialEquation( const Expression& rhs_ )
{
if( rhs_.getDim() > 0 ) {
Parameter dummy0;
Control dummy1;
DifferentialState dummy2;
AlgebraicState dummy3;
DifferentialStateDerivative dummy4;
dummy0.clearStaticCounters();
dummy1.clearStaticCounters();
dummy2.clearStaticCounters();
dummy3.clearStaticCounters();
dummy4.clearStaticCounters();
NX2 = rhs_.getDim() - NXA;
x = DifferentialState(NX1+NX2);
z = AlgebraicState(NXA);
dx = DifferentialStateDerivative(NDX);
u = Control(NU);
p = Parameter(NP);
DifferentialEquation f;
f << rhs_;
DifferentialEquation g;
for( uint i = 0; i < rhs_.getDim(); i++ ) {
g << forwardDerivative( rhs_(i), x );
g << forwardDerivative( rhs_(i), u );
// There are not supposed to be algebraic states or differential state derivatives !
}
return (rhs.init( f,"acado_rhs",NX,NXA,NU ) & diffs_rhs.init( g,"acado_diffs",NX,NXA,NU ) );
}
return SUCCESSFUL_RETURN;
}
returnValue PlotWindow::addSubplot( const Expression& _expressionX,
const Expression& _expressionY,
const char* const _title,
const char* const _xLabel,
const char* const _yLabel,
PlotMode _plotMode,
double _xRangeLowerLimit,
double _xRangeUpperLimit,
double _yRangeLowerLimit,
double _yRangeUpperLimit
)
{
ASSERT( _expressionX.getDim() == _expressionY.getDim() );
uint run1;
for( run1 = 0; run1 < _expressionX.getDim(); run1++ ){
PlotWindowSubplot* newSubplot = new PlotWindowSubplot( _expressionX(run1),_expressionY(run1),
_title,_xLabel,_yLabel,_plotMode,
_xRangeLowerLimit,_xRangeUpperLimit,
_yRangeLowerLimit,_yRangeUpperLimit );
if ( number == 0 )
{
first = newSubplot;
last = newSubplot;
}
else
{
if ( last->setNext( newSubplot ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_OPTIONS_LIST_CORRUPTED );
last = newSubplot;
}
Expression tmpX( _expressionX(run1) );
if ( addPlotDataItem( &tmpX ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_LOG_RECORD_CORRUPTED );
Expression tmpY( _expressionY(run1) );
if ( addPlotDataItem( &tmpY ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_LOG_RECORD_CORRUPTED );
++number;
}
return SUCCESSFUL_RETURN;
}
returnValue OCP::subjectTo( const DVector& _lb, const Expression& _expr, const DVector& _ub )
{
ASSERT(_lb.getDim() == _expr.getDim() && _lb.getDim() == _ub.getDim());
constraint->add(_lb, _expr, _ub);
return SUCCESSFUL_RETURN;
}
returnValue ModelData::setModel( const DifferentialEquation& _f )
{
if( rhs_name.empty() && NX2 == 0 ) {
differentialEquation = _f;
Expression rhs;
differentialEquation.getExpression( rhs );
NXA = differentialEquation.getNXA();
NX2 = rhs.getDim() - NXA;
if( NDX == 0 ) NDX = _f.getNDX();
// if( _f.getNDX() > 0 && _f.getNDX() != (int)NX2 ) { // TODO: this test returns an error for well-defined models when using a linear input subsystem!
// std::cout << "nonlinear model of size " << NX2 << " depends on " << _f.getNDX() << " differential state derivatives!" << std::endl;
// return ACADOERROR( RET_INVALID_OPTION );
// }
if( NU == 0 ) NU = _f.getNU();
if( NP == 0 ) NP = _f.getNP();
if( NOD == 0 ) NOD = _f.getNOD();
export_rhs = BT_TRUE;
}
else {
return ACADOERROR( RET_INVALID_OPTION );
}
return SUCCESSFUL_RETURN;
}
Expression CFunction::operator()( const Expression &arg ){
uint run1,run2;
CFunction thisFunction(*this);
thisFunction.nn = arg.getDim();
for( run1 = 0; run1 < maxAlloc; run1++ ){
thisFunction.xStore [run1] = new double[thisFunction.nn];
thisFunction.seedStore[run1] = new double[thisFunction.nn];
for( run2 = 0; run2 < thisFunction.nn; run2++ ){
thisFunction.xStore [run1][run2] = 0.0;
thisFunction.seedStore[run1][run2] = 0.0;
}
}
Expression tmp(dim);
COperator dummy;
dummy.increaseID();
for( run1 = 0; run1 < dim; run1++ ){
delete tmp.element[run1];
tmp.element[run1] = new COperator( thisFunction, arg, run1 );
}
return tmp;
}
Expression KinVec::explicitize(const Expression & ddq_) const {
if ( q.getDim() != 3 ){ ACADOERROR( RET_DDQ_DIMENSION_MISMATCH ); ASSERT( 1 == 0 ); }
//if ( abs(order) !=2 ){ ACADOERROR( RET_CAN_ONLY_SOLVE_2ND_ORDER_KINVECS ); ASSERT( 1 == 0 ); }
if ( !J.getDim()) throw("KinVec jacobian information was lost. Some operators destroy this information. (e.g. cross)");
//Expression Jc; // SHOULD BE INTERMEDIATE STATE.
IntermediateState Jc((uint) 3,ddq_.getDim());
IntermediateState cc=c;
uint i;
std::map<uint,uint> di;
uint j;
uint count;
Matrix p=ddq.getDependencyPattern(ddq_);
for ( j = 0; j < ddq_.getDim(); j++ ){
for ( i = 0; i < ddq.getDim(); i++ ){
if (p(i,j)>0.0) {
di[i]=count;
count++;
}
}
}
for ( j = 0; j < ddq.getDim(); j++ ){
for ( i = 0; i < 3; i++ ){
if (di.count(j)) {
Jc(i,di[j])=J(i,j);
} else {
cc(i)+=J(i,j)*ddq(j);
}
}
}
printf("test\n");
Function ff;
ff << Jc;
FILE *myfile=fopen("debug.txt","w");
myfile << ff;
fclose(myfile);
Jc.getInverse();
return -Jc.getInverse()*cc;
}
Expression Frame::chain(Expression e_,const Frame &ei,bool type) const {
if (e_.getDim()==1 or e_.getDim()==0) return e_; // empty expression
//printf("Chain Expression %d x %d",e_.getNumRows(),e_.getNumCols());
FrameNode* common=getCommonFramePtr(ei);
std::vector<FrameNode*> thisChain= getFrameChain(common);
std::vector<FrameNode*> eiChain = ei.getFrameChain(common);
Expression e(e_);
for (std::vector<FrameNode*>::iterator it=thisChain.begin() ; it != thisChain.end(); ++it ) {
e=(*it)->R * e; // Going up
if (type) e=e+(*it)->p;
}
for (std::vector<FrameNode*>::reverse_iterator it=eiChain.rbegin() ; it != eiChain.rend(); ++it ) {
e=((*it)->R).transpose()*e; // Going down
if (type) e=e-((((*it)->R).transpose() * (*it)->p));
}
return e;
}
BooleanType FunctionEvaluationTree::isRationalIn( const Expression &variable ){
int nn = variable.getDim();
int run1;
BooleanType *implicit_dep = new BooleanType [n ];
VariableType *varType = new VariableType[nn];
int *component = new int [nn];
for( run1 = 0; run1 < nn; run1++ ){
Operator *tmp2 = (variable.element[run1])->clone();
if( tmp2->isVariable( varType[run1], component[run1] ) == BT_FALSE ){
ACADOERROR(RET_INVALID_ARGUMENTS);
delete tmp2 ;
delete[] varType ;
delete[] component;
return BT_TRUE ;
}
if( varType[run1] == VT_INTERMEDIATE_STATE ){
ACADOERROR(RET_INVALID_ARGUMENTS);
delete tmp2 ;
delete[] varType ;
delete[] component;
return BT_TRUE ;
}
delete tmp2;
}
for( run1 = 0; run1 < n; run1++ ){
implicit_dep[run1] = sub[run1]->isRationalIn( 1, varType, component, implicit_dep );
}
for( run1 = 0; run1 < dim; run1++ ){
if( f[run1]->isRationalIn( 1, varType, component, implicit_dep ) == BT_FALSE ){
delete[] implicit_dep;
delete[] varType ;
delete[] component;
return BT_FALSE;
}
}
delete[] implicit_dep;
delete[] varType ;
delete[] component;
return BT_TRUE;
}
uint ModelData::addOutput( const OutputFcn& outputEquation_ ){
if( rhs_name.isEmpty() && outputNames.size() == 0 ) {
Expression next;
outputEquation_.getExpression( next );
outputExpressions.push_back( next );
dim_outputs.push_back( next.getDim() );
}
else {
return ACADOERROR( RET_INVALID_OPTION );
}
return dim_outputs.size();
}
returnValue OCP::subjectTo( int _index, const DVector& _lb, const Expression& _expr, const DVector& _ub )
{
ASSERT(_index >= AT_START);
cout << _lb.getDim() << " " << _expr.getDim() << endl;
ASSERT(_lb.getDim() == _expr.getDim());
ASSERT(_lb.getDim() == _ub.getDim());
if (_index == AT_START)
{
for (unsigned el = 0; el < _lb.getDim(); ++el)
ACADO_TRY( constraint->add(0, _lb( el ), _expr( el ), _ub( el )) );
}
else if (_index == AT_END)
{
for (unsigned el = 0; el < _lb.getDim(); ++el)
ACADO_TRY(constraint->add(grid->getLastIndex(), _lb( el ), _expr( el ), _ub( el )) );
}
else
for (unsigned el = 0; el < _lb.getDim(); ++el)
constraint->add(_index, _lb( el ), _expr( el ), _ub( el ));
return SUCCESSFUL_RETURN;
}
returnValue DiscreteTimeExport::setDifferentialEquation( const Expression& rhs_ )
{
if( rhs_.getDim() > 0 ) {
OnlineData dummy0;
Control dummy1;
DifferentialState dummy2;
AlgebraicState dummy3;
DifferentialStateDerivative dummy4;
dummy0.clearStaticCounters();
dummy1.clearStaticCounters();
dummy2.clearStaticCounters();
dummy3.clearStaticCounters();
dummy4.clearStaticCounters();
NX2 = rhs_.getDim() - NXA;
x = DifferentialState("", NX1+NX2, 1);
z = AlgebraicState("", NXA, 1);
dx = DifferentialStateDerivative("", NDX, 1);
u = Control("", NU, 1);
od = OnlineData("", NOD, 1);
DifferentialEquation f;
f << rhs_;
DifferentialEquation g;
for( uint i = 0; i < rhs_.getDim(); i++ ) {
g << forwardDerivative( rhs_(i), x );
g << forwardDerivative( rhs_(i), u );
// There are not supposed to be algebraic states or differential state derivatives !
}
return (rhs.init(f, "rhs", NX, NXA, NU, NP, NDX, NOD) &
diffs_rhs.init(g, "diffs", NX, NXA, NU, NP, NDX, NOD));
}
return SUCCESSFUL_RETURN;
}
returnValue ModelData::setModel( const DifferentialEquation& _f )
{
if( rhs_name.isEmpty() && NX2 == 0 ) {
differentialEquation = _f;
Expression rhs;
differentialEquation.getExpression( rhs );
NX2 = rhs.getDim() - differentialEquation.getNXA();
if( NDX == 0 ) NDX = differentialEquation.getNDX();
NXA = differentialEquation.getNXA();
if( NU == 0 ) NU = differentialEquation.getNU();
if( NP == 0 ) NP = differentialEquation.getNP();
model_dimensions_set = BT_TRUE;
export_rhs = BT_TRUE;
}
else {
return ACADOERROR( RET_INVALID_OPTION );
}
return SUCCESSFUL_RETURN;
}
returnValue FunctionEvaluationTree::operator<<( const Expression& arg ){
safeCopy << arg;
uint run1;
for( run1 = 0; run1 < arg.getDim(); run1++ ){
int nn;
nn = n;
f = (Operator**)realloc(f,(dim+1)*sizeof(Operator*));
f[dim] = arg.element[run1]->clone();
f[dim]-> loadIndices ( indexList );
sub = (Operator**)realloc(sub,
(indexList->getNumberOfOperators())*sizeof(Operator*));
lhs_comp = (int*)realloc(lhs_comp,
(indexList->getNumberOfOperators())*sizeof(int));
indexList->getOperators( sub, lhs_comp, &n );
while( nn < n ){
sub[nn]-> enumerateVariables( indexList );
nn++;
}
f[dim]-> enumerateVariables( indexList );
dim++;
}
return SUCCESSFUL_RETURN;
}
uint ModelData::addOutput( const OutputFcn& outputEquation_, const Grid& grid ){
if( rhs_name.empty() && outputNames.size() == 0 ) {
Expression next;
outputEquation_.getExpression( next );
outputExpressions.push_back( next );
dim_outputs.push_back( next.getDim() );
if( NDX == 0 ) NDX = outputEquation_.getNDX();
if( NU == 0 ) NU = outputEquation_.getNU();
if( NP == 0 ) NP = outputEquation_.getNP();
if( NOD == 0 ) NOD = outputEquation_.getNOD();
outputGrids.push_back( grid );
uint numOuts = (int) ceil((double)grid.getNumIntervals());
num_meas.push_back( numOuts );
}
else {
return ACADOERROR( RET_INVALID_OPTION );
}
return dim_outputs.size();
}
/* >>> start tutorial code >>> */
int main( ) {
DifferentialState xT; // the trolley position
DifferentialState vT; // the trolley velocity
IntermediateState aT; // the trolley acceleration
DifferentialState xL; // the cable length
DifferentialState vL; // the cable velocity
IntermediateState aL; // the cable acceleration
DifferentialState phi; // the excitation angle
DifferentialState omega; // the angular velocity
DifferentialState uT; // trolley velocity control
DifferentialState uL; // cable velocity control
Control duT;
Control duL;
//
// DEFINE THE PARAMETERS:
//
const double tau1 = 0.012790605943772;
const double a1 = 0.047418203070092;
const double tau2 = 0.024695192379264;
const double a2 = 0.034087337273386;
const double g = 9.81;
const double c = 0.2;
const double m = 1318.0;
//
// DEFINE THE MODEL EQUATIONS:
//
DifferentialEquation f, f2, test_expr;
ExportAcadoFunction fun, fun2;
aT = -1.0 / tau1 * vT + a1 / tau1 * uT;
aL = -1.0 / tau2 * vL + a2 / tau2 * uL;
Expression states;
states << xT;
states << vT;
states << xL;
states << vL;
states << phi;
states << omega;
states << uT;
states << uL;
Expression controls;
controls << duT;
controls << duL;
Expression arg;
arg << states;
arg << controls;
int NX = states.getDim();
int NU = controls.getDim();
IntermediateState expr(2);
expr(0) = - 1.0/xL*(-g*sin(phi)-aT*cos(phi)-2*vL*omega-c*omega/(m*xL)) + log(duT/duL)*pow(xL,2);
expr(1) = - 1.0/xL*(-g*tan(phi)-aT*acos(phi)-2*atan(vL)*omega-c*asin(omega)/exp(xL)) + duT/pow(omega,3);
//~ expr(0) = - 1.0/xL*(-g*sin(phi));
//~ expr(1) = duT/pow(omega,3);
DifferentialState lambda("", expr.getDim(),1);
DifferentialState Sx("", states.getDim(),states.getDim());
DifferentialState Su("", states.getDim(),controls.getDim());
Expression S = Sx;
S.appendCols(Su);
// SYMMETRIC DERIVATIVES
Expression S_tmp = S;
S_tmp.appendRows(zeros<double>(NU,NX).appendCols(eye<double>(NU)));
IntermediateState dfS,dl;
Expression f_tmp = symmetricDerivative( expr, arg, S_tmp, lambda, &dfS, &dl );
f << returnLowerTriangular( f_tmp );
fun.init(f, "symmetricDerivative", NX*(1+NX+NU)+expr.getDim(), 0, 2, 0, 0, 0);
// ALTERNATIVE DERIVATIVES
IntermediateState tmp = backwardDerivative( expr, states, lambda );
IntermediateState tmp2 = forwardDerivative( tmp, states );
Expression tmp3 = backwardDerivative( expr, controls, lambda );
Expression tmp4 = multipleForwardDerivative( tmp3, states, Su );
Expression tmp5 = tmp4 + tmp4.transpose() + forwardDerivative( tmp3, controls );
Expression f2_tmp1;
f2_tmp1 = symmetricDoubleProduct( tmp2, Sx );
f2_tmp1.appendCols( zeros<double>(NX,NU) );
Expression f2_tmp2;
f2_tmp2 = Su.transpose()*tmp2*Sx + multipleForwardDerivative( tmp3, states, Sx );
f2_tmp2.appendCols( symmetricDoubleProduct( tmp2, Su ) + tmp5 );
f2_tmp1.appendRows( f2_tmp2 );
f2 << returnLowerTriangular( f2_tmp1 );
fun2.init(f2, "alternativeSymmetric", NX*(1+NX+NU)+expr.getDim(), 0, 2, 0, 0, 0);
Function f1;
f1 << dfS;
f1 << dl;
std::ofstream stream2( "ADtest/ADsymbolic_output2.c" );
stream2 << f1;
stream2.close();
std::ofstream stream( "ADtest/ADsymbolic_output.c" );
fun.exportCode( stream, "double" );
fun2.exportCode( stream, "double" );
test_expr << expr;
stream << test_expr;
stream.close();
return 0;
}
