void EventDriven::initOSIs() { for (OSIIterator itosi = _allOSI->begin(); itosi != _allOSI->end(); ++itosi) { // Initialize the acceleration like for NewMarkAlphaScheme if ((*itosi)->getType() == OSI::NEWMARKALPHAOSI) { SP::NewMarkAlphaOSI osi_NewMark = std11::static_pointer_cast<NewMarkAlphaOSI>(*itosi); DynamicalSystemsGraph::VIterator dsi, dsend; SP::DynamicalSystemsGraph osiDSGraph = (*itosi)->dynamicalSystemsGraph(); for (std11::tie(dsi, dsend) = osiDSGraph->vertices(); dsi != dsend; ++dsi) { if (!(*itosi)->checkOSI(dsi)) continue; SP::DynamicalSystem ds = osiDSGraph->bundle(*dsi); if ((Type::value(*ds) == Type::LagrangianDS) || (Type::value(*ds) == Type::LagrangianLinearTIDS)) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS>(ds); *(d->workspace(DynamicalSystem::acce_like)) = *(d->acceleration()); // set a0 = ddotq0 // Allocate the memory to stock coefficients of the polynomial for the dense output d->allocateWorkMatrix(LagrangianDS::coeffs_denseoutput, ds->dimension(), (osi_NewMark->getOrderDenseOutput() + 1)); } } } } }
void LagrangianDSTest::testcomputeDS() { std::cout << "-->Test: computeDS." <<std::endl; DynamicalSystem * ds(new LagrangianDS(tmpxml2)); SP::LagrangianDS copy = std11::static_pointer_cast<LagrangianDS>(ds); double time = 1.5; ds->initialize("EventDriven", time); ds->computeRhs(time); std::cout << "-->Test: computeDS." <<std::endl; ds->computeJacobianRhsx(time); std::cout << "-->Test: computeDS." <<std::endl; SimpleMatrix M(3, 3); M(0, 0) = 1; M(1, 1) = 2; M(2, 2) = 3; SP::SiconosMatrix jx = ds->jacobianRhsx(); SP::SiconosVector vf = ds->rhs(); CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSI : ", *(vf->vector(0)) == *velocity0, true); CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSJ : ", prod(M, *(vf->vector(1))) == (copy->getFExt() - copy->getFInt() - copy->getFGyr()) , true); CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSL : ", prod(M, *(jx->block(1, 0))) == (copy->getJacobianFL(0)) , true); CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSL : ", prod(M, *(jx->block(1, 1))) == (copy->getJacobianFL(1)) , true); std::cout << "--> computeDS test ended with success." <<std::endl; }
void MoreauJeanDirectProjectionOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds) { DEBUG_BEGIN("MoreauJeanDirectProjectionOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds) \n"); MoreauJeanOSI::initializeWorkVectorsForDS(t, ds); const DynamicalSystemsGraph::VDescriptor& dsv = _dynamicalSystemsGraph->descriptor(ds); VectorOfVectors& workVectors = *_dynamicalSystemsGraph->properties(dsv).workVectors; Type::Siconos dsType = Type::value(*ds); if(dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); workVectors[MoreauJeanOSI::QTMP].reset(new SiconosVector(d->dimension())); } else if(dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS>(ds); workVectors[MoreauJeanOSI::QTMP].reset(new SiconosVector(d->getqDim())); } else { RuntimeException::selfThrow("MoreauJeanDirectProjectionOSI::initialize() - DS not of the right type"); } for (unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++) { DEBUG_PRINTF("ds->initializeNonSmoothInput(%i)\n", k); ds->initializeNonSmoothInput(k); DEBUG_EXPR_WE( SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); if (d->p(k)) std::cout << "d->p(" << k <<" ) exists" << std::endl; ); }
void D1MinusLinearOSI::initializeWorkVectorsForDS(double t, SP::DynamicalSystem ds) { // Get work buffers from the graph VectorOfVectors& ds_work_vectors = *_initializeDSWorkVectors(ds); // Check dynamical system type Type::Siconos dsType = Type::value(*ds); assert(dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS || dsType == Type::NewtonEulerDS); if(dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS) { SP::LagrangianDS lds = std11::static_pointer_cast<LagrangianDS> (ds); lds->init_generalized_coordinates(2); // acceleration is required for the ds lds->init_inverse_mass(); // invMass required to update post-impact velocity ds_work_vectors.resize(D1MinusLinearOSI::WORK_LENGTH); ds_work_vectors[D1MinusLinearOSI::RESIDU_FREE].reset(new SiconosVector(lds->dimension())); ds_work_vectors[D1MinusLinearOSI::FREE].reset(new SiconosVector(lds->dimension())); ds_work_vectors[D1MinusLinearOSI::FREE_TDG].reset(new SiconosVector(lds->dimension())); // Update dynamical system components (for memory swap). lds->computeForces(t, lds->q(), lds->velocity()); lds->swapInMemory(); } else if(dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS neds = std11::static_pointer_cast<NewtonEulerDS> (ds); neds->init_inverse_mass(); // invMass required to update post-impact velocity ds_work_vectors.resize(D1MinusLinearOSI::WORK_LENGTH); ds_work_vectors[D1MinusLinearOSI::RESIDU_FREE].reset(new SiconosVector(neds->dimension())); ds_work_vectors[D1MinusLinearOSI::FREE].reset(new SiconosVector(neds->dimension())); ds_work_vectors[D1MinusLinearOSI::FREE_TDG].reset(new SiconosVector(neds->dimension())); //Compute a first value of the forces to store it in _forcesMemory neds->computeForces(t, neds->q(), neds->twist()); neds->swapInMemory(); } else RuntimeException::selfThrow("D1MinusLinearOSI::initialize - not implemented for Dynamical system type: " + dsType); for (unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++) { ds->initializeNonSmoothInput(k); } }
void LsodarOSI::updateState(const unsigned int level) { // Compute all required (ie time-dependent) data for the DS of the OSI. DSIterator it; if (level == 1) // ie impact case: compute velocity { for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it) { SP::LagrangianDS lds = std11::static_pointer_cast<LagrangianDS>(*it); lds->computePostImpactVelocity(); } } else if (level == 2) { double time = simulationLink->model()->currentTime(); for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it) (*it)->update(time); } else RuntimeException::selfThrow("LsodarOSI::updateState(index), index is out of range. Index = " + level); }
// ================= Creation of the model ======================= void Disks::init() { SP::TimeDiscretisation timedisc_; SP::TimeStepping simulation_; SP::FrictionContact osnspb_; // User-defined main parameters double t0 = 0; // initial computation time double T = std::numeric_limits<double>::infinity(); double h = 0.01; // time step double g = 9.81; double theta = 0.5; // theta for MoreauJeanOSI integrator std::string solverName = "NSGS"; // ----------------------------------------- // --- Dynamical systems && interactions --- // ----------------------------------------- double R; double m; try { // ------------ // --- Init --- // ------------ std::cout << "====> Model loading ..." << std::endl << std::endl; _plans.reset(new SimpleMatrix("plans.dat", true)); if (_plans->size(0) == 0) { /* default plans */ double A1 = P1A; double B1 = P1B; double C1 = P1C; double A2 = P2A; double B2 = P2B; double C2 = P2C; _plans.reset(new SimpleMatrix(6, 6)); _plans->zero(); (*_plans)(0, 0) = 0; (*_plans)(0, 1) = 1; (*_plans)(0, 2) = -GROUND; (*_plans)(1, 0) = 1; (*_plans)(1, 1) = 0; (*_plans)(1, 2) = WALL; (*_plans)(2, 0) = 1; (*_plans)(2, 1) = 0; (*_plans)(2, 2) = -WALL; (*_plans)(3, 0) = 0; (*_plans)(3, 1) = 1; (*_plans)(3, 2) = -TOP; (*_plans)(4, 0) = A1; (*_plans)(4, 1) = B1; (*_plans)(4, 2) = C1; (*_plans)(5, 0) = A2; (*_plans)(5, 1) = B2; (*_plans)(5, 2) = C2; } /* set center positions */ for (unsigned int i = 0 ; i < _plans->size(0); ++i) { SP::DiskPlanR tmpr; tmpr.reset(new DiskPlanR(1, (*_plans)(i, 0), (*_plans)(i, 1), (*_plans)(i, 2), (*_plans)(i, 3), (*_plans)(i, 4), (*_plans)(i, 5))); (*_plans)(i, 3) = tmpr->getXCenter(); (*_plans)(i, 4) = tmpr->getYCenter(); } /* _moving_plans.reset(new FMatrix(1,6)); (*_moving_plans)(0,0) = &A; (*_moving_plans)(0,1) = &B; (*_moving_plans)(0,2) = &C; (*_moving_plans)(0,3) = &DA; (*_moving_plans)(0,4) = &DB; (*_moving_plans)(0,5) = &DC;*/ SP::SiconosMatrix Disks; Disks.reset(new SimpleMatrix("disks.dat", true)); // -- OneStepIntegrators -- SP::OneStepIntegrator osi; osi.reset(new MoreauJeanOSI(theta)); // -- Model -- _model.reset(new Model(t0, T)); for (unsigned int i = 0; i < Disks->size(0); i++) { R = Disks->getValue(i, 2); m = Disks->getValue(i, 3); SP::SiconosVector qTmp; SP::SiconosVector vTmp; qTmp.reset(new SiconosVector(NDOF)); vTmp.reset(new SiconosVector(NDOF)); vTmp->zero(); (*qTmp)(0) = (*Disks)(i, 0); (*qTmp)(1) = (*Disks)(i, 1); SP::LagrangianDS body; if (R > 0) body.reset(new Disk(R, m, qTmp, vTmp)); else body.reset(new Circle(-R, m, qTmp, vTmp)); // -- Set external forces (weight) -- SP::SiconosVector FExt; FExt.reset(new SiconosVector(NDOF)); FExt->zero(); FExt->setValue(1, -m * g); body->setFExtPtr(FExt); // add the dynamical system to the one step integrator osi->insertDynamicalSystem(body); // add the dynamical system in the non smooth dynamical system _model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body); } _model->nonSmoothDynamicalSystem()->setSymmetric(true); // ------------------ // --- Simulation --- // ------------------ // -- Time discretisation -- timedisc_.reset(new TimeDiscretisation(t0, h)); // -- OneStepNsProblem -- osnspb_.reset(new FrictionContact(2)); osnspb_->numericsSolverOptions()->iparam[0] = 100; // Max number of // iterations osnspb_->numericsSolverOptions()->iparam[1] = 20; // compute error // iterations osnspb_->numericsSolverOptions()->dparam[0] = 1e-3; // Tolerance osnspb_->setMaxSize(6 * ((3 * Ll * Ll + 3 * Ll) / 2 - Ll)); osnspb_->setMStorageType(1); // Sparse storage osnspb_->setNumericsVerboseMode(0); osnspb_->setKeepLambdaAndYState(true); // inject previous solution // -- Simulation -- simulation_.reset(new TimeStepping(timedisc_)); std11::static_pointer_cast<TimeStepping>(simulation_)->setNewtonMaxIteration(3); simulation_->insertIntegrator(osi); simulation_->insertNonSmoothProblem(osnspb_); simulation_->setCheckSolverFunction(localCheckSolverOuput); // --- Simulation initialization --- std::cout << "====> Simulation initialisation ..." << std::endl << std::endl; SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0, 0, 0.3, 2)); _playground.reset(new SpaceFilter(3, 6, _model, _plans, _moving_plans)); _playground->insert(nslaw, 0, 0); _model->initialize(simulation_); } catch (SiconosException e) { std::cout << e.report() << std::endl; exit(1); } catch (...) { std::cout << "Exception caught in Disks::init()" << std::endl; exit(1); } }
void LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd) { DEBUG_BEGIN("LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)\n"); // Computes matrix _interactionBlocks[inter1][inter1] (and allocates memory if // necessary) one or two DS are concerned by inter1 . How // _interactionBlocks are computed depends explicitely on the type of // Relation of each Interaction. // Warning: we suppose that at this point, all non linear // operators (G for lagrangian relation for example) have been // computed through plug-in mechanism. // Get dimension of the NonSmoothLaw (ie dim of the interactionBlock) SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel()); SP::Interaction inter = indexSet->bundle(vd); // Get osi property from interaction // We assume that all ds in vertex_inter have the same osi. SP::OneStepIntegrator Osi = indexSet->properties(vd).osi; //SP::OneStepIntegrator Osi = simulation()->integratorOfDS(ds); OSI::TYPES osiType = Osi->getType(); // At most 2 DS are linked by an Interaction SP::DynamicalSystem DS1; SP::DynamicalSystem DS2; unsigned int pos1, pos2; // --- Get the dynamical system(s) (edge(s)) connected to the current interaction (vertex) --- if (indexSet->properties(vd).source != indexSet->properties(vd).target) { DEBUG_PRINT("a two DS Interaction\n"); DS1 = indexSet->properties(vd).source; DS2 = indexSet->properties(vd).target; } else { DEBUG_PRINT("a single DS Interaction\n"); DS1 = indexSet->properties(vd).source; DS2 = DS1; // \warning this looks like some debug code, but it gets executed even with NDEBUG. // may be compiler does something smarter, but still it should be rewritten. --xhub InteractionsGraph::OEIterator oei, oeiend; for (std11::tie(oei, oeiend) = indexSet->out_edges(vd); oei != oeiend; ++oei) { // note : at most 4 edges DS2 = indexSet->bundle(*oei); if (DS2 != DS1) { assert(false); break; } } } assert(DS1); assert(DS2); pos1 = indexSet->properties(vd).source_pos; pos2 = indexSet->properties(vd).target_pos; // --- Check block size --- assert(indexSet->properties(vd).block->size(0) == inter->nonSmoothLaw()->size()); assert(indexSet->properties(vd).block->size(1) == inter->nonSmoothLaw()->size()); // --- Compute diagonal block --- // Block to be set in OSNS Matrix, corresponding to // the current interaction SP::SiconosMatrix currentInteractionBlock = indexSet->properties(vd).block; SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock; RELATION::TYPES relationType; double h = simulation()->currentTimeStep(); // General form of the interactionBlock is : interactionBlock = // a*extraInteractionBlock + b * leftInteractionBlock * centralInteractionBlocks // * rightInteractionBlock a and b are scalars, centralInteractionBlocks a // matrix depending on the integrator (and on the DS), the // simulation type ... left, right and extra depend on the relation // type and the non smooth law. relationType = inter->relation()->getType(); VectorOfSMatrices& workMInter = *indexSet->properties(vd).workMatrices; inter->getExtraInteractionBlock(currentInteractionBlock, workMInter); unsigned int nslawSize = inter->nonSmoothLaw()->size(); // loop over the DS connected to the interaction. bool endl = false; unsigned int pos = pos1; for (SP::DynamicalSystem ds = DS1; !endl; ds = DS2) { assert(ds == DS1 || ds == DS2); endl = (ds == DS2); unsigned int sizeDS = ds->dimension(); // get _interactionBlocks corresponding to the current DS // These _interactionBlocks depends on the relation type. leftInteractionBlock.reset(new SimpleMatrix(nslawSize, sizeDS)); inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter); DEBUG_EXPR(leftInteractionBlock->display();); // Computing depends on relation type -> move this in Interaction method? if (relationType == FirstOrder) { rightInteractionBlock.reset(new SimpleMatrix(sizeDS, nslawSize)); inter->getRightInteractionBlockForDS(pos, rightInteractionBlock, workMInter); if (osiType == OSI::EULERMOREAUOSI) { if ((std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGamma() || (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGammaForRelation()) { *rightInteractionBlock *= (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->gamma(); } } // for ZOH, we have a different formula ... if (osiType == OSI::ZOHOSI && indexSet->properties(vd).forControl) { *rightInteractionBlock = std11::static_pointer_cast<ZeroOrderHoldOSI>(Osi)->Bd(ds); prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false); } else { // centralInteractionBlock contains a lu-factorized matrix and we solve // centralInteractionBlock * X = rightInteractionBlock with PLU SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds); centralInteractionBlock->PLUForwardBackwardInPlace(*rightInteractionBlock); inter->computeKhat(*rightInteractionBlock, workMInter, h); // if K is non 0 // integration of r with theta method removed // *currentInteractionBlock += h *Theta[*itDS]* *leftInteractionBlock * (*rightInteractionBlock); //left = C, right = W.B //gemm(h,*leftInteractionBlock,*rightInteractionBlock,1.0,*currentInteractionBlock); *leftInteractionBlock *= h; prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false); //left = C, right = inv(W).B } } else if (relationType == Lagrangian || relationType == NewtonEuler) { SP::BoundaryCondition bc; Type::Siconos dsType = Type::value(*ds); if (dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); if (d->boundaryConditions()) bc = d->boundaryConditions(); } else if (dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); if (d->boundaryConditions()) bc = d->boundaryConditions(); } if (bc) { for (std::vector<unsigned int>::iterator itindex = bc->velocityIndices()->begin() ; itindex != bc->velocityIndices()->end(); ++itindex) { // (nslawSize,sizeDS)); SP::SiconosVector coltmp(new SiconosVector(nslawSize)); coltmp->zero(); leftInteractionBlock->setCol(*itindex, *coltmp); } } DEBUG_PRINT("leftInteractionBlock after application of boundary conditions\n"); DEBUG_EXPR(leftInteractionBlock->display(););
void D1MinusLinearOSI::updateState(const unsigned int level) { DEBUG_PRINTF("\n D1MinusLinearOSI::updateState(const unsigned int level) start for level = %i\n",level); for (DSIterator it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it) { // type of the current DS Type::Siconos dsType = Type::value(**it); /* \warning the following conditional statement should be removed with a MechanicalDS class */ /* Lagrangian DS*/ if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS)) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (*it); SP::SiconosMatrix M = d->mass(); SP::SiconosVector v = d->velocity(); DEBUG_PRINT("Position and velocity before update\n"); DEBUG_EXPR(d->q()->display()); DEBUG_EXPR(d->velocity()->display()); /* Add the contribution of the impulse if any */ if (d->p(1)) { DEBUG_EXPR(d->p(1)->display()); /* copy the value of the impulse */ SP::SiconosVector dummy(new SiconosVector(*(d->p(1)))); /* Compute the velocity jump due to the impulse */ M->PLUForwardBackwardInPlace(*dummy); /* Add the velocity jump to the free velocity */ *v += *dummy; } DEBUG_PRINT("Position and velocity after update\n"); DEBUG_EXPR(d->q()->display()); DEBUG_EXPR(d->velocity()->display()); } /* NewtonEuler Systems */ else if (dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (*it); SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization; SP::SiconosVector v = d->velocity(); // POINTER CONSTRUCTOR : contains new velocity if (d->p(1)) { // Update the velocity SP::SiconosVector dummy(new SiconosVector(*(d->p(1)))); // value = nonsmooth impulse M->PLUForwardBackwardInPlace(*dummy); // solution for its velocity equivalent *v += *dummy; // add free velocity // update \f$ \dot q \f$ SP::SiconosMatrix T = d->T(); SP::SiconosVector dotq = d->dotq(); prod(*T, *v, *dotq, true); DEBUG_PRINT("\nRIGHT IMPULSE\n"); DEBUG_EXPR(d->p(1)->display()); } DEBUG_EXPR(d->q()->display()); DEBUG_EXPR(d->velocity()->display()); } else RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); } DEBUG_PRINT("\n D1MinusLinearOSI::updateState(const unsigned int level) end\n"); }
void D1MinusLinearOSI::computeFreeState() { DEBUG_PRINT("\n D1MinusLinearOSI::computeFreeState(), start\n"); for (DSIterator it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it) { // type of the current DS Type::Siconos dsType = Type::value(**it); /* \warning the following conditional statement should be removed with a MechanicalDS class */ if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS)) { // Lagrangian Systems SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (*it); // get left state from memory SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit DEBUG_EXPR(vold->display()); // get right information //SP::SiconosMatrix M = d->mass(); SP::SiconosVector vfree = d->velocity(); // POINTER CONSTRUCTOR : contains free velocity (*vfree) = *(d->workspace(DynamicalSystem::freeresidu)); DEBUG_EXPR(d->workspace(DynamicalSystem::freeresidu)->display()); // d->computeMass(); // M->resetLU(); // M->PLUForwardBackwardInPlace(*vfree); // DEBUG_EXPR(M->display()); *vfree *= -1.; *vfree += *vold; DEBUG_EXPR(vfree->display()); } else if (dsType == Type::NewtonEulerDS) { // NewtonEuler Systems SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (*it); // get left state from memory SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit DEBUG_EXPR(vold->display()); // get right information SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization; SP::SiconosVector vfree = d->velocity(); // POINTER CONSTRUCTOR : contains free velocity (*vfree) = *(d->workspace(DynamicalSystem::freeresidu)); DEBUG_EXPR(d->workspace(DynamicalSystem::freeresidu)->display()); *vfree *= -1.; *vfree += *vold; DEBUG_EXPR(vfree->display()); } else RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); } DEBUG_PRINT("D1MinusLinearOSI::computeFreeState(), end\n"); }
void D1MinusLinearOSI::initialize() { DEBUG_PRINT("D1MinusLinearOSI::initialize() starts \n"); for (DSIterator it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it) { Type::Siconos dsType = Type::value(**it); if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (*it); d->computeMass(); } else if (dsType == Type::NewtonEulerDS) { //SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (*it); } else RuntimeException::selfThrow("D1MinusLinearOSI::initialize - not implemented for Dynamical system type: " + dsType); } DEBUG_PRINTF("D1MinusLinearOSI::initialize(). Type of OSI %i ", _typeOfD1MinusLinearOSI ); SP::OneStepNSProblems allOSNSP = simulationLink->oneStepNSProblems(); // all OSNSP bool isOSNSPinitialized = false ; switch (_typeOfD1MinusLinearOSI) { case halfexplicit_acceleration_level: // set evaluation levels (first is of velocity, second of acceleration type) (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY]->setIndexSetLevel(1); (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY]->setInputOutputLevel(1); (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY]->initialize(simulationLink); (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY + 1]->setIndexSetLevel(2); (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY + 1]->setInputOutputLevel(2); (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY + 1]->initialize(simulationLink); isOSNSPinitialized = true ; DEBUG_EXPR((*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY + 1]->display()); break; case halfexplicit_acceleration_level_full: // set evaluation levels (first is of velocity, second of acceleration type) (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY]->setIndexSetLevel(1); (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY]->setInputOutputLevel(1); (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY]->initialize(simulationLink); (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY + 1]->setIndexSetLevel(2); (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY + 1]->setInputOutputLevel(2); (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY + 1]->initialize(simulationLink); isOSNSPinitialized = true ; break; case halfexplicit_velocity_level: // set evaluation levels (first is of velocity, second of acceleration type) (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY]->setIndexSetLevel(1); (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY]->setInputOutputLevel(1); (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY]->initialize(simulationLink); (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY + 1]->setIndexSetLevel(1); /** !!! */ (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY + 1]->setInputOutputLevel(2); (*allOSNSP)[SICONOS_OSNSP_TS_VELOCITY + 1]->initialize(simulationLink); isOSNSPinitialized = true ; break; } if (!isOSNSPinitialized) { RuntimeException::selfThrow("D1MinusLinearOSI::initialize() - not implemented for type of D1MinusLinearOSI: " + _typeOfD1MinusLinearOSI ); } DEBUG_PRINT("D1MinusLinearOSI::initialize() ends \n"); }
// ================= Creation of the model ======================= void Spheres::init() { SP::TimeDiscretisation timedisc_; SP::Simulation simulation_; SP::FrictionContact osnspb_; // User-defined main parameters double t0 = 0; // initial computation time double T = std::numeric_limits<double>::infinity(); double h = 0.01; // time step double g = 9.81; double theta = 0.5; // theta for MoreauJeanOSI integrator std::string solverName = "NSGS"; // ----------------------------------------- // --- Dynamical systems && interactions --- // ----------------------------------------- double R; double m; try { // ------------ // --- Init --- // ------------ std::cout << "====> Model loading ..." << std::endl << std::endl; _plans.reset(new SimpleMatrix("plans.dat", true)); SP::SiconosMatrix Spheres; Spheres.reset(new SimpleMatrix("spheres.dat", true)); // -- OneStepIntegrators -- SP::OneStepIntegrator osi; osi.reset(new MoreauJeanOSI(theta)); // -- Model -- _model.reset(new Model(t0, T)); for (unsigned int i = 0; i < Spheres->size(0); i++) { R = Spheres->getValue(i, 3); m = Spheres->getValue(i, 4); SP::SiconosVector qTmp; SP::SiconosVector vTmp; qTmp.reset(new SiconosVector(NDOF)); vTmp.reset(new SiconosVector(NDOF)); vTmp->zero(); (*qTmp)(0) = (*Spheres)(i, 0); (*qTmp)(1) = (*Spheres)(i, 1); (*qTmp)(2) = (*Spheres)(i, 2); (*qTmp)(3) = M_PI / 2; (*qTmp)(4) = M_PI / 4; (*qTmp)(5) = M_PI / 2; (*vTmp)(0) = 0; (*vTmp)(1) = 0; (*vTmp)(2) = 0; (*vTmp)(3) = 0; (*vTmp)(4) = 0; (*vTmp)(5) = 0; SP::LagrangianDS body; body.reset(new SphereLDS(R, m, std11::shared_ptr<SiconosVector>(qTmp), std11::shared_ptr<SiconosVector>(vTmp))); // -- Set external forces (weight) -- SP::SiconosVector FExt; FExt.reset(new SiconosVector(NDOF)); FExt->zero(); FExt->setValue(2, -m * g); body->setFExtPtr(FExt); // add the dynamical system to the one step integrator osi->insertDynamicalSystem(body); // add the dynamical system in the non smooth dynamical system _model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body); } // ------------------ // --- Simulation --- // ------------------ // -- Time discretisation -- timedisc_.reset(new TimeDiscretisation(t0, h)); // -- OneStepNsProblem -- osnspb_.reset(new FrictionContact(3)); osnspb_->numericsSolverOptions()->iparam[0] = 100; // Max number of // iterations osnspb_->numericsSolverOptions()->iparam[1] = 20; // compute error // iterations osnspb_->numericsSolverOptions()->iparam[4] = 2; // projection osnspb_->numericsSolverOptions()->dparam[0] = 1e-6; // Tolerance osnspb_->numericsSolverOptions()->dparam[2] = 1e-8; // Local tolerance osnspb_->setMaxSize(16384); // max number of interactions osnspb_->setMStorageType(1); // Sparse storage osnspb_->setNumericsVerboseMode(0); // 0 silent, 1 verbose osnspb_->setKeepLambdaAndYState(true); // inject previous solution simulation_.reset(new TimeStepping(timedisc_)); simulation_->insertIntegrator(osi); simulation_->insertNonSmoothProblem(osnspb_); // simulation_->setCheckSolverFunction(localCheckSolverOuput); // --- Simulation initialization --- std::cout << "====> Simulation initialisation ..." << std::endl << std::endl; SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0, 0, 0.8, 3)); _playground.reset(new SpaceFilter(3, 6, _model, _plans, _moving_plans)); _playground->insert(nslaw, 0, 0); _model->initialize(simulation_); } catch (SiconosException e) { std::cout << e.report() << std::endl; exit(1); } catch (...) { std::cout << "Exception caught in Spheres::init()" << std::endl; exit(1); } }
double D1MinusLinearOSI::computeResiduHalfExplicitAccelerationLevel() { DEBUG_BEGIN("\n D1MinusLinearOSI::computeResiduHalfExplicitAccelerationLevel()\n"); double t = _simulation->nextTime(); // end of the time step double told = _simulation->startingTime(); // beginning of the time step double h = _simulation->timeStep(); // time step length SP::OneStepNSProblems allOSNS = _simulation->oneStepNSProblems(); // all OSNSP SP::Topology topo = _simulation->nonSmoothDynamicalSystem()->topology(); SP::InteractionsGraph indexSet2 = topo->indexSet(2); /************************************************************************************************************** * Step 1- solve a LCP at acceleration level for lambda^+_{k} for the last set indices * if index2 is empty we should skip this step **************************************************************************************************************/ DEBUG_PRINT("\nEVALUATE LEFT HAND SIDE\n"); DEBUG_EXPR(std::cout<< "allOSNS->empty() " << std::boolalpha << allOSNS->empty() << std::endl << std::endl); DEBUG_EXPR(std::cout<< "allOSNS->size() " << allOSNS->size() << std::endl << std::endl); // -- LEFT SIDE -- DynamicalSystemsGraph::VIterator dsi, dsend; for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi) { if (!checkOSI(dsi)) continue; SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi); Type::Siconos dsType = Type::value(*ds); SP::SiconosVector accFree; SP::SiconosVector work_tdg; SP::SiconosMatrix Mold; DEBUG_EXPR((*it)->display()); if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS)) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); accFree = d->workspace(DynamicalSystem::free); /* POINTER CONSTRUCTOR : will contain * the acceleration without contact force */ accFree->zero(); // get left state from memory SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit Mold = d->mass(); DEBUG_EXPR(accFree->display()); DEBUG_EXPR(qold->display()); DEBUG_EXPR(vold->display()); DEBUG_EXPR(Mold->display()); if (! d->workspace(DynamicalSystem::free_tdg)) { d->allocateWorkVector(DynamicalSystem::free_tdg, d->dimension()) ; } work_tdg = d->workspace(DynamicalSystem::free_tdg); work_tdg->zero(); DEBUG_EXPR(work_tdg->display()); if (d->forces()) { d->computeForces(told, qold, vold); DEBUG_EXPR(d->forces()->display()); *accFree += *(d->forces()); } Mold->PLUForwardBackwardInPlace(*accFree); // contains left (right limit) acceleration without contact force d->addWorkVector(accFree,DynamicalSystem::free_tdg); // store the value in WorkFreeFree } else if(dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force accFree->zero(); // get left state from memory SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit //Mold = d->mass(); assert(!d->mass()->isPLUInversed()); Mold.reset(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization DEBUG_EXPR(accFree->display()); DEBUG_EXPR(qold->display()); DEBUG_EXPR(vold->display()); DEBUG_EXPR(Mold->display()); if (! d->workspace(DynamicalSystem::free_tdg)) { d->allocateWorkVector(DynamicalSystem::free_tdg, d->dimension()) ; } work_tdg = d->workspace(DynamicalSystem::free_tdg); work_tdg->zero(); DEBUG_EXPR(work_tdg->display()); if (d->forces()) { d->computeForces(told, qold, vold); DEBUG_EXPR(d->forces()->display()); *accFree += *(d->forces()); } Mold->PLUForwardBackwardInPlace(*accFree); // contains left (right limit) acceleration without contact force d->addWorkVector(accFree,DynamicalSystem::free_tdg); // store the value in WorkFreeFree } else { RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); } DEBUG_PRINT("accFree contains right limit acceleration at t^+_k with contact force :\n"); DEBUG_EXPR(accFree->display()); DEBUG_PRINT("work_tdg contains right limit acceleration at t^+_k without contact force :\n"); DEBUG_EXPR(work_tdg->display()); } if (!allOSNS->empty()) { if (indexSet2->size() >0) { InteractionsGraph::VIterator ui, uiend; SP::Interaction inter; for (std11::tie(ui, uiend) = indexSet2->vertices(); ui != uiend; ++ui) { inter = indexSet2->bundle(*ui); inter->relation()->computeJach(t, *inter, indexSet2->properties(*ui)); inter->relation()->computeJacg(told, *inter, indexSet2->properties(*ui)); } if (_simulation->nonSmoothDynamicalSystem()->topology()->hasChanged()) { for (OSNSIterator itOsns = allOSNS->begin(); itOsns != allOSNS->end(); ++itOsns) { (*itOsns)->setHasBeenUpdated(false); } } assert((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]); if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->hasInteractions())) // it should be equivalent to indexSet2 { DEBUG_PRINT("We compute lambda^+_{k} \n"); (*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->compute(told); DEBUG_EXPR((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->display()); } // Note Franck : at the time this results in a call to swapInMem of all Interactions of the NSDS // So let the simu do this. //(*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->saveInMemory(); // we push y and lambda in Memories _simulation->nonSmoothDynamicalSystem()->pushInteractionsInMemory(); _simulation->nonSmoothDynamicalSystem()->updateInput(_simulation->nextTime(),2); for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi) { if (!checkOSI(dsi)) continue; SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi); Type::Siconos dsType = Type::value(*ds); if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS)) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force SP::SiconosVector dummy(new SiconosVector(*(d->p(2)))); // value = contact force SP::SiconosMatrix Mold = d->mass(); Mold->PLUForwardBackwardInPlace(*dummy); *accFree += *(dummy); DEBUG_EXPR(d->p(2)->display()); } else if (dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force SP::SiconosVector dummy(new SiconosVector(*(d->p(2)))); // value = contact force SP::SiconosMatrix Mold(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization DEBUG_EXPR(Mold->display()); Mold->PLUForwardBackwardInPlace(*dummy); *accFree += *(dummy); DEBUG_EXPR(d->p(2)->display()); } else RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); } } } /************************************************************************************************************** * Step 2 - compute v_{k,1} **************************************************************************************************************/ DEBUG_PRINT("\n PREDICT RIGHT HAND SIDE\n"); for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi) { if (!checkOSI(dsi)) continue; SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi); // type of the current DS Type::Siconos dsType = Type::value(*ds); /* \warning the following conditional statement should be removed with a MechanicalDS class */ if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS)) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // contains acceleration without contact force // get left state from memory SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // initialize *it->residuFree and predicted right velocity (left limit) SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu); // contains residu without nonsmooth effect SP::SiconosVector v = d->velocity(); //contains velocity v_{k+1}^- and not free velocity residuFree->zero(); v->zero(); DEBUG_EXPR(accFree->display()); DEBUG_EXPR(qold->display()); DEBUG_EXPR(vold->display()); *residuFree -= 0.5 * h**accFree; *v += h**accFree; *v += *vold; DEBUG_EXPR(residuFree->display()); DEBUG_EXPR(v->display()); SP::SiconosVector q = d->q(); // POINTER CONSTRUCTOR : contains position q_{k+1} *q = *qold; scal(0.5 * h, *vold + *v, *q, false); DEBUG_EXPR(q->display()); } else if (dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // get left state from memory SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // initialize *it->residuFree and predicted right velocity (left limit) SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu); // contains residu without nonsmooth effect SP::SiconosVector v = d->velocity(); //contains velocity v_{k+1}^- and not free velocity residuFree->zero(); v->zero(); DEBUG_EXPR(accFree->display()); DEBUG_EXPR(qold->display()); DEBUG_EXPR(vold->display()); *residuFree -= 0.5 * h**accFree; *v += h**accFree; *v += *vold; DEBUG_EXPR(residuFree->display()); DEBUG_EXPR(v->display()); //first step consists in computing \dot q. //second step consists in updating q. // SP::SiconosMatrix T = d->T(); SP::SiconosVector dotq = d->dotq(); prod(*T, *v, *dotq, true); SP::SiconosVector dotqold = d->dotqMemory()->getSiconosVector(0); SP::SiconosVector q = d->q(); // POINTER CONSTRUCTOR : contains position q_{k+1} *q = *qold; scal(0.5 * h, *dotqold + *dotq, *q, false); DEBUG_PRINT("new q before normalizing\n"); DEBUG_EXPR(q->display()); //q[3:6] must be normalized d->normalizeq(); d->computeT(); DEBUG_PRINT("new q after normalizing\n"); DEBUG_EXPR(q->display()); } else RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); /** At this step, we obtain * \f[ * \begin{cases} * v_{k,0} = \mbox{\tt vold} \\ * q_{k,0} = qold \\ * F_{k,+} = F(told,qold,vold) \\ * Work_{freefree} = M^{-1}_k (F^+_{k}) \mbox{stored in work_tdg} \\ * Work_{free} = M^{-1}_k (P^+_{2,k}+F^+_{k}) \mbox{stored in accFree} \\ * R_{free} = -h/2 * M^{-1}_k (P^+_{2,k}+F^+_{k}) \mbox{stored in ResiduFree} \\ * v_{k,1} = v_{k,0} + h * M^{-1}_k (P^+_{2,k}+F^+_{k}) \mbox{stored in v} \\ * q_{k,1} = q_{k,0} + \frac{h}{2} (v_{k,0} + v_{k,1}) \mbox{stored in q} \\ * \end{cases} * \f] **/ } DEBUG_PRINT("\n DECIDE STRATEGY\n"); /** Decide of the strategy impact or smooth multiplier. * Compute _isThereImpactInTheTimeStep */ _isThereImpactInTheTimeStep = false; if (!allOSNS->empty()) { for (unsigned int level = _simulation->levelMinForOutput(); level < _simulation->levelMaxForOutput(); level++) { _simulation->nonSmoothDynamicalSystem()->updateOutput(_simulation->nextTime(),level); } _simulation->updateIndexSets(); SP::Topology topo = _simulation->nonSmoothDynamicalSystem()->topology(); SP::InteractionsGraph indexSet3 = topo->indexSet(3); if (indexSet3->size() > 0) { _isThereImpactInTheTimeStep = true; DEBUG_PRINT("There is an impact in the step. indexSet3->size() > 0. _isThereImpactInTheTimeStep = true;\n"); } else { _isThereImpactInTheTimeStep = false; DEBUG_PRINT("There is no impact in the step. indexSet3->size() = 0. _isThereImpactInTheTimeStep = false;\n"); } } /* If _isThereImpactInTheTimeStep = true; * we recompute residuFree by removing the contribution of the nonimpulsive contact forces. * We add the contribution of the external forces at the end * of the time--step * If _isThereImpactInTheTimeStep = false; * we recompute residuFree by adding the contribution of the external forces at the end * and the contribution of the nonimpulsive contact forces that are computed by solving the osnsp. */ if (_isThereImpactInTheTimeStep) { DEBUG_PRINT("There is an impact in the step. indexSet3->size() > 0. _isThereImpactInTheTimeStep = true\n"); for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi) { if (!checkOSI(dsi)) continue; SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi); // type of the current DS Type::Siconos dsType = Type::value(*ds); /* \warning the following conditional statement should be removed with a MechanicalDS class */ if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS)) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); SP::SiconosVector residuFree = d->workspace(DynamicalSystem::freeresidu); SP::SiconosVector v = d->velocity(); SP::SiconosVector q = d->q(); SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit SP::SiconosMatrix M = d->mass(); // POINTER CONSTRUCTOR : contains mass matrix //residuFree->zero(); //v->zero(); SP::SiconosVector work_tdg = d->workspace(DynamicalSystem::free_tdg); assert(work_tdg); *residuFree = - 0.5 * h**work_tdg; d->computeMass(); DEBUG_EXPR(M->display()); if (d->forces()) { d->computeForces(t, q, v); *work_tdg = *(d->forces()); DEBUG_EXPR(d->forces()->display()); } M->PLUForwardBackwardInPlace(*work_tdg); // contains right (left limit) acceleration without contact force *residuFree -= 0.5 * h**work_tdg; DEBUG_EXPR(residuFree->display()); } else if (dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); SP::SiconosVector residuFree = d->workspace(DynamicalSystem::freeresidu); SP::SiconosVector v = d->velocity(); SP::SiconosVector q = d->q(); SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization; DEBUG_EXPR(M->display()); //residuFree->zero(); v->zero(); SP::SiconosVector work_tdg = d->workspace(DynamicalSystem::free_tdg); assert(work_tdg); *residuFree = 0.5 * h**work_tdg; work_tdg->zero(); if (d->forces()) { d->computeForces(t, q, v); *work_tdg += *(d->forces()); } M->PLUForwardBackwardInPlace(*work_tdg); // contains right (left limit) acceleration without contact force *residuFree -= 0.5 * h**work_tdg; DEBUG_EXPR(residuFree->display()); } else RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); } } else { DEBUG_PRINT("There is no impact in the step. indexSet3->size() = 0. _isThereImpactInTheTimeStep = false;\n"); // -- RIGHT SIDE -- // calculate acceleration without contact force for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi) { if (!checkOSI(dsi)) continue; SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi); // type of the current DS Type::Siconos dsType = Type::value(*ds); /* \warning the following conditional statement should be removed with a MechanicalDS class */ if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS)) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force accFree->zero(); // get right state from memory SP::SiconosVector q = d->q(); // contains position q_{k+1} SP::SiconosVector v = d->velocity(); // contains velocity v_{k+1}^- and not free velocity SP::SiconosMatrix M = d->mass(); // POINTER CONSTRUCTOR : contains mass matrix DEBUG_EXPR(accFree->display()); DEBUG_EXPR(q->display()); DEBUG_EXPR(v->display()); // Lagrangian Nonlinear Systems if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS) { d->computeMass(); DEBUG_EXPR(M->display()); if (d->forces()) { d->computeForces(t, q, v); *accFree += *(d->forces()); } } else RuntimeException::selfThrow ("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); M->PLUForwardBackwardInPlace(*accFree); // contains right (left limit) acceleration without contact force DEBUG_PRINT("accFree contains left limit acceleration at t^-_{k+1} without contact force :\n"); DEBUG_EXPR(accFree->display()); } else if (dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force accFree->zero(); // get right state from memory SP::SiconosVector q = d->q(); // contains position q_{k+1} SP::SiconosVector v = d->velocity(); // contains velocity v_{k+1}^- and not free velocity SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization; DEBUG_EXPR(accFree->display()); DEBUG_EXPR(q->display()); DEBUG_EXPR(v->display()); if (d->forces()) { d->computeForces(t, q, v); *accFree += *(d->forces()); } M->PLUForwardBackwardInPlace(*accFree); // contains right (left limit) acceleration without contact force DEBUG_PRINT("accFree contains left limit acceleration at t^-_{k+1} without contact force :\n"); DEBUG_EXPR(accFree->display()); } else RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); } // solve a LCP at acceleration level only for contacts which have been active at the beginning of the time-step if (!allOSNS->empty()) { // for (unsigned int level = _simulation->levelMinForOutput(); level < _simulation->levelMaxForOutput(); level++) // { // _simulation->updateOutput(level); // } // _simulation->updateIndexSets(); DEBUG_PRINT("We compute lambda^-_{k+1} \n"); InteractionsGraph::VIterator ui, uiend; SP::Interaction inter; for (std11::tie(ui, uiend) = indexSet2->vertices(); ui != uiend; ++ui) { inter = indexSet2->bundle(*ui); inter->relation()->computeJach(t, *inter, indexSet2->properties(*ui)); inter->relation()->computeJacg(t, *inter, indexSet2->properties(*ui)); } if (_simulation->nonSmoothDynamicalSystem()->topology()->hasChanged()) { for (OSNSIterator itOsns = allOSNS->begin(); itOsns != allOSNS->end(); ++itOsns) { (*itOsns)->setHasBeenUpdated(false); } } if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->hasInteractions())) { (*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->compute(t); DEBUG_EXPR((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->display();); _simulation->nonSmoothDynamicalSystem()->updateInput(_simulation->nextTime(),2); }
void KernelTest::t6() { SP::Model bouncingBall = Siconos::load("BouncingBall1.xml"); try { double T = bouncingBall->finalT(); double t0 = bouncingBall->t0(); double h = bouncingBall->simulation()->timeStep(); int N = (int)((T - t0) / h); // Number of time steps SP::DynamicalSystemsGraph dsg = bouncingBall->nonSmoothDynamicalSystem()->topology()->dSG(0); SP::LagrangianDS ball = std11::static_pointer_cast<LagrangianDS> (dsg->bundle(*(dsg->begin()))); SP::TimeStepping s = std11::static_pointer_cast<TimeStepping>(bouncingBall->simulation()); SP::Interaction inter; InteractionsGraph::VIterator ui, uiend; SP::InteractionsGraph indexSet0 = bouncingBall->nonSmoothDynamicalSystem()->topology()->indexSet(0); for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui) inter = indexSet0->bundle(*ui); // --- Get the values to be plotted --- // -> saved in a matrix dataPlot unsigned int outputSize = 5; SimpleMatrix dataPlot(N + 1, outputSize); SP::SiconosVector q = ball->q(); SP::SiconosVector v = ball->velocity(); SP::SiconosVector p = ball->p(1); SP::SiconosVector lambda = inter->lambda(1); dataPlot(0, 0) = bouncingBall->t0(); dataPlot(0, 1) = (*q)(0); dataPlot(0, 2) = (*v)(0); dataPlot(0, 3) = (*p)(0); dataPlot(0, 4) = (*lambda)(0); // --- Time loop --- cout << "====> Start computation ... " << endl << endl; // ==== Simulation loop - Writing without explicit event handling ===== int k = 1; boost::progress_display show_progress(N); boost::timer time; time.restart(); while (s->hasNextEvent()) { s->computeOneStep(); // --- Get values to be plotted --- dataPlot(k, 0) = s->nextTime(); dataPlot(k, 1) = (*q)(0); dataPlot(k, 2) = (*v)(0); dataPlot(k, 3) = (*p)(0); dataPlot(k, 4) = (*lambda)(0); s->nextStep(); ++show_progress; k++; } cout << endl << "End of computation - Number of iterations done: " << k - 1 << endl; cout << "Computation Time " << time.elapsed() << endl; // --- Output files --- cout << "====> Output file writing ..." << endl; dataPlot.resize(k, outputSize); ioMatrix::write("result.dat", "ascii", dataPlot, "noDim"); // Comparison with a reference file SimpleMatrix dataPlotRef(dataPlot); dataPlotRef.zero(); ioMatrix::read("result.ref", "ascii", dataPlotRef); if ((dataPlot - dataPlotRef).normInf() > 1e-12) { std::cout << "Warning. The results is rather different from the reference file :" << (dataPlot - dataPlotRef).normInf() << std::endl; CPPUNIT_ASSERT(false); } } catch (SiconosException e) { cout << e.report() << endl; CPPUNIT_ASSERT(false); } catch (...) { cout << "Exception caught in BouncingBallTS.cpp" << endl; CPPUNIT_ASSERT(false); } }
void SchatzmanPaoliOSI::updateState(const unsigned int level) { double h = simulationLink->timeStep(); double RelativeTol = simulationLink->relativeConvergenceTol(); bool useRCC = simulationLink->useRelativeConvergenceCriteron(); if (useRCC) simulationLink->setRelativeConvergenceCriterionHeld(true); DSIterator it; SP::SiconosMatrix W; for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it) { SP::DynamicalSystem ds = *it; W = WMap[ds->number()]; // Get the DS type Type::Siconos dsType = Type::value(*ds); // 1 - Lagrangian Systems if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS) { // get dynamical system SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); // SiconosVector *vfree = d->velocityFree(); SP::SiconosVector q = d->q(); bool baux = dsType == Type::LagrangianDS && useRCC && simulationLink->relativeConvergenceCriterionHeld(); if (level != LEVELMAX) { // To compute q, we solve W(q - qfree) = p if (d->p(level)) { *q = *d->p(level); // q = p W->PLUForwardBackwardInPlace(*q); } // if (d->boundaryConditions()) // for (vector<unsigned int>::iterator // itindex = d->boundaryConditions()->velocityIndices()->begin() ; // itindex != d->boundaryConditions()->velocityIndices()->end(); // ++itindex) // v->setValue(*itindex, 0.0); *q += * ds->workspace(DynamicalSystem::free); } else *q = * ds->workspace(DynamicalSystem::free); // Computation of the velocity SP::SiconosVector v = d->velocity(); SP::SiconosVector q_k_1 = d->qMemory()->getSiconosVector(1); // q_{k-1} // std::cout << "SchatzmanPaoliOSI::updateState - q_k_1 =" <<std::endl; // q_k_1->display(); // std::cout << "SchatzmanPaoliOSI::updateState - q =" <<std::endl; // q->display(); *v = 1.0 / (2.0 * h) * (*q - *q_k_1); // std::cout << "SchatzmanPaoliOSI::updateState - v =" <<std::endl; // v->display(); // int bc=0; // SP::SiconosVector columntmp(new SiconosVector(ds->getDim())); // if (d->boundaryConditions()) // { // for (vector<unsigned int>::iterator itindex = d->boundaryConditions()->velocityIndices()->begin() ; // itindex != d->boundaryConditions()->velocityIndices()->end(); // ++itindex) // { // _WBoundaryConditionsMap[ds]->getCol(bc,*columntmp); // /*\warning we assume that W is symmetric in the Lagrangian case*/ // double value = - inner_prod(*columntmp, *v); // value += (d->p(level))->getValue(*itindex); // /* \warning the computation of reactionToBoundaryConditions take into // account the contact impulse but not the external and internal forces. // A complete computation of the residue should be better */ // d->reactionToBoundaryConditions()->setValue(bc,value) ; // bc++; // } if (baux) { ds->subWorkVector(q, DynamicalSystem::local_buffer); double aux = ((ds->workspace(DynamicalSystem::local_buffer))->norm2()) / (ds->normRef()); if (aux > RelativeTol) simulationLink->setRelativeConvergenceCriterionHeld(false); } } //2 - Newton Euler Systems else if (dsType == Type::NewtonEulerDS) { // // get dynamical system // SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); // SP::SiconosVector v = d->velocity(); // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"SchatzmanPaoliOSI::updatestate prev v"<<endl; // v->display(); // #endif // /*d->p has been fill by the Relation->computeInput, it contains // B \lambda _{k+1}*/ // *v = *d->p(level); // v = p // d->luW()->PLUForwardBackwardInPlace(*v); // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"SchatzmanPaoliOSI::updatestate hWB lambda"<<endl; // v->display(); // #endif // *v += * ds->workspace(DynamicalSystem::free); // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"SchatzmanPaoliOSI::updatestate work free"<<endl; // ds->workspace(DynamicalSystem::free)->display(); // std::cout<<"SchatzmanPaoliOSI::updatestate new v"<<endl; // v->display(); // #endif // //compute q // //first step consists in computing \dot q. // //second step consists in updating q. // // // SP::SiconosMatrix T = d->T(); // SP::SiconosVector dotq = d->dotq(); // prod(*T,*v,*dotq,true); // // std::cout<<"SchatzmanPaoliOSI::updateState v"<<endl; // // v->display(); // // std::cout<<"SchatzmanPaoliOSI::updateState dotq"<<endl; // // dotq->display(); // SP::SiconosVector q = d->q(); // // -> get previous time step state // SP::SiconosVector dotqold = d->dotqMemory()->getSiconosVector(0); // SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); // // *q = *qold + h*(theta * *v +(1.0 - theta)* *vold) // double coeff = h*_theta; // scal(coeff, *dotq, *q) ; // q = h*theta*v // coeff = h*(1-_theta); // scal(coeff,*dotqold,*q,false); // q += h(1-theta)*vold // *q += *qold; // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"new q before normalizing"<<endl; // q->display(); // #endif // //q[3:6] must be normalized // d->normalizeq(); // dotq->setValue(3,(q->getValue(3)-qold->getValue(3))/h); // dotq->setValue(4,(q->getValue(4)-qold->getValue(4))/h); // dotq->setValue(5,(q->getValue(5)-qold->getValue(5))/h); // dotq->setValue(6,(q->getValue(6)-qold->getValue(6))/h); // d->updateT(); RuntimeException::selfThrow("SchatzmanPaoliOSI::updateState - not yet implemented for Dynamical system type: " + dsType); } else RuntimeException::selfThrow("SchatzmanPaoliOSI::updateState - not yet implemented for Dynamical system type: " + dsType); } }
void MLCPProjectOnConstraints::computeInteractionBlock(const InteractionsGraph::EDescriptor& ed) { // Computes matrix _interactionBlocks[inter1][inter2] (and allocates memory if // necessary) if inter1 and inter2 have commond DynamicalSystem. How // _interactionBlocks are computed depends explicitely on the type of // Relation of each Interaction. // Warning: we suppose that at this point, all non linear // operators (G for lagrangian relation for example) have been // computed through plug-in mechanism. #ifdef MLCPPROJ_DEBUG std::cout << "MLCPProjectOnConstraints::computeInteractionBlock currentInteractionBlock start " << std::endl; #endif // Get dimension of the NonSmoothLaw (ie dim of the interactionBlock) SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel()); SP::DynamicalSystem ds = indexSet->bundle(ed); SP::Interaction inter1 = indexSet->bundle(indexSet->source(ed)); SP::Interaction inter2 = indexSet->bundle(indexSet->target(ed)); // For the edge 'ds', we need to find relative position of this ds // in inter1 and inter2 relation matrices (--> pos1 and pos2 below) // - find if ds is source or target in inter_i InteractionsGraph::VDescriptor vertex_inter; // - get the corresponding position unsigned int pos1, pos2; // source of inter1 : vertex_inter = indexSet->source(ed); VectorOfSMatrices& workMInter1 = *indexSet->properties(vertex_inter).workMatrices; SP::OneStepIntegrator Osi = indexSet->properties(vertex_inter).osi; SP::DynamicalSystem tmpds = indexSet->properties(vertex_inter).source; if (tmpds == ds) pos1 = indexSet->properties(vertex_inter).source_pos; else { tmpds = indexSet->properties(vertex_inter).target; pos1 = indexSet->properties(vertex_inter).target_pos; } // now, inter2 vertex_inter = indexSet->target(ed); VectorOfSMatrices& workMInter2 = *indexSet->properties(vertex_inter).workMatrices; tmpds = indexSet->properties(vertex_inter).source; if (tmpds == ds) pos2 = indexSet->properties(vertex_inter).source_pos; else { tmpds = indexSet->properties(vertex_inter).target; pos2 = indexSet->properties(vertex_inter).target_pos; } unsigned int index1 = indexSet->index(indexSet->source(ed)); unsigned int index2 = indexSet->index(indexSet->target(ed)); unsigned int sizeY1 = 0; sizeY1 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints> (_M)->computeSizeForProjection(inter1); unsigned int sizeY2 = 0; sizeY2 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints> (_M)->computeSizeForProjection(inter2); SP::SiconosMatrix currentInteractionBlock; assert(index1 != index2); if (index2 > index1) // upper block { // if (! indexSet->properties(ed).upper_block) // { // indexSet->properties(ed).upper_block.reset(new SimpleMatrix(sizeY1, sizeY2)); // } currentInteractionBlock = indexSet->upper_blockProj[ed]; #ifdef MLCPPROJ_DEBUG std::cout << "MLCPProjectOnConstraints::computeInteractionBlock currentInteractionBlock " << std::endl; // currentInteractionBlock->display(); std::cout << "sizeY1 " << sizeY1 << std::endl; std::cout << "sizeY2 " << sizeY2 << std::endl; std::cout << "upper_blockProj " << indexSet->upper_blockProj[ed].get() << " of edge " << ed << " of size " << currentInteractionBlock->size(0) << " x " << currentInteractionBlock->size(0) << " for interaction " << inter1->number() << " and interaction " << inter2->number() << std::endl; // std::cout<<"inter1->display() "<< inter1->number()<< std::endl; //inter1->display(); // std::cout<<"inter2->display() "<< inter2->number()<< std::endl; //inter2->display(); #endif assert(currentInteractionBlock->size(0) == sizeY1); assert(currentInteractionBlock->size(1) == sizeY2); } else // lower block { // if (! indexSet->properties(ed).lower_block) // { // indexSet->properties(ed).lower_block.reset(new SimpleMatrix(sizeY1, sizeY2)); // } assert(indexSet->lower_blockProj[ed]->size(0) == sizeY1); assert(indexSet->lower_blockProj[ed]->size(1) == sizeY2); currentInteractionBlock = indexSet->lower_blockProj[ed]; } SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock; RELATION::TYPES relationType1, relationType2; // General form of the interactionBlock is : interactionBlock = // a*extraInteractionBlock + b * leftInteractionBlock * centralInteractionBlocks // * rightInteractionBlock a and b are scalars, centralInteractionBlocks a // matrix depending on the integrator (and on the DS), the // simulation type ... left, right and extra depend on the relation // type and the non smooth law. relationType1 = inter1->relation()->getType(); relationType2 = inter2->relation()->getType(); if (relationType1 == NewtonEuler && relationType2 == NewtonEuler) { assert(inter1 != inter2); currentInteractionBlock->zero(); #ifdef MLCPPROJ_WITH_CT unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getDim(); leftInteractionBlock.reset(new SimpleMatrix(sizeY1, sizeDS)); inter1->getLeftInteractionBlockForDS(pos1, leftInteractionBlock); SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds)); SP::SimpleMatrix T = neds->T(); SP::SimpleMatrix workT(new SimpleMatrix(*T)); workT->trans(); SP::SimpleMatrix workT2(new SimpleMatrix(6, 6)); prod(*workT, *T, *workT2, true); rightInteractionBlock.reset(new SimpleMatrix(sizeY2, sizeDS)); inter2->getLeftInteractionBlockForDS(pos2, rightInteractionBlock); rightInteractionBlock->trans(); workT2->PLUForwardBackwardInPlace(*rightInteractionBlock); prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false); #else unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getqDim(); leftInteractionBlock.reset(new SimpleMatrix(sizeY1, sizeDS)); inter1->getLeftInteractionBlockForDSProjectOnConstraints(pos1, leftInteractionBlock); SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds)); rightInteractionBlock.reset(new SimpleMatrix(sizeY2, sizeDS)); inter2->getLeftInteractionBlockForDSProjectOnConstraints(pos2, rightInteractionBlock); rightInteractionBlock->trans(); prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false); } #endif else if (relationType1 == Lagrangian && relationType2 == Lagrangian) { unsigned int sizeDS = ds->getDim(); leftInteractionBlock.reset(new SimpleMatrix(sizeY1, sizeDS)); inter1->getLeftInteractionBlockForDS(pos1, leftInteractionBlock, workMInter1); Type::Siconos dsType = Type::value(*ds); if (dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); if (d->boundaryConditions()) // V.A. Should we do that ? { for (std::vector<unsigned int>::iterator itindex = d->boundaryConditions()->velocityIndices()->begin() ; itindex != d->boundaryConditions()->velocityIndices()->end(); ++itindex) { // (sizeY1,sizeDS)); SP::SiconosVector coltmp(new SiconosVector(sizeY1)); coltmp->zero(); leftInteractionBlock->setCol(*itindex, *coltmp); } } } #ifdef MLCPPROJ_DEBUG std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : leftInteractionBlock" << std::endl; leftInteractionBlock->display(); #endif // inter1 != inter2 rightInteractionBlock.reset(new SimpleMatrix(sizeY2, sizeDS)); inter2->getLeftInteractionBlockForDS(pos2, rightInteractionBlock, workMInter2); #ifdef MLCPPROJ_DEBUG std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : rightInteractionBlock" << std::endl; rightInteractionBlock->display(); #endif // Warning: we use getLeft for Right interactionBlock // because right = transpose(left) and because of // size checking inside the getBlock function, a // getRight call will fail. SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds); #ifdef MLCPPROJ_DEBUG std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : centralInteractionBlocks " << std::endl; centralInteractionBlock->display(); #endif rightInteractionBlock->trans(); if (_useMassNormalization) { centralInteractionBlock->PLUForwardBackwardInPlace(*rightInteractionBlock); //*currentInteractionBlock += *leftInteractionBlock ** work; prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false); } else { prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false); } #ifdef MLCPPROJ_DEBUG std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : currentInteractionBlock" << std::endl; currentInteractionBlock->display(); #endif } else RuntimeException::selfThrow("MLCPProjectOnConstraints::computeInteractionBlock not yet implemented for relation of type " + relationType1); }
void MLCPProjectOnConstraints::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd) { SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel()); SP::DynamicalSystem DS1 = indexSet->properties(vd).source; SP::DynamicalSystem DS2 = indexSet->properties(vd).target; SP::Interaction inter = indexSet->bundle(vd); SP::OneStepIntegrator Osi = indexSet->properties(vd).osi; unsigned int pos1, pos2; pos1 = indexSet->properties(vd).source_pos; pos2 = indexSet->properties(vd).target_pos; unsigned int sizeY = 0; sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints> (_M)->computeSizeForProjection(inter); #ifdef MLCPPROJ_DEBUG std::cout << "\nMLCPProjectOnConstraints::computeDiagonalInteractionBlock" <<std::endl; std::cout << "indexSetLevel()" << indexSetLevel() << std::endl; // std::cout << "indexSet :"<< indexSet << std::endl; // std::cout << "vd :"<< vd << std::endl; // indexSet->display(); // std::cout << "DS1 :" << std::endl; // DS1->display(); // std::cout << "DS2 :" << std::endl; // DS2->display(); #endif assert(indexSet->blockProj[vd]); SP::SiconosMatrix currentInteractionBlock = indexSet->blockProj[vd]; #ifdef MLCPPROJ_DEBUG // std::cout<<"MLCPProjectOnConstraints::computeDiagonalInteractionBlock "<<std::endl; // currentInteractionBlock->display(); std::cout << "sizeY " << sizeY << std::endl; std::cout << "blockProj " << indexSet->blockProj[vd].get() << " of edge " << vd << " of size " << currentInteractionBlock->size(0) << " x " << currentInteractionBlock->size(0) << " for interaction " << inter->number() << std::endl; // std::cout<<"inter1->display() "<< inter1->number()<< std::endl; //inter1->display(); // std::cout<<"inter2->display() "<< inter2->number()<< std::endl; //inter2->display(); #endif assert(currentInteractionBlock->size(0) == sizeY); assert(currentInteractionBlock->size(1) == sizeY); if (!_hasBeenUpdated) computeOptions(inter, inter); // Computes matrix _interactionBlocks[inter1][inter2] (and allocates memory if // necessary) if inter1 and inter2 have commond DynamicalSystem. How // _interactionBlocks are computed depends explicitely on the type of // Relation of each Interaction. // Warning: we suppose that at this point, all non linear // operators (G for lagrangian relation for example) have been // computed through plug-in mechanism. // Get the W and Theta maps of one of the Interaction - // Warning: in the current version, if OSI!=MoreauJeanOSI, this fails. // If OSI = MOREAU, centralInteractionBlocks = W if OSI = LSODAR, // centralInteractionBlocks = M (mass matrices) SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock, leftInteractionBlock1; // General form of the interactionBlock is : interactionBlock = // a*extraInteractionBlock + b * leftInteractionBlock * centralInteractionBlocks // * rightInteractionBlock a and b are scalars, centralInteractionBlocks a // matrix depending on the integrator (and on the DS), the // simulation type ... left, right and extra depend on the relation // type and the non smooth law. VectorOfSMatrices& workMInter = *indexSet->properties(vd).workMatrices; currentInteractionBlock->zero(); // loop over the common DS bool endl = false; unsigned int pos = pos1; for (SP::DynamicalSystem ds = DS1; !endl; ds = DS2) { assert(ds == DS1 || ds == DS2); endl = (ds == DS2); if (Type::value(*ds) == Type::LagrangianLinearTIDS || Type::value(*ds) == Type::LagrangianDS) { if (inter->relation()->getType() != Lagrangian) { RuntimeException::selfThrow( "MLCPProjectOnConstraints::computeDiagonalInteractionBlock - relation is not of type Lagrangian with a LagrangianDS."); } SP::LagrangianDS lds = (std11::static_pointer_cast<LagrangianDS>(ds)); unsigned int sizeDS = lds->getDim(); leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS)); inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter); if (lds->boundaryConditions()) // V.A. Should we do that ? { for (std::vector<unsigned int>::iterator itindex = lds->boundaryConditions()->velocityIndices()->begin() ; itindex != lds->boundaryConditions()->velocityIndices()->end(); ++itindex) { // (sizeY,sizeDS)); SP::SiconosVector coltmp(new SiconosVector(sizeY)); coltmp->zero(); leftInteractionBlock->setCol(*itindex, *coltmp); } } // (inter1 == inter2) SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock)); // // std::cout<<"LinearOSNS : leftUBlock\n"; // work->display(); work->trans(); // std::cout<<"LinearOSNS::computeInteractionBlock leftInteractionBlock"<<endl; // leftInteractionBlock->display(); if (_useMassNormalization) { SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds); centralInteractionBlock->PLUForwardBackwardInPlace(*work); prod(*leftInteractionBlock, *work, *currentInteractionBlock, false); // gemm(CblasNoTrans,CblasNoTrans,1.0,*leftInteractionBlock,*work,1.0,*currentInteractionBlock); } else { prod(*leftInteractionBlock, *work, *currentInteractionBlock, false); } //*currentInteractionBlock *=h; } else if (Type::value(*ds) == Type::NewtonEulerDS) { if (inter->relation()->getType() != NewtonEuler) { RuntimeException::selfThrow("MLCPProjectOnConstraints::computeDiagonalInteractionBlock - relation is not from NewtonEulerR."); } SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds)); #ifdef MLCPPROJ_WITH_CT unsigned int sizeDS = neds->getDim(); SP::SimpleMatrix T = neds->T(); SP::SimpleMatrix workT(new SimpleMatrix(*T)); workT->trans(); SP::SimpleMatrix workT2(new SimpleMatrix(6, 6)); prod(*workT, *T, *workT2, true); leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS)); inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock); SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock)); std::cout << "LinearOSNS : leftUBlock\n"; work->display(); work->trans(); std::cout << "LinearOSNS::computeInteractionBlock workT2" <<std::endl; workT2->display(); workT2->PLUForwardBackwardInPlace(*work); prod(*leftInteractionBlock, *work, *currentInteractionBlock, false); #else if (0) //(std11::static_pointer_cast<NewtonEulerR> inter->relation())->_isConstact){ { // unsigned int sizeDS = neds->getDim(); // SP::SimpleMatrix T = neds->T(); // SP::SimpleMatrix workT(new SimpleMatrix(*T)); // workT->trans(); // SP::SimpleMatrix workT2(new SimpleMatrix(6, 6)); // prod(*workT, *T, *workT2, true); // leftInteractionBlock1.reset(new SimpleMatrix(sizeY, sizeDS)); // inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock); // leftInteractionBlock.reset(new SimpleMatrix(1, sizeDS)); // for (unsigned int ii = 0; ii < sizeDS; ii++) // leftInteractionBlock->setValue(1, ii, leftInteractionBlock1->getValue(1, ii)); // // SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock)); // //cout<<"LinearOSNS : leftUBlock\n"; // //work->display(); // work->trans(); // //cout<<"LinearOSNS::computeInteractionBlock workT2"<<endl; // //workT2->display(); // workT2->PLUForwardBackwardInPlace(*work); // prod(*leftInteractionBlock, *work, *currentInteractionBlock, false); } else { unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getqDim(); leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS)); inter->getLeftInteractionBlockForDSProjectOnConstraints(pos, leftInteractionBlock); // #ifdef MLCPPROJ_DEBUG // std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock - NewtonEuler case leftInteractionBlock : " << std::endl; // leftInteractionBlock->display(); // #endif SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock)); //cout<<"LinearOSNS sizeY="<<sizeY<<": leftUBlock\n"; //work->display(); work->trans(); prod(*leftInteractionBlock, *work, *currentInteractionBlock, false); // #ifdef MLCPPROJ_DEBUG // std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock - NewtonEuler case currentInteractionBlock : "<< std::endl; // currentInteractionBlock->display(); // #endif } } else RuntimeException::selfThrow("MLCPProjectOnConstraints::computeDiagonalInteractionBlock - ds is not from NewtonEulerDS neither a LagrangianDS."); #endif #ifdef MLCPPROJ_DEBUG std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock DiaginteractionBlock " << std::endl; currentInteractionBlock->display(); #endif // Set pos for next loop. pos = pos2; } }
void MLCPProjectOnConstraints::postComputeLagrangianR(SP::Interaction inter, unsigned int pos) { SP::LagrangianR lr = std11::static_pointer_cast<LagrangianR>(inter->relation()); #ifdef MLCPPROJ_DEBUG printf("MLCPProjectOnConstraints::postComputeLagrangian inter->y(0)\n"); inter->y(0)->display(); printf("MLCPProjectOnConstraints::postComputeLagrangian lr->jachq \n"); lr->jachq()->display(); printf("MLCPProjectOnConstraints::postComputeLagrangianR q before update\n"); SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel()); InteractionsGraph::VDescriptor ui = indexSet->descriptor(inter); InteractionsGraph::OEIterator oei, oeiend; for(std11::tie(oei, oeiend) = indexSet->out_edges(ui); oei != oeiend; ++oei) { SP::LagrangianDS lds = std11::static_pointer_cast<LagrangianDS>(indexSet->bundle(*oei)); lds->q()->display(); } #endif //unsigned int sizeY = inter->nonSmoothLaw()->size(); // y and lambda vectors SP::SiconosVector lambda = inter->lambda(0); SP::SiconosVector y = inter->y(0); unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints> (_M)->computeSizeForProjection(inter); // Copy _w/_z values, starting from index pos into y/lambda. //setBlock(*_w, y, sizeY, pos, 0); setBlock(*_z, lambda, sizeY, pos, 0); #ifdef MLCPPROJ_DEBUG printf("MLCPP lambda of Interaction is pos =%i :\n", pos); // aBuff->display(); lambda->display(); unsigned int nslawsize = inter->nonSmoothLaw()->size(); SP::SiconosVector aBuff(new SiconosVector(nslawsize)); setBlock(*_z, aBuff, sizeY, pos, 0); SP::SiconosMatrix J = lr->jachq(); SP::SimpleMatrix aux(new SimpleMatrix(*J)); aux->trans(); // SP::SiconosVector tmp(new SiconosVector(*(lr->q()))); // prod(*aux, *aBuff, *(tmp), false); // //prod(*aux,*lambda,*(lr->q()),false); // std:: std::cout << " tmp = tmp + J^T * lambda" << std::endl; // tmp->display(); #endif // // WARNING : Must not be done here. and should be called with the correct time. // // compute p(0) // inter->computeInput(0.0 ,0); // // \warning aBuff should normally be in lambda[0] // // The update of the position in DS should be made // // in MoreauJeanOSI::upateState or ProjectedMoreauJeanOSI::updateState // SP::SiconosMatrix J=lr->jachq(); // SP::SimpleMatrix aux(new SimpleMatrix(*J)); // aux->trans(); // SP::SiconosVector tmp (new SiconosVector(*(lr->q()))); // std:: std::cout << " tmp ="<<std::endl; // tmp->display(); // std:: std::cout << " lr->q() ="<<std::endl; // lr->q()->display(); // //prod(*aux,*lambda,*(lr->q()),false); // prod(*aux,*aBuff,*(tmp),false); // std:: std::cout << " tmp = tmp + J * lambda"<<std::endl; // tmp->display(); // // The following step should be done on MoreauJeanOSI::upateState or ProjectedMoreauJeanOSI::updateState // DSIterator itDS = inter->dynamicalSystemsBegin(); // while(itDS!=inter->dynamicalSystemsEnd()) // { // Type::Siconos dsType = Type::value(**itDS); // if((dsType !=Type::LagrangianDS) and // (dsType !=Type::LagrangianLinearTIDS) ) // { // RuntimeException::selfThrow("MLCPProjectOnConstraint::postCompute- ds is not of Lagrangian DS type."); // } // SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (*itDS); // SP::SiconosVector q = d->q(); // *q += *d->p(0); // std::cout << " q=" << std::endl; // q->display(); // itDS++; // } // if ((*lr->q() - *tmp).normInf() > 1e-12) // { // RuntimeException::selfThrow("youyou"); // } #ifdef MLCPPROJ_DEBUG printf("MLCPProjectOnConstraints::postComputeLagrangianR _z\n"); _z->display(); printf("MLCPProjectOnConstraints::postComputeLagrangianR updated\n"); VectorOfBlockVectors& DSlink = *(indexSet->properties(ui)).DSlink; // (*DSlink[LagrangianR::q0]).display(); // (lr->q())->display(); #endif //RuntimeException::selfThrow("MLCPProjectOnConstraints::postComputeLagrangianR() - not yet implemented"); }