// convert current matrix to NumericsMatrix structure
void OSNSMatrix::convert()
{
DEBUG_BEGIN("OSNSMatrix::convert()\n");
DEBUG_PRINTF("_storageType = %i\n", _storageType);
_numericsMat->storageType = _storageType;
_numericsMat->size0 = _dimRow;
_numericsMat->size1 = _dimColumn;
switch (_storageType)
{
case NM_DENSE:
{
_numericsMat->matrix0 = _M1->getArray(); // Pointer link
// _numericsMat->matrix1 = NULL; matrix1 is not set to NULL: we
// keep previous allocation. May be usefull if we switch between
// different storages during simu
break;
}
case NM_SPARSE_BLOCK:
{
_M2->convert();
_numericsMat->matrix1 = &*_M2->getNumericsMatSparse();
break;
}
case NM_SPARSE:
{
// we already filled the matrix
break;
}
default:
{
RuntimeException::selfThrow("OSNSMatrix::convert unknown _storageType");
}
}
DEBUG_END("OSNSMatrix::convert()\n");
}
OSNSMatrix::OSNSMatrix(unsigned int n, unsigned int m, int stor):
_dimRow(n), _dimColumn(m), _storageType(stor)
{
// Note:
// for _storageType = NM_DENSE (dense) n represents the real dimension of
// the matrix and for sparse storage (_storageType == 1) the number
// of interactionBlocks in a row or column.
DEBUG_BEGIN("OSNSMatrix::OSNSMatrix(unsigned int n, unsigned int m, int stor)\n");
switch(_storageType)
{
case NM_DENSE:
{
// A zero matrix M of size nXn is built. interactionBlocksPositions
// remains empty (=NULL) since we have no information concerning
// the Interaction.
_M1.reset(new SimpleMatrix(n, n));
break;
}
case NM_SPARSE_BLOCK:
{
_M2.reset(new BlockCSRMatrix(n));
break;
}
default: {} // do nothing here
}
_numericsMat.reset(new NumericsMatrix);
NM_null(_numericsMat.get());
DEBUG_END("OSNSMatrix::OSNSMatrix(unsigned int n, unsigned int m, int stor)\n");
}
//p_表示plugin，pi_表示plugin_info
//建立proto_graph并建立连接
void init_plugin_graph() {
//add plugin到graph
DEBUG_BEGIN();
vector<plugin_info>::iterator begin = plugin_info_list.begin();
cout << "plugin size:" << plugin_info_list.size() << endl;
for (; begin != plugin_info_list.end(); begin++) {
plugin_graph.add(begin->plug_in);
DEBUG("%s%s","pluin_info_vector add:",begin->plug_in.proto_name);
}
//link各个plugin
begin = plugin_info_list.begin();
for (; begin != plugin_info_list.end(); begin++) {
plugin& p = begin->plug_in;
const char *p_lower_name = p.lower_proto_name;
vector<plugin_info>::iterator pi_lower_itr;
pi_lower_itr = find_if(plugin_info_list.begin(), plugin_info_list.end(),
bind2nd(find_plugin_info(), p_lower_name));
if (pi_lower_itr != plugin_info_list.end()) {
plugin& p_lower = pi_lower_itr->plug_in;
plugin_graph.link(p_lower,p);
DEBUG("%s%s%s%s","proto_graph link:",p_lower.proto_name," ==> ",p.proto_name);
}
}
DEBUG_END();
}
// Basic constructor
BlockCSRMatrix::BlockCSRMatrix(SP::InteractionsGraph indexSet):
_nr(indexSet->size()),
_blockCSR(new CompressedRowMat(_nr, _nr)),
_sparseBlockStructuredMatrix(new SparseBlockStructuredMatrix()),
_diagsize0(new IndexInt(_nr)),
_diagsize1(new IndexInt(_nr)),
rowPos(new IndexInt(_nr)),
colPos(new IndexInt(_nr))
{
DEBUG_BEGIN("BlockCSRMatrix::BlockCSRMatrix(SP::InteractionsGraph indexSet)\n");
fill(indexSet);
DEBUG_END("BlockCSRMatrix::BlockCSRMatrix(SP::InteractionsGraph indexSet)\n");
}
void NewtonEulerR::initialize(Interaction& inter)
{
DEBUG_BEGIN("NewtonEulerR::initialize(Interaction& inter)\n");
unsigned int ySize = inter.dimension();
unsigned int xSize = inter.getSizeOfDS();
unsigned int qSize = 7 * (xSize / 6);
if (!_jachq)
_jachq.reset(new SimpleMatrix(ySize, qSize));
else
{
if (_jachq->size(0) == 0)
{
// if the matrix dim are null
_jachq->resize(ySize, qSize);
}
else
{
assert((_jachq->size(1) == qSize && _jachq->size(0) == ySize) ||
(printf("NewtonEuler::initializeWorkVectorsAndMatrices _jachq->size(1) = %d ,_qsize = %d , _jachq->size(0) = %d ,_ysize =%d \n", _jachq->size(1), qSize, _jachq->size(0), ySize) && false) ||
("NewtonEuler::initializeWorkVectorsAndMatrices inconsistent sizes between _jachq matrix and the interaction." && false));
}
}
DEBUG_EXPR(_jachq->display());
if (! _jachqT)
_jachqT.reset(new SimpleMatrix(ySize, xSize));
if (! _T)
{
_T.reset(new SimpleMatrix(7, 6));
_T->zero();
_T->setValue(0, 0, 1.0);
_T->setValue(1, 1, 1.0);
_T->setValue(2, 2, 1.0);
}
DEBUG_EXPR(_jachqT->display());
VectorOfBlockVectors& DSlink = inter.linkToDSVariables();
if (!_contactForce)
{
_contactForce.reset(new SiconosVector(DSlink[NewtonEulerR::p1]->size()));
_contactForce->zero();
}
DEBUG_END("NewtonEulerR::initialize(Interaction& inter)\n");
}
int
duf_sqlite_close( void )
{
int r3 = 0;
/* */ DEBUG_START( );
if ( pDb )
r3 = sqlite3_close_v2( pDb );
pDb = NULL;
DUF_TRACE( action, 0, "DB Close %s (%d)", r3 == SQLITE_OK ? "OK" : "FAIL", r3 );
r3 = sqlite3_shutdown( );
DUF_TRACE( action, 0, "DB Shutdown %s (%d)", r3 == SQLITE_OK ? "OK" : "FAIL", r3 );
/* */ DEBUG_END( );
return ( r3 );
}
void ControlZOHSimulation::run()
{
DEBUG_BEGIN("void ControlZOHSimulation::run()\n");
EventsManager& eventsManager = *_processSimulation->eventsManager();
unsigned k = 0;
boost::progress_display show_progress(_N);
boost::timer time;
time.restart();
TimeStepping& sim = static_cast<TimeStepping&>(*_processSimulation);
while (sim.hasNextEvent())
{
Event& nextEvent = *eventsManager.nextEvent();
if (nextEvent.getType() == TD_EVENT)
{
sim.computeOneStep();
}
sim.nextStep();
if (sim.hasNextEvent() && eventsManager.nextEvent()->getType() == TD_EVENT) // We store only on TD_EVENT
{
(*_dataM)(k, 0) = sim.startingTime();
storeData(k);
++k;
if (!_silent)
{
++show_progress;
}
}
}
/* saves last status */
(*_dataM)(k, 0) = sim.startingTime();
storeData(k);
++k;
_elapsedTime = time.elapsed();
_dataM->resize(k, _nDim + 1);
DEBUG_END("void ControlZOHSimulation::run()\n");
}
void BulletR::computeh(double time, BlockVector& q0, SiconosVector& y)
{
DEBUG_BEGIN("BulletR::computeh(...)\n");
NewtonEulerR::computeh(time, q0, y);
DEBUG_PRINT("start of computeh\n");
btVector3 posa = _contactPoints->getPositionWorldOnA();
btVector3 posb = _contactPoints->getPositionWorldOnB();
if (_flip) {
posa = _contactPoints->getPositionWorldOnB();
posb = _contactPoints->getPositionWorldOnA();
}
(*pc1())(0) = posa[0];
(*pc1())(1) = posa[1];
(*pc1())(2) = posa[2];
(*pc2())(0) = posb[0];
(*pc2())(1) = posb[1];
(*pc2())(2) = posb[2];
{
y.setValue(0, _contactPoints->getDistance());
(*nc())(0) = _contactPoints->m_normalWorldOnB[0] * (_flip ? -1 : 1);
(*nc())(1) = _contactPoints->m_normalWorldOnB[1] * (_flip ? -1 : 1);
(*nc())(2) = _contactPoints->m_normalWorldOnB[2] * (_flip ? -1 : 1);
}
DEBUG_PRINTF("distance : %g\n", y.getValue(0));
DEBUG_PRINTF("position on A : %g,%g,%g\n", posa[0], posa[1], posa[2]);
DEBUG_PRINTF("position on B : %g,%g,%g\n", posb[0], posb[1], posb[2]);
DEBUG_PRINTF("normal on B : %g,%g,%g\n", (*nc())(0), (*nc())(1), (*nc())(2));
DEBUG_END("BulletR::computeh(...)\n");
}
int
duf_sqlite_open( const char *dbpath )
{
int r3 = 0;
/* */ DEBUG_START( );
if ( !pDb )
{
r3 = sqlite3_initialize( );
DUF_TRACE( action, 0, "DB Initialize %s (%d)", r3 == SQLITE_OK ? "OK" : "FAIL", r3 );
if ( r3 == SQLITE_OK )
{
/* r3 = sqlite3_open( dbpath, &pDb ); */
r3 = sqlite3_open_v2( dbpath, &pDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, NULL );
sqlite3_extended_result_codes( pDb, 1 );
DUF_TRACE( action, 0, "DB Open %s %s (%d)", dbpath, r3 == SQLITE_OK ? "OK" : "FAIL", r3 );
}
}
/* */ DEBUG_END( );
return ( r3 );
}
void KneeJointR::computeh(double time, BlockVector& q0, SiconosVector& y)
{
DEBUG_BEGIN("KneeJointR::computeh(double time, BlockVector& q0, SiconosVector& y)\n");
DEBUG_EXPR(q0.display());
double X1 = q0.getValue(0);
double Y1 = q0.getValue(1);
double Z1 = q0.getValue(2);
double q10 = q0.getValue(3);
double q11 = q0.getValue(4);
double q12 = q0.getValue(5);
double q13 = q0.getValue(6);
DEBUG_PRINTF("X1 = %12.8e,\t Y1 = %12.8e,\t Z1 = %12.8e,\n",X1,Y1,Z1);
DEBUG_PRINTF("q10 = %12.8e,\t q11 = %12.8e,\t q12 = %12.8e,\t q13 = %12.8e,\n",q10,q11,q12,q13);
double X2 = 0;
double Y2 = 0;
double Z2 = 0;
double q20 = 1;
double q21 = 0;
double q22 = 0;
double q23 = 0;
if(q0.getNumberOfBlocks()>1)
{
// SP::SiconosVector x2 = _d2->q();
// DEBUG_EXPR( _d2->q()->display(););
X2 = q0.getValue(7);
Y2 = q0.getValue(8);
Z2 = q0.getValue(9);
q20 = q0.getValue(10);
q21 = q0.getValue(11);
q22 = q0.getValue(12);
q23 = q0.getValue(13);
}
y.setValue(0, Hx(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
y.setValue(1, Hy(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
y.setValue(2, Hz(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
DEBUG_EXPR(y.display());
DEBUG_END("KneeJointR::computeh(double time, BlockVector& q0, SiconosVector& y)\n");
}
void KneeJointR::computeJachq(double time, Interaction& inter, SP::BlockVector q0)
{
DEBUG_BEGIN("KneeJointR::computeJachq(double time, Interaction& inter, SP::BlockVector q0) \n");
_jachq->zero();
SP::SiconosVector q1 = (q0->getAllVect())[0];
double X1 = q1->getValue(0);
double Y1 = q1->getValue(1);
double Z1 = q1->getValue(2);
double q10 = q1->getValue(3);
double q11 = q1->getValue(4);
double q12 = q1->getValue(5);
double q13 = q1->getValue(6);
double X2 = 0;
double Y2 = 0;
double Z2 = 0;
double q20 = 1;
double q21 = 0;
double q22 = 0;
double q23 = 0;
if(q0->getNumberOfBlocks()>1)
{
SP::SiconosVector q2 = (q0->getAllVect())[1];
X2 = q2->getValue(0);
Y2 = q2->getValue(1);
Z2 = q2->getValue(2);
q20 = q2->getValue(3);
q21 = q2->getValue(4);
q22 = q2->getValue(5);
q23 = q2->getValue(6);
Jd1d2(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23);
}
else
Jd1(X1, Y1, Z1, q10, q11, q12, q13);
DEBUG_END("KneeJointR::computeJachq(double time, Interaction& inter, SP::BlockVector q0 ) \n");
}
void KneeJointR::computeDotJachq(double time, SP::SiconosVector qdot1, SP::SiconosVector qdot2 )
{
DEBUG_BEGIN("KneeJointR::computeDotJachq(double time, SP::SiconosVector qdot1, SP::SiconosVector qdot2) \n");
_dotjachq->zero();
double Xdot1 = qdot1->getValue(0);
double Ydot1 = qdot1->getValue(1);
double Zdot1 = qdot1->getValue(2);
double qdot10 = qdot1->getValue(3);
double qdot11 = qdot1->getValue(4);
double qdot12 = qdot1->getValue(5);
double qdot13 = qdot1->getValue(6);
double Xdot2 = 0;
double Ydot2 = 0;
double Zdot2 = 0;
double qdot20 = 1;
double qdot21 = 0;
double qdot22 = 0;
double qdot23 = 0;
if(qdot2)
{
Xdot2 = qdot2->getValue(0);
Ydot2 = qdot2->getValue(1);
Zdot2 = qdot2->getValue(2);
qdot20 = qdot2->getValue(3);
qdot21 = qdot2->getValue(4);
qdot22 = qdot2->getValue(5);
qdot23 = qdot2->getValue(6);
DotJd1d2(Xdot1, Ydot1, Zdot1, qdot10, qdot11, qdot12, qdot13, Xdot2, Ydot2, Zdot2, qdot20, qdot21, qdot22, qdot23);
}
else
DotJd1(Xdot1, Ydot1, Zdot1, qdot10, qdot11, qdot12, qdot13);
DEBUG_END("KneeJointR::computeDotJachq(double time, SP::SiconosVector qdot1, SP::SiconosVector qdot2 ) \n");
}
/* ------------------------------------- *\
Subroutine used to kill DDI processes
\* ------------------------------------- */
void Filicide() {
char c=0;
int i,nsocks = gv(nprocs);
int *sockets = gv(sockets);
if(USING_DATA_SERVERS()) nsocks *= 2;
if(sockets == NULL) {
fprintf(stdout," ddikick.x: No DDI processes to kill.\n");
} else {
fprintf(stdout," ddikick.x: Sending kill signal to DDI processes.\n");
for(i=0; i<nsocks; i++) {
if(sockets[i] < 0) continue;
# if DDI_DEBUG
DEBUG_START(DEBUG_MAX)
fprintf(stdout," ddikick.x: Sending kill signal to DDI process %i.\n",i);
DEBUG_END()
# endif
send(sockets[i],&c,1,MSG_OOB);
close(sockets[i]);
sockets[i] = -1;
}
}
}
void SlidingReducedOrderObserver::initialize(const NonSmoothDynamicalSystem& nsds,
const Simulation& s)
{
DEBUG_BEGIN(" SlidingReducedOrderObserver::initialize(const NonSmoothDynamicalSystem& nsds, const Simulation& s)\n");
if (!_C)
{
RuntimeException::selfThrow("SlidingReducedOrderObserver::initialize - you have to set C before initializing the Observer");
}
else
{
Observer::initialize(nsds, s);
}
bool isDSinDSG0 = true;
DynamicalSystemsGraph& originalDSG0 = *nsds.topology()->dSG(0);
DynamicalSystemsGraph::VDescriptor originaldsgVD;
if (!_DS) // No DynamicalSystem was given
{
// We can only work with FirstOrderNonLinearDS, FirstOrderLinearDS and FirstOrderLinearTIDS
// We can use the Visitor mighty power to check if we have the right type
DynamicalSystem& observedDS = *_sensor->getDS();
Type::Siconos dsType;
dsType = Type::value(observedDS);
// create the DS for the controller
// if the DS we use is different from the DS we are controlling
// when we want for instant to see how well the controller behaves
// if the plant model is not exact, we can use the setSimulatedDS
// method
if (dsType == Type::FirstOrderLinearDS)
{
DEBUG_PRINT("dsType == Type::FirstOrderLinearDS\n");
_DS.reset(new FirstOrderLinearDS(static_cast<FirstOrderLinearDS&>(observedDS)));
}
else if (dsType == Type::FirstOrderLinearTIDS)
{
DEBUG_PRINT("dsType == Type::FirstOrderLinearTIDS\n");
_DS.reset(new FirstOrderLinearTIDS(static_cast<FirstOrderLinearTIDS&>(observedDS)));
}
else
RuntimeException::selfThrow("SlidingReducedOrderObserver is not yet implemented for system of type" + dsType);
// is it controlled ?
originaldsgVD = originalDSG0.descriptor(_sensor->getDS());
}
else
{
// is it controlled ?
if (originalDSG0.is_vertex(_DS))
originaldsgVD = originalDSG0.descriptor(_DS);
else
isDSinDSG0 = false;
}
// Initialize with the guessed state
_DS->setX0Ptr(_xHat);
_DS->resetToInitialState();
_e.reset(new SiconosVector(_C->size(0)));
_y.reset(new SiconosVector(_C->size(0)));
double t0 = nsds.t0();
double h = s.currentTimeStep();
double T = nsds.finalT() + h;
_nsds.reset(new NonSmoothDynamicalSystem(t0, T));
_integrator.reset(new ZeroOrderHoldOSI());
std11::static_pointer_cast<ZeroOrderHoldOSI>(_integrator)->setExtraAdditionalTerms(
std11::shared_ptr<ControlZOHAdditionalTerms>(new ControlZOHAdditionalTerms()));
_nsds->insertDynamicalSystem(_DS);
// Add the necessary properties
DynamicalSystemsGraph& DSG0 = *_nsds->topology()->dSG(0);
DynamicalSystemsGraph::VDescriptor dsgVD = DSG0.descriptor(_DS);
// Observer part
DSG0.L[dsgVD] = _L;
DSG0.e[dsgVD] = _e;
// Was the original DynamicalSystem controlled ?
if (isDSinDSG0 && originalDSG0.B.hasKey(originaldsgVD))
{
DSG0.B[dsgVD] = originalDSG0.B[originaldsgVD];
assert(originalDSG0.u[originaldsgVD] && "A DynamicalSystem is controlled but its control input has not been initialized yet");
DSG0.u[dsgVD] = originalDSG0.u[originaldsgVD];
}
// all necessary things for simulation
_simulation.reset(new TimeStepping(_nsds, _td, 0));
_simulation->setName("Internal simultion of SlidingReducedOrderObserver");
_simulation->associate(_integrator, _DS);
// initialize error
*_y = _sensor->y();
DEBUG_END(" SlidingReducedOrderObserver::initialize(const NonSmoothDynamicalSystem& nsds, const Simulation& s)\n");
}
void Kickoff_PBS(const Node_info *ddinodes,const Cmdline_info *info) {
char ddiinfo[] = "-ddi";
char procid[8];
char portid[8];
char nodeid[8];
char snodes[8];
char sprocs[8];
char **rargs;
char **argv = info->argv;
int i,j,r,iarg,nargs = info->ddiarg + info->nnodes + 8;
int inode,ncpus,np = info->nprocs;
int ntests;
if(info->nnodes == 1) return;
int tm_errno;
tm_task_id *tid;
tm_event_t *spawn;
tm_event_t polled;
struct tm_roots roots;
tm_node_id *nodelist;
/* ---------------------------------- *\
Initialize PBS Task Management API
\* ---------------------------------- */
if(tm_init(0, &roots) != TM_SUCCESS) {
fprintf(stderr, " ddikick.x: tm_init failed\n");
Fatal_error(911);
}
if(tm_nodeinfo(&nodelist, &np) != TM_SUCCESS) {
fprintf(stderr, " ddikick.x: tm_nodeinfo failed.\n");
Fatal_error(911);
}
tid = (tm_task_id *) Malloc(2*np*sizeof(tm_task_id));
spawn = (tm_event_t *) Malloc(2*np*sizeof(tm_event_t));
for(i=0; i<2*np; i++) {
*(tid + i) = TM_NULL_TASK;
*(spawn + i) = TM_NULL_EVENT;
}
/* ----------------------------------------- *\
Initialize arguments to kickoff DDI tasks
\* ----------------------------------------- */
rargs = (char **) Malloc(nargs*sizeof(char*));
sprintf(portid, "%d", info->kickoffport);
sprintf(snodes, "%d", info->nnodes);
sprintf(sprocs, "%d", info->nprocs);
for(i=1,r=0; i<info->ddiarg-1; i++) rargs[r++] = argv[i];
rargs[r++] = ddiinfo;
rargs[r++] = info->kickoffhost; /* kickoff host name */
rargs[r++] = portid; /* kickoff port number */
rargs[r++] = nodeid; /* rank of this node */
rargs[r++] = procid; /* rank of this process */
rargs[r++] = snodes; /* number of nodes */
rargs[r++] = sprocs; /* number of processors */
for(i=0,iarg=info->nodearg; i<info->nnodes; i++,iarg++) {
rargs[r++] = argv[iarg];
}
rargs[r] = NULL;
/* ------------------------ *\
Spawn DDI tasks to nodes
\* ------------------------ */
ncpus=ddinodes[0].cpus+ddinodes[1].cpus;
for(i=ddinodes[0].cpus,inode=1; i<np; i++) {
if(i == ncpus) ncpus += ddinodes[++inode].cpus;
sprintf(nodeid,"%d",inode);
sprintf(procid,"%d",i);
# if DDI_DEBUG
DEBUG_START(DEBUG_MAX)
fprintf(stdout,"DDI Process %i PBS tm_spawn arguments: ",i);
for(iarg=0; iarg<r; iarg++) fprintf(stdout,"%s ",rargs[iarg]);
fprintf(stdout,"\n");
DEBUG_END()
# endif
/* ------------------------- *\
Spawn DDI Compute Process
\* ------------------------- */
if(tm_spawn(r,rargs,NULL,*(nodelist+i),(tid+i),spawn+i) != TM_SUCCESS) {
fprintf(stderr," ddikick.x: tm_spawn failed.\n");
Fatal_error(911);
}
/* ---------------------------------- *\
No data server on single node runs
\* ---------------------------------- */
if(info->nnodes == 1) continue;
# if DDI_DEBUG
DEBUG_START(DEBUG_MAX)
fprintf(stdout,"DDI Process %i PBS tm_spawn arguments: ",j);
for(iarg=0; iarg<r; iarg++) fprintf(stdout,"%s ",rargs[iarg]);
fprintf(stdout,"\n");
DEBUG_END()
# endif
j = i+np;
sprintf(procid,"%d",j);
/* --------------------- *\
Spawn DDI Data Server
\* --------------------- */
if(tm_spawn(r,rargs,NULL,*(nodelist+i),(tid+j),spawn+j) != TM_SUCCESS) {
fprintf(stderr," ddikick.x: tm_spawn failed.\n");
Fatal_error(911);
} }
/* -------------------------------------------------------- *\
Poll PBS to ensure each DDI process started successfully
\* -------------------------------------------------------- */
ntests = np-ddinodes[0].cpus;
if(USING_DATA_SERVERS()) ntests *= 2;
for(i=ntests; i--; ) {
if(tm_poll(TM_NULL_EVENT,&polled,1,&tm_errno) != TM_SUCCESS) {
fprintf(stderr," ddikick.x: tm_poll failed.\n");
Fatal_error(911);
}
for(j=0; j<np; j++) {
if(polled == *(spawn+j)) {
if(tm_errno) {
fprintf(stderr," ddikick.x: error spawning DDI task %i.\n",j);
Fatal_error(911);
} else {
# if DDI_DEBUG
DEBUG_START(DEBUG_MAX)
fprintf(stdout," ddikick.x: DDI task %i started.\n",j);
DEBUG_END()
# endif
} }
if(info->nnodes == 1) continue;
if(polled == *(spawn+j+np)) {
if(tm_errno) {
fprintf(stderr," ddikick.x: error spawning DDI task %i.\n",j+np);
Fatal_error(911);
} else {
# if DDI_DEBUG
DEBUG_START(DEBUG_MAX)
fprintf(stdout," ddikick.x: DDI task %i started.\n",j+np);
DEBUG_END()
# endif
} } } }
/* -------------------------------------- *\
Close the link to the PBS Task Manager
\* -------------------------------------- */
tm_finalize();
/* ---------------- *\
Free used memory
\* ---------------- */
free(tid);
free(spawn);
free(rargs);
}
// Fill the matrix
void OSNSMatrix::fill(SP::InteractionsGraph indexSet, bool update)
{
DEBUG_BEGIN("void OSNSMatrix::fill(SP::InteractionsGraph indexSet, bool update)\n");
assert(indexSet);
if (update)
{
// Computes _dimRow and interactionBlocksPositions according to indexSet
_dimColumn = updateSizeAndPositions(indexSet);
_dimRow = _dimColumn;
}
if (_storageType == NM_DENSE)
{
// === Memory allocation, if required ===
// Mem. is allocate only if !M or if its size has changed.
if (update)
{
if (! _M1)
_M1.reset(new SimpleMatrix(_dimRow, _dimColumn));
else
{
if (_M1->size(0) != _dimRow || _M1->size(1) != _dimColumn)
_M1->resize(_dimRow, _dimColumn);
_M1->zero();
}
}
// ======> Aim: find inter1 and inter2 both in indexSet and which have
// common DynamicalSystems. Then get the corresponding matrix
// from map interactionBlocks, and copy it into M
unsigned int pos = 0, col = 0; // index position used for
// interactionBlock copy into M, see
// below.
// === Loop through "active" Interactions (ie present in
// indexSets[level]) ===
InteractionsGraph::VIterator vi, viend;
for (std11::tie(vi, viend) = indexSet->vertices();
vi != viend; ++vi)
{
SP::Interaction inter = indexSet->bundle(*vi);
pos = inter->absolutePosition();
std11::static_pointer_cast<SimpleMatrix>(_M1)
->setBlock(pos, pos, *indexSet->properties(*vi).block);
DEBUG_PRINTF("OSNSMatrix _M1: %i %i\n", _M1->size(0), _M1->size(1));
DEBUG_PRINTF("OSNSMatrix block: %i %i\n", indexSet->properties(*vi).block->size(0), indexSet->properties(*vi).block->size(1));
}
InteractionsGraph::EIterator ei, eiend;
for (std11::tie(ei, eiend) = indexSet->edges();
ei != eiend; ++ei)
{
InteractionsGraph::VDescriptor vd1 = indexSet->source(*ei);
InteractionsGraph::VDescriptor vd2 = indexSet->target(*ei);
SP::Interaction inter1 = indexSet->bundle(vd1);
SP::Interaction inter2 = indexSet->bundle(vd2);
pos = inter1->absolutePosition();
assert(indexSet->is_vertex(inter2));
col = inter2->absolutePosition();
assert(pos < _dimRow);
assert(col < _dimColumn);
DEBUG_PRINTF("OSNSMatrix _M1: %i %i\n", _M1->size(0), _M1->size(1));
DEBUG_PRINTF("OSNSMatrix upper: %i %i\n", indexSet->properties(*ei).upper_block->size(0), indexSet->properties(*ei).upper_block->size(1));
DEBUG_PRINTF("OSNSMatrix lower: %i %i\n", indexSet->properties(*ei).lower_block->size(0), indexSet->properties(*ei).lower_block->size(1));
assert(indexSet->properties(*ei).lower_block);
assert(indexSet->properties(*ei).upper_block);
std11::static_pointer_cast<SimpleMatrix>(_M1)
->setBlock(std::min(pos, col), std::max(pos, col),
*indexSet->properties(*ei).upper_block);
std11::static_pointer_cast<SimpleMatrix>(_M1)
->setBlock(std::max(pos, col), std::min(pos, col),
*indexSet->properties(*ei).lower_block);
}
}
else if (_storageType == NM_SPARSE_BLOCK)
{
if (! _M2)
{
DEBUG_PRINT("Reset _M2 shared pointer using new BlockCSRMatrix(indexSet) \n ");
_M2.reset(new BlockCSRMatrix(indexSet));
}
else
{
DEBUG_PRINT("fill existing _M2\n");
_M2->fill(indexSet);
}
}
if (update)
convert();
DEBUG_END("void OSNSMatrix::fill(SP::InteractionsGraph indexSet, bool update)\n");
}
int main(int argc, char *argv[])
{
RedditUserLogged *user = NULL;
char *subreddit = NULL;
char *password = NULL, *username = NULL;
optParser parser;
DEBUG_START(DEBUG_FILE, DEBUG_FILENAME);
memset(&parser, 0, sizeof(optParser));
parser.argc = argc;
parser.argv = argv;
optAddOptions (&parser, mainOptions, MOPT_ARG_COUNT);
handleArguments(&parser);
if (mainOptions[MOPT_HELP].isSet) {
displayHelp(&parser);
return 0;
}
optClearParser(&parser);
setlocale(LC_CTYPE, "");
initscr();
raw();//We want character for character input
keypad(stdscr,1);//Enable extra keys like arrowkeys
noecho();
start_color();
use_default_colors();
init_pair(1, -1, -1);
init_pair(2, COLOR_BLACK, COLOR_WHITE);
DEBUG_PRINT(L"Starting...\n");
/* Start libreddit */
redditGlobalInit();
globalState = redditStateNew();
globalState->userAgent = redditCopyString("cReddit/0.0.1");
redditStateSet(globalState);
if (mainOptions[MOPT_USERNAME].isSet) {
username = mainOptions[MOPT_USERNAME].svalue;
if (!mainOptions[MOPT_PASSWORD].isSet)
password = getPassword();
else
password = mainOptions[MOPT_PASSWORD].svalue;
user = redditUserLoggedNew();
redditUserLoggedLogin(user, username, password);
/* Don't want to leave that important Reddit password in memory */
memset(password, 0, strlen(password));
if (!mainOptions[MOPT_PASSWORD].isSet)
free(password);
}
if (mainOptions[MOPT_SUBREDDIT].isSet) {
subreddit = mainOptions[MOPT_SUBREDDIT].svalue;
if (!startsWith("/r/", subreddit) && strcmp("/", subreddit) != 0)
prepend("/r/", subreddit);
} else {
subreddit = "/";
}
showSubreddit(subreddit);
redditUserLoggedFree(user);
redditStateFree(globalState);
redditGlobalCleanup();
endwin();
DEBUG_END(DEBUG_FILE);
return 0;
}
/*
* Select on the list of fds.
* Returns: -1 = error
* 0 = timeout or nothing to select
* >0 = number of signaled fds
*/
int
_gpgme_io_select(struct io_select_fd_s *fds, size_t nfds, int nonblock)
{
fd_set readfds;
fd_set writefds;
unsigned int i;
int any, max_fd, n, count;
struct timeval timeout = { 1, 0 }; /* Use a 1s timeout. */
void *dbg_help = NULL;
FD_ZERO(&readfds);
FD_ZERO(&writefds);
max_fd = 0;
if(nonblock)
timeout.tv_sec = 0;
DEBUG_BEGIN(dbg_help, 3, "gpgme:select on [ ");
any = 0;
for(i = 0; i < nfds; i++)
{
if(fds[i].fd == -1)
continue;
if(fds[i].frozen)
DEBUG_ADD1(dbg_help, "f%d ", fds[i].fd);
else if(fds[i].for_read)
{
assert(!FD_ISSET(fds[i].fd, &readfds));
FD_SET(fds[i].fd, &readfds);
if(fds[i].fd > max_fd)
max_fd = fds[i].fd;
DEBUG_ADD1(dbg_help, "r%d ", fds[i].fd);
any = 1;
}
else if(fds[i].for_write)
{
assert(!FD_ISSET(fds[i].fd, &writefds));
FD_SET(fds[i].fd, &writefds);
if(fds[i].fd > max_fd)
max_fd = fds[i].fd;
DEBUG_ADD1(dbg_help, "w%d ", fds[i].fd);
any = 1;
}
fds[i].signaled = 0;
}
DEBUG_END(dbg_help, "]");
if(!any)
return 0;
do
{
count = _gpgme_ath_select(max_fd + 1, &readfds, &writefds, NULL,
&timeout);
}
while(count < 0 && errno == EINTR);
if(count < 0)
{
int saved_errno = errno;
DEBUG1("_gpgme_io_select failed: %s\n", strerror(errno));
errno = saved_errno;
return -1; /* error */
}
DEBUG_BEGIN(dbg_help, 3, "select OK [ ");
if(DEBUG_ENABLED(dbg_help))
{
for(i = 0; i <= max_fd; i++)
{
if(FD_ISSET(i, &readfds))
DEBUG_ADD1(dbg_help, "r%d ", i);
if(FD_ISSET(i, &writefds))
DEBUG_ADD1(dbg_help, "w%d ", i);
}
DEBUG_END(dbg_help, "]");
}
/* n is used to optimize it a little bit. */
for(n = count, i = 0; i < nfds && n; i++)
{
if(fds[i].fd == -1)
;
else if(fds[i].for_read)
{
if(FD_ISSET(fds[i].fd, &readfds))
{
fds[i].signaled = 1;
n--;
}
}
else if(fds[i].for_write)
{
if(FD_ISSET(fds[i].fd, &writefds))
{
fds[i].signaled = 1;
n--;
}
}
}
return count;
}
/*
* Select on the list of fds.
* Returns: -1 = error
* 0 = timeout or nothing to select
* >0 = number of signaled fds
*/
int
_gpgme_io_select ( struct io_select_fd_s *fds, size_t nfds, int nonblock )
{
HANDLE waitbuf[MAXIMUM_WAIT_OBJECTS];
int waitidx[MAXIMUM_WAIT_OBJECTS];
int code, nwait;
int i, any;
int count;
void *dbg_help;
restart:
DEBUG_BEGIN (dbg_help, 3, "select on [ ");
any = 0;
nwait = 0;
count = 0;
for ( i=0; i < nfds; i++ ) {
if ( fds[i].fd == -1 )
continue;
fds[i].signaled = 0;
if ( fds[i].for_read || fds[i].for_write ) {
if ( fds[i].frozen ) {
DEBUG_ADD1 (dbg_help, "f%d ", fds[i].fd );
}
else if ( fds[i].for_read ) {
struct reader_context_s *c = find_reader (fds[i].fd,1);
if (!c) {
DEBUG1 ("oops: no reader thread for fd %d", fds[i].fd);
}
else {
if ( nwait >= DIM (waitbuf) ) {
DEBUG_END (dbg_help, "oops ]");
DEBUG0 ("Too many objects for WFMO!" );
return -1;
}
waitidx[nwait] = i;
waitbuf[nwait++] = c->have_data_ev;
}
DEBUG_ADD1 (dbg_help, "r%d ", fds[i].fd );
any = 1;
}
else if ( fds[i].for_write ) {
struct writer_context_s *c = find_writer (fds[i].fd,1);
if (!c) {
DEBUG1 ("oops: no writer thread for fd %d", fds[i].fd);
}
else {
if ( nwait >= DIM (waitbuf) ) {
DEBUG_END (dbg_help, "oops ]");
DEBUG0 ("Too many objects for WFMO!" );
return -1;
}
waitidx[nwait] = i;
waitbuf[nwait++] = c->is_empty;
}
DEBUG_ADD1 (dbg_help, "w%d ", fds[i].fd );
any = 1;
}
}
}
DEBUG_END (dbg_help, "]");
if (!any)
return 0;
code = WaitForMultipleObjects ( nwait, waitbuf, 0, nonblock ? 0 : 1000);
if ( code >= WAIT_OBJECT_0 && code < WAIT_OBJECT_0 + nwait ) {
/* This WFMO is a really silly function: It does return either
* the index of the signaled object or if 2 objects have been
* signalled at the same time, the index of the object with the
* lowest object is returned - so and how do we find out
* how many objects have been signaled???.
* The only solution I can imagine is to test each object starting
* with the returned index individually - how dull.
*/
any = 0;
for (i=code - WAIT_OBJECT_0; i < nwait; i++ ) {
if (WaitForSingleObject (waitbuf[i], 0) == WAIT_OBJECT_0) {
assert (waitidx[i] >=0 && waitidx[i] < nfds);
fds[waitidx[i]].signaled = 1;
any = 1;
count++;
}
}
if (!any) {
DEBUG0 ("Oops: No signaled objects found after WFMO");
count = -1;
}
}
else if ( code == WAIT_TIMEOUT ) {
DEBUG0 ("WFMO timed out\n" );
}
else if (code == WAIT_FAILED ) {
int le = (int)GetLastError ();
if ( le == ERROR_INVALID_HANDLE ) {
int k, j = handle_to_fd (waitbuf[i]);
DEBUG1 ("WFMO invalid handle %d removed\n", j);
for (k=0 ; k < nfds; k++ ) {
if ( fds[k].fd == j ) {
fds[k].for_read = fds[k].for_write = 0;
goto restart;
}
}
DEBUG0 (" oops, or not???\n");
}
DEBUG1 ("WFMO failed: %d\n", le );
count = -1;
}
else {
DEBUG1 ("WFMO returned %d\n", code );
count = -1;
}
if ( count ) {
DEBUG_BEGIN (dbg_help, 3, " signaled [ ");
for ( i=0; i < nfds; i++ ) {
if ( fds[i].fd == -1 )
continue;
if ( (fds[i].for_read || fds[i].for_write) && fds[i].signaled ) {
DEBUG_ADD2 (dbg_help, "%c%d ",
fds[i].for_read? 'r':'w',fds[i].fd );
}
}
DEBUG_END (dbg_help, "]");
}
return count;
}
/**
* Turn servo towards 'pos' in 1 microsecond steps, waiting delay_ms
* milliseconds between steps (speed = 1/delay). If check_weight weight
* is true, might abort with WHERE_THE_FUCK_IS_THE_CUP error. If a valid pointer
* stable_weight is passed, turns bottle until a stable weight is measured
* (returns WEIGHT_NOT_STABLE if pos is reached before weight stable).
*
* Returns 0 when the position is reached or SERVO_OUT_OF_RANGE on error.
*
* For details about the built-in Servo class see:
* /usr/share/arduino/libraries/Servo/Servo.cpp
*
*/
errv_t Bottle::turn_to(int pos, int delay_ms, bool check_weight, int* stable_weight, bool enable_abortcheck) {
int weight_previous1 = -9999; // just any impossible value
int weight_previous2 = -9999; // ..before we have real values
if (pos < SERVO_MIN || pos > SERVO_MAX) {
DEBUG_MSG_LN("Invalid pos");
return SERVO_OUT_OF_RANGE;
}
int current_pos = servo.readMicroseconds();
if (pos == current_pos)
return 0;
int step = (current_pos < pos) ? 1 : -1;
DEBUG_START();
DEBUG_MSG("turn ");
DEBUG_MSG(number);
DEBUG_MSG(", params ");
DEBUG_VAL(current_pos);
DEBUG_VAL(step);
DEBUG_VAL(pos);
DEBUG_VAL(delay_ms);
DEBUG_END();
unsigned long last_called = millis();
for (int i = current_pos + step; i * step <= pos * step; i += step) {
// ˆˆˆˆˆˆ ˆˆˆˆˆˆ
// this inverts the relation if turning down
// Warning: printing to serial delays turning!
// Might help to to debug servo movement. Not necessary now, commenting
// out to save bytes.
//if (print_steps && i % 10 == 0) {
// DEBUG_VAL_LN(i);
//}
// check abort only if not already aborted...
if (enable_abortcheck) {
// turn up and return if we should abort...
errv_t ret = check_aborted();
if (ret) {
// turn_up might not be necessary here, called another time
// later (does not matter if called twice)
turn_up(FAST_TURN_UP_DELAY, false);
return ret;
}
}
if (check_weight || stable_weight) {
int weight;
int ret = ads1231_get_noblock(weight);
if (ret == 0) {
// we got a valid weight from scale
if (check_weight && weight < WEIGHT_EPSILON) {
return WHERE_THE_FUCK_IS_THE_CUP;
}
// get next weight sample and return if weight is stable
if (stable_weight) {
if (weight_previous2 == weight_previous1
&& weight_previous1 == weight) {
*stable_weight = weight;
return 0;
}
weight_previous2 = weight_previous1;
weight_previous1 = weight;
}
}
else if (ret != ADS1231_WOULD_BLOCK) {
// ignoring if it would take too long to get weight, but
// return in case of other error != 0
return ret;
}
}
// turn servo one step
delay(delay_ms);
servo.writeMicroseconds(i);
}
// pos reached before weight stable
if (stable_weight) {
return WEIGHT_NOT_STABLE;
}
return 0;
}
/**
* Pour requested_amount grams from bottle..
* Return 0 on success, other values are return values of
* delay_until (including scale error codes).
*/
errv_t Bottle::pour(int requested_amount, int& measured_amount) {
// orig_weight is weight including ingredients poured until now
int orig_weight, ret;
while (1) {
// get weight while turning bottle, because ads1231_stable_millis()
// blocks bottle in pause position too long
int below_pause = (pos_down + get_pause_pos()) / 2;
ret = turn_to(below_pause, TURN_DOWN_DELAY, true, &orig_weight);
// Note that checking weight here is a critical issue, allows hacking
// the robot. If a heavy weight is placed while measuring and then
// removed while pouring (results in more alcohol). Stable weight
// should resolve most problems.
if (ret == WEIGHT_NOT_STABLE) {
ret = ads1231_get_stable_grams(orig_weight);
}
if (ret != 0 && ret != WHERE_THE_FUCK_IS_THE_CUP) {
return ret;
}
if (ret == WHERE_THE_FUCK_IS_THE_CUP || orig_weight < WEIGHT_EPSILON) {
// no cup...
RETURN_IFN_0(wait_for_cup());
}
else {
// everything fine
break;
}
}
// loop until successfully poured or aborted or other fatal error
while(1) {
// petres wants POURING message also after resume...
// https://github.com/rfjakob/barwin-arduino/issues/10
MSG(String("POURING ") + String(number) + String(" ") + String(orig_weight));
DEBUG_MSG_LN("Turn down");
ret = turn_down(TURN_DOWN_DELAY, true); // enable check_weight
// wait for requested weight
// FIXME here we do not want WEIGHT_EPSILON and sharp >
if (ret == 0) {
DEBUG_MSG_LN("Waiting");
ret = delay_until(POURING_TIMEOUT,
orig_weight + requested_amount - UPGRIGHT_OFFSET, true);
}
if (ret == 0)
break; // All good
DEBUG_MSG_LN(String("pour: got err ") + String(ret));
// Bottle empty
// Note that this does not work if requested_amount is less than
// UPGRIGHT_OFFSET!
if(ret == BOTTLE_EMPTY) {
ERROR(strerror(BOTTLE_EMPTY) + String(" ") + String(number) );
// TODO other speed here? it is empty already!
RETURN_IFN_0(turn_to(pos_up + BOTTLE_EMPTY_POS_OFFSET, TURN_UP_DELAY));
RETURN_IFN_0(wait_for_resume()); // might return ABORTED
}
// Cup was removed early
else if(ret == WHERE_THE_FUCK_IS_THE_CUP) {
ERROR(strerror(WHERE_THE_FUCK_IS_THE_CUP));
RETURN_IFN_0(turn_to_pause_pos(FAST_TURN_UP_DELAY));
RETURN_IFN_0(wait_for_cup());
}
// other error - turn bottle up and return error code
// includes: scale error, user abort, ...
else {
return ret;
}
}
// We turn to pause pos and not completely up so we can crossfade
RETURN_IFN_0(turn_to_pause_pos(TURN_UP_DELAY));
RETURN_IFN_0(ads1231_get_grams(measured_amount));
measured_amount -= orig_weight;
DEBUG_START();
DEBUG_MSG("Stats: ");
DEBUG_VAL(requested_amount);
DEBUG_VAL(measured_amount);
DEBUG_END();
return 0;
}
void show_plugin_graph() {
DEBUG_BEGIN();
plugin_graph.debug(print_plugin_info);
DEBUG_END();
}
