void NonLinImpRep::dsdv() {
int ieq, i, in, is, iis;
NrnThread* nt = nrn_threads;
ieq = neq_ - n_ode_;
for (NrnThreadMembList* tml = nt->tml; tml; tml = tml->next) {
Memb_list* ml = tml->ml;
i = tml->index;
if (memb_func[i].ode_count && ml->nodecount) {
int nc = ml->nodecount;
Pfridot s = (Pfridot)memb_func[i].ode_count;
int cnt = (*s)(i);
if (memb_func[i].current) {
double* x1 = rv_; // use as temporary storage
double* x2 = jv_;
// zero rhs, save v
for (in = 0; in < ml->nodecount; ++in) { Node* nd = ml->nodelist[in];
for (is = ieq + in*cnt, iis = 0; iis < cnt; ++iis, ++is) {
*pvdot_[is] = 0.;
}
x1[in] = NODEV(nd);
}
// increment v only once in case there are multiple
// point processes at the same location
for (in = 0; in < ml->nodecount; ++in) { Node* nd = ml->nodelist[in];
if (x1[in] == NODEV(nd)) {
NODEV(nd) += delta_;
}
}
// compute rhs. this is the rhs(v+dv)
ode(i, ml);
// save rhs, restore v, and zero rhs
for (in = 0; in < ml->nodecount; ++in) { Node* nd = ml->nodelist[in];
for (is = ieq + in*cnt, iis = 0; iis < cnt; ++iis, ++is) {
x2[is] = *pvdot_[is];
*pvdot_[is] = 0;
}
NODEV(nd) = x1[in];
}
// compute the rhs(v)
ode(i, ml);
// fill the ds/dv elements
for (in = 0; in < ml->nodecount; ++in) { Node* nd = ml->nodelist[in];
for (is = ieq + in*cnt, iis = 0; iis < cnt; ++iis, ++is) {
double ds = (x2[is] - *pvdot_[is])/delta_;
if (ds != 0.) {
double* elm = cmplx_spGetElement(m_, is+1, v_index_[nd->v_node_index]);
elm[0] = -ds;
}
}
}
}
ieq += cnt*nc;
}
}
}
void KinematicCarModel::EulerIntegration(const ob::State *start, const oc::Control *control, const double duration, ob::State *result) const
{
double t = timeStep_;
std::valarray<double> dstate;
space_->copyState(result, start);
while (t < duration + std::numeric_limits<double>::epsilon())
{
ode(result, control, dstate);
update(result, timeStep_ * dstate);
t += timeStep_;
}
if (t + std::numeric_limits<double>::epsilon() > duration)
{
ode(result, control, dstate);
update(result, (t - duration) * dstate);
}
}
void NonLinImpRep::dsds() {
int ieq, i, in, is, iis, ks, kks;
NrnThread* nt = nrn_threads;
// jw term
for (i = neq_v_; i < neq_; ++i) {
diag_[i][1] += omega_;
}
ieq = neq_v_;
for (NrnThreadMembList* tml = nt->tml; tml; tml = tml->next) {
Memb_list* ml = tml->ml;
i = tml->index;
if (memb_func[i].ode_count && ml->nodecount) {
int nc = ml->nodecount;
Pfridot s = (Pfridot)memb_func[i].ode_count;
int cnt = (*s)(i);
double* x1 = rv_; // use as temporary storage
double* x2 = jv_;
// zero rhs, save s
for (in = 0; in < ml->nodecount; ++in) {
for (is = ieq + in*cnt, iis = 0; iis < cnt; ++iis, ++is) {
*pvdot_[is] = 0.;
x1[is] = *pv_[is];
}
}
// compute rhs. this is the rhs(s)
ode(i, ml);
// save rhs
for (in = 0; in < ml->nodecount; ++in) {
for (is = ieq + in*cnt, iis = 0; iis < cnt; ++iis, ++is) {
x2[is] = *pvdot_[is];
}
}
// iterate over the states
for (kks=0; kks < cnt; ++kks) {
// zero rhs, increment s(ks)
for (in = 0; in < ml->nodecount; ++in) {
ks = ieq + in*cnt + kks;
for (is = ieq + in*cnt, iis = 0; iis < cnt; ++iis, ++is) {
*pvdot_[is] = 0.;
}
*pv_[ks] += deltavec_[ks];
}
ode(i, ml);
// store element and restore s
// fill the ds/dv elements
for (in = 0; in < ml->nodecount; ++in) { Node* nd = ml->nodelist[in];
ks = ieq + in*cnt + kks;
for (is = ieq + in*cnt, iis = 0; iis < cnt; ++iis, ++is) {
double ds = ( *pvdot_[is] - x2[is])/deltavec_[is];
if (ds != 0.) {
double* elm = cmplx_spGetElement(m_, is+1, ks+1);
elm[0] = -ds;
}
*pv_[ks] = x1[ks];
}
}
// perhaps not necessary but ensures the last computation is with
// the base states.
ode(i, ml);
}
ieq += cnt*nc;
}
}
}
bool XmlSimulationSettings::GetSettings(WorldSimulation& sim)
{
string globals="globals";
TiXmlElement* c = e->FirstChildElement(globals);
if(c) {
printf("Parsing timestep...\n");
//parse timestep
double timestep;
if(c->QueryValueAttribute("timestep",×tep)==TIXML_SUCCESS)
sim.simStep = timestep;
}
printf("Parsing ODE...\n");
XmlODESettings ode(e);
if(!ode.GetSettings(sim.odesim)) {
return false;
}
printf("Parsing robot controllers / sensors\n");
c = e->FirstChildElement("robot");
while(c != NULL) {
int index;
if(c->QueryValueAttribute("index",&index)!=TIXML_SUCCESS) {
fprintf(stderr,"Unable to read index of robot element\n");
continue;
}
Assert(index < (int)sim.robotControllers.size());
ControlledRobotSimulator& robotSim=sim.controlSimulators[index];
TiXmlElement* ec=c->FirstChildElement("controller");
if(ec) {
RobotControllerFactory::RegisterDefault(*robotSim.robot);
SmartPointer<RobotController> controller=RobotControllerFactory::Load(ec,*robotSim.robot);
if(controller)
sim.SetController(index,controller);
else {
fprintf(stderr,"Unable to load controller from xml file\n");
return false;
}
Real temp;
if(ec->QueryValueAttribute("rate",&temp)==TIXML_SUCCESS){
if(!(temp > 0)) {
fprintf(stderr,"Invalid rate %g\n",temp);
continue;
}
sim.controlSimulators[index].controlTimeStep = 1.0/temp;
}
if(ec->QueryValueAttribute("timeStep",&temp)==TIXML_SUCCESS){
if(!(temp > 0)) {
fprintf(stderr,"Invalid timestep %g\n",temp);
continue;
}
sim.controlSimulators[index].controlTimeStep = temp;
}
}
TiXmlElement*es=c->FirstChildElement("sensors");
if(es) {
if(!sim.controlSimulators[index].sensors.LoadSettings(es)) {
fprintf(stderr,"Unable to load sensors from xml file\n");
return false;
}
}
/*
TiXmlElement* es=c->FirstChildElement("sensors");
if(es) {
printf("Parsing sensors...\n");
TiXmlElement* pos=es->FirstChildElement("position");
int ival;
Real fval;
if(pos) {
if(pos->QueryValueAttribute("enabled",&ival)==TIXML_SUCCESS) {
if(bodyIndex >= 0)
fprintf(stderr,"Warning: cannot enable individual joint encoders yet\n");
robotSim.sensors.hasJointPosition=(ival!=0);
}
if(pos->QueryValueAttribute("variance",&fval)==TIXML_SUCCESS) {
if(robotSim.sensors.qvariance.n==0)
robotSim.sensors.qvariance.resize(robotSim.robot->q.n,Zero);
if(bodyIndex >= 0)
robotSim.sensors.qvariance(bodyIndex)=fval;
else
robotSim.sensors.qvariance.set(fval);
}
if(pos->QueryValueAttribute("resolution",&fval)==TIXML_SUCCESS) {
if(robotSim.sensors.qresolution.n==0)
robotSim.sensors.qresolution.resize(robotSim.robot->q.n,Zero);
if(bodyIndex >= 0)
robotSim.sensors.qresolution(bodyIndex)=fval;
else
robotSim.sensors.qresolution.set(fval);
}
}
TiXmlElement* vel=c->FirstChildElement("velocity");
if(vel) {
if(vel->QueryValueAttribute("enabled",&ival)==TIXML_SUCCESS) {
if(bodyIndex >= 0)
fprintf(stderr,"Warning: cannot enable individual joint encoders yet\n");
robotSim.sensors.hasJointVelocity=(ival!=0);
}
if(vel->QueryValueAttribute("variance",&fval)==TIXML_SUCCESS) {
if(robotSim.sensors.dqvariance.n==0)
robotSim.sensors.dqvariance.resize(robotSim.robot->q.n,Zero);
if(bodyIndex >= 0)
robotSim.sensors.dqvariance(bodyIndex)=fval;
else
robotSim.sensors.dqvariance.set(fval);
}
if(vel->QueryValueAttribute("resolution",&fval)==TIXML_SUCCESS) {
if(robotSim.sensors.dqresolution.n==0)
robotSim.sensors.dqresolution.resize(robotSim.robot->q.n,Zero);
if(bodyIndex >= 0)
robotSim.sensors.dqresolution(bodyIndex)=fval;
else
robotSim.sensors.dqresolution.set(fval);
}
}
//TODO: other sensors?
}
*/
/*
TiXmlElement* ec=c->FirstChildElement("controller");
if(ec) {
printf("Parsing controller...\n");
TiXmlAttribute* setting = ec->FirstAttribute();
while(setting) {
bool res=robotSim.controller->SetSetting(setting->Name(),setting->Value());
if(!res) {
printf("Setting %s not valid for current controller, or failed parsing\n",setting->Name());
}
setting = setting->Next();
}
}
*/
c = c->NextSiblingElement("robot");
}
printf("Parsing state\n");
c = e->FirstChildElement("state");
if(c) {
const char* data=c->Attribute("data");
if(!data) {
fprintf(stderr,"No 'data' attribute in state\n");
return false;
}
string decoded=FromBase64(data);
if(!sim.ReadState(decoded)) {
fprintf(stderr,"Unable to read state from data\n");
return false;
}
}
return true;
}
int main(int argc, char *argv[])
{
/*no_mc == #MC ODEs; tstep = time step */
int i, j, no_mc = 27;
double tstep=1;
int par_index, sim_index;
gsl_matrix *pars_mat;
int prior = 0;
if(prior==1) {
pars_mat = readMatrix("../data/prior.csv");
} else {
pars_mat = readMatrix("../data/post.csv");
}
gsl_matrix *sim_mat = readMatrix("../data/sim.csv");
double *par_wts, *sim_wts;
par_wts = calloc(sizeof(double), pars_mat->size1);
sim_wts = calloc(sizeof(double), sim_mat->size1);
double *sps, *pars, *sps_gil;
sps = malloc(no_mc*sizeof(double));
sps_gil = malloc(6*sizeof(double));
/*Add in (fixed) k1 and k2 to pars*/
pars = malloc((pars_mat->size2+1)*sizeof(double));
/*total g & i*/
pars[pars_mat->size2-1] = 10; pars[pars_mat->size2] = 2;
gsl_ran_discrete_t *sim_sample, *par_sample;
gsl_rng *r = gsl_rng_alloc(gsl_rng_mt19937);
gsl_rng_set (r, 1);
par_sample = gsl_ran_discrete_preproc(pars_mat->size1, (const double *) par_wts);
sim_sample = gsl_ran_discrete_preproc(sim_mat->size1, (const double *) sim_wts);
/* 1. Select parameters from post (select row)
2. Select time point of interest (select column)
3. Simulate from MC and Gillespie one time unit
4. Compare
5. Profit
*/
for(i=0; i<10000; i++) {
if(prior == 1) {
par_index = i;
} else {
par_index = gsl_ran_discrete(r, par_sample);
}
sim_index = gsl_ran_discrete(r, sim_sample);
/*Reset MC */
for(j=0; j<no_mc;j++) {
sps[j] = 0;
}
/* Sps order: rg, ri, g, i, G, I
column zero iteration number
*/
sps[0] = MGET(sim_mat, sim_index, 1);
sps[2] = MGET(sim_mat, sim_index, 2);
sps[5] = MGET(sim_mat, sim_index, 3);
sps[9] = MGET(sim_mat, sim_index, 4);
sps[14] = MGET(sim_mat, sim_index, 5);
sps[20] = MGET(sim_mat, sim_index, 6);
for(j=0; j<(pars_mat->size2-1); j++) {
pars[j] = MGET(pars_mat, par_index, j+1);
}
sps_gil[0] = sps[0]; sps_gil[1] = sps[2]; sps_gil[2] = sps[5];
sps_gil[3] = sps[9]; sps_gil[4] = sps[14]; sps_gil[5] = sps[20];
/* simulate from MC */
ode(pars, tstep, sps);
gillespie(sps_gil, pars, tstep, r);
printf("%f, %f, %f, %f, %f, %f, %d, %d\n",
(sps[0] - sps_gil[0])/(pow(sps[1], 0.5)),
(sps[2] - sps_gil[1])/(pow(sps[4], 0.5)),
(sps[5] - sps_gil[2])/(pow(sps[8], 0.5)),
(sps[9] - sps_gil[3])/(pow(sps[13], 0.5)),
(sps[14] - sps_gil[4])/(pow(sps[19], 0.5)),
(sps[20] - sps_gil[5])/(pow(sps[26], 0.5)),
par_index, sim_index
);
}
return(EXIT_SUCCESS);
}
