void ControlLinearAdditionalTermsED::addSmoothTerms(DynamicalSystemsGraph& DSG0, const DynamicalSystemsGraph::VDescriptor& dsgVD, const double t, SiconosVector& xdot)
{
    // check whether we have a system with a control input
    if (DSG0.u.hasKey(dsgVD))
    {
        if (DSG0.B.hasKey(dsgVD))
        {
            prod(DSG0.B.getRef(dsgVD), DSG0.u.getRef(dsgVD), xdot, false); // xdot += B*u
        }
        else if (DSG0.pluginU.hasKey(dsgVD))
        {
            DynamicalSystem& ds = *DSG0.bundle(dsgVD);
            SiconosVector& u = DSG0.u.getRef(dsgVD);
            SiconosVector& tmpXdot = DSG0.tmpXdot.getRef(dsgVD);
            ((AdditionalTermsEDfctU)DSG0.pluginU.getRef(dsgVD).fPtr)(t, xdot.size(), ds.getx().getArray(), u.size(), u.getArray(), tmpXdot.getArray(), ds.getz().size(), ds.getz().getArray());
            xdot += tmpXdot; // xdot += g(x, u)
        }
        else
        {
            RuntimeException::selfThrow("ControlLinearAdditionalTermsED :: input u but no B nor pluginU");
        }
    }
    // check whether the DynamicalSystem is an Observer
    if (DSG0.e.hasKey(dsgVD))
    {
        assert(DSG0.L.hasKey(dsgVD));
        prod(*DSG0.L[dsgVD], *DSG0.e[dsgVD], xdot, false); // xdot += -L*e
    }
}
Ejemplo n.º 2
0
/** store all the states of the graph in a matrix
 * \param indx row index in the matrix
 * \param startColumn the starting column
 * \param DSG0 the graph of DynamicalSystem
 * \param IG0 the graph of Interaction
 * \param data the matrix where to save the data
 * \return the last written column
 */
static inline unsigned storeAllStates(unsigned indx, unsigned startColumn, DynamicalSystemsGraph& DSG0, InteractionsGraph& IG0, SimpleMatrix& data)
{
  DynamicalSystemsGraph::VIterator dsvi, dsvdend;
  unsigned column = startColumn;
  for (std11::tie(dsvi, dsvdend) = DSG0.vertices(); dsvi != dsvdend; ++dsvi)
  {
    unsigned i = column;
    SiconosVector& x = *DSG0.bundle(*dsvi)->x();
    for (unsigned j = 0; j < x.size(); ++i, ++j)
    {
      data(indx, i) = x(j);
    }
    column += x.size();

    if (DSG0.u.hasKey(*dsvi))
    {
      SiconosVector& u = *DSG0.u[*dsvi];
      for (unsigned j = 0; j < u.size(); ++i, ++j)
      {
        data(indx, i) = u(j);
      }
      column += u.size();
    }

    if (DSG0.e.hasKey(*dsvi))
    {
      SiconosVector& e = *DSG0.e[*dsvi];
      for (unsigned j = 0; j < e.size(); ++i, ++j)
      {
        data(indx, i) = e(j);
      }
      column += e.size();
    }

  }

  InteractionsGraph::VIterator ivi, ivdend;
  for (std11::tie(ivi, ivdend) = IG0.vertices(); ivi != ivdend; ++ivi)
  {
    unsigned i = column;
    SiconosVector& y = *IG0.bundle(*ivi)->y(0);
    for (unsigned j = 0; j < y.size(); ++i, ++j)
    {
      data(indx, i) = y(j);
    }
    column += y.size();

    SiconosVector& lambda = *IG0.bundle(*ivi)->lambda(0);
    for (unsigned j = 0; j < lambda.size(); ++i, ++j)
    {
      data(indx, i) = lambda(j);
    }
    column += lambda.size();
  }

  return column;
}
void ControlLinearAdditionalTermsED::addJacobianRhsContribution(DynamicalSystemsGraph& DSG0, const DynamicalSystemsGraph::VDescriptor& dsgVD, const double t, SiconosMatrix& jacRhs)
{
    // check whether we have a system with a control input
    if (DSG0.pluginJacgx.hasKey(dsgVD))
    {
        DynamicalSystem& ds = *DSG0.bundle(dsgVD);
        SiconosVector& u = DSG0.u.getRef(dsgVD);
        SimpleMatrix& tmpJacgx = DSG0.jacgx.getRef(dsgVD);
        ((AdditionalTermsEDfctU)DSG0.pluginJacgx.getRef(dsgVD).fPtr)(t, ds.getx().size(), ds.getx().getArray(), u.size(), u.getArray(), tmpJacgx.getArray(), ds.getz().size(), ds.getz().getArray());
        jacRhs += tmpJacgx; // JacRhs += \nabla_x g(x, u)
    }
    else
    {
        RuntimeException::selfThrow("ControlLinearAdditionalTermsED :: input u but no B nor pluginU");
    }
}
void ControlLinearAdditionalTermsED::init(DynamicalSystemsGraph& DSG0, const Model& model)
{

    DynamicalSystemsGraph::VIterator dsvi, dsvdend;
    for (std11::tie(dsvi, dsvdend) = DSG0.vertices(); dsvi != dsvdend; ++dsvi)
    {
        DynamicalSystem& ds = *DSG0.bundle(*dsvi);
        if (DSG0.pluginU.hasKey(*dsvi))
        {
            DSG0.tmpXdot[*dsvi].reset(new SiconosVector(ds.getx().size()));
        }
        if (DSG0.pluginJacgx.hasKey(*dsvi))
        {
            DSG0.jacgx[*dsvi].reset(new SimpleMatrix(ds.getx().size(), ds.getx().size()));
        }

    }
}
Ejemplo n.º 5
0
static inline std::pair<unsigned, std::string> getNumberOfStates(DynamicalSystemsGraph& DSG0, InteractionsGraph& IG0)
{
  std::string legend;
  DynamicalSystemsGraph::VIterator dsvi, dsvdend;
  unsigned nb = 0;
  unsigned counter = 0;
  for (std11::tie(dsvi, dsvdend) = DSG0.vertices(); dsvi != dsvdend; ++dsvi)
  {
    SiconosVector& x = *DSG0.bundle(*dsvi)->x();
    nb += x.size();
    std::string nameDS;
    if (DSG0.name.hasKey(*dsvi))
    {
      nameDS = DSG0.name[*dsvi];
    }
    else
    {
      nameDS = "unknownDS" + TO_STR(counter);
      ++counter;
    }

    for (unsigned i = 0; i < x.size(); ++i)
    {
      legend.append(" " + nameDS + "_" + TO_STR(i));
    }



    if (DSG0.u.hasKey(*dsvi))
    {
      unsigned sizeU = DSG0.u[*dsvi]->size();
      nb += sizeU;
      for (unsigned i = 0; i < sizeU; ++i)
      {
        legend.append(" " + nameDS + "_u_" + TO_STR(i));
      }
    }

    if (DSG0.e.hasKey(*dsvi))
    {
      unsigned sizeE = DSG0.e[*dsvi]->size();
      for (unsigned i = 0; i < sizeE; ++i)
      {
        legend.append(" " + nameDS + "_e_" + TO_STR(i));
      }
      nb += DSG0.e[*dsvi]->size();
    }
  }

  InteractionsGraph::VIterator ivi, ivdend;
  counter = 0;
  for (std11::tie(ivi, ivdend) = IG0.vertices(); ivi != ivdend; ++ivi)
  {
    std::string nameInter;
    if (IG0.name.hasKey(*ivi))
    {
      nameInter = IG0.name[*ivi];
    }
    else
    {
      nameInter = "unknownInteraction" + TO_STR(counter);
      ++counter;
    }
    SiconosVector& y = *IG0.bundle(*ivi)->y(0);
    nb += y.size();
    for (unsigned i = 0; i < y.size(); ++i)
    {
      legend.append(" " + nameInter + "_y_" + TO_STR(i));
    }

    SiconosVector& lambda = *IG0.bundle(*ivi)->lambda(0);
    nb += lambda.size();
    for (unsigned i = 0; i < lambda.size(); ++i)
    {
      legend.append(" " + nameInter + "_lambda_" + TO_STR(i));
    }
  }

  return std::make_pair(nb, legend);
}