Color LEDStrip::nearColorTo(Color color1, Color color2, byte fadeSpeed) { Color newColor = color1; // calculate step size based on the maximum remaining difference unsigned char maximumDifference = getMaximumDifference(color1, color2); newColor.r = nearValue(color1.r, color2.r, getStepSize(color1.r, color2.r, fadeSpeed, maximumDifference)); newColor.g = nearValue(color1.g, color2.g, getStepSize(color1.g, color2.g, fadeSpeed, maximumDifference)); newColor.b = nearValue(color1.b, color2.b, getStepSize(color1.b, color2.b, fadeSpeed, maximumDifference)); newColor.a = nearValue(color1.a, color2.a, getStepSize(color1.b, color2.b, fadeSpeed, maximumDifference)); return newColor; }
Controller::Controller(byte *temp/*, int leftOutputPin, int rightOutputPin, int leftEdgePin, int rightEdgePin, int leftButtonPin, int rightButtonPin*/) { _times = temp; // set pinmodes pinMode(MOVE_LEFT, OUTPUT); //digitalWrite(leftOutputPin, HIGH); //_leftOutputPin = leftOutputPin; pinMode(MOVE_RIGHT, OUTPUT); //digitalWrite(rightOutputPin, HIGH); //_rightOutputPin = rightOutputPin; pinMode(LEFT_BTN, INPUT); //_leftButtonPin = leftButtonPin; pinMode(RIGHT_BTN, INPUT); //_rightButtonPin = rightButtonPin; pinMode(LEFT_END, INPUT); //_leftEdgePin = leftEdgePin; pinMode(RIGHT_END, INPUT); //_rightEdgePin = rightEdgePin; getInterval(); getStepSize(); }
void Integrator::step( int steps ) { timeval start, end; gettimeofday( &start, 0 ); mSimpleMinimizations = 0; mQuadraticMinimizations = 0; for( mLastCompleted = 0; mLastCompleted < steps; ++mLastCompleted ) { if( DoStep() == false ) { break; } } // Update Time context->setTime( context->getTime() + getStepSize() * mLastCompleted ); // Print Minimizations const unsigned int total = mSimpleMinimizations + mQuadraticMinimizations; const double averageSimple = ( double )mSimpleMinimizations / ( double )mLastCompleted; const double averageQuadratic = ( double )mQuadraticMinimizations / ( double )mLastCompleted; const double averageTotal = ( double )total / ( double )mLastCompleted; std::cout << "[OpenMM::Minimize] " << total << " total minimizations( " << mSimpleMinimizations << " simple, " << mQuadraticMinimizations << " quadratic ). " << averageTotal << " per-step minimizations( " << averageSimple << " simple, " << averageQuadratic << " quadratic ). Steps: " << mLastCompleted << std::endl; gettimeofday( &end, 0 ); double elapsed = ( end.tv_sec - start.tv_sec ) * 1000.0 + ( end.tv_usec - start.tv_usec ) / 1000.0; std::cout << "[Integrator] Total dynamics: " << elapsed << "ms" << std::endl; }
//! Writes attributes of the element. void CGUISpinBox::serializeAttributes(io::IAttributes * out, io::SAttributeReadWriteOptions * options) const { IGUIElement::serializeAttributes(out, options); out->addFloat("Min", getMin()); out->addFloat("Max", getMax()); out->addFloat("Step", getStepSize()); out->addInt("DecimalPlaces", DecimalPlaces); }
PathBasedFlowMoveWithStep* ObjectManager::getPathBasedFlowMoveWithStep(){ assert(getIfAdditive() == true); if (flowMoveWithStep_ == NULL) { std::cout << "Creating PathBasedFlowMoveWithStep" << std::endl; flowMoveWithStep_ = new PathBasedFlowMoveWithStep(getStepSize(), getDescDirectionPath(), getFloatValue("ZERO_FLOW")); flowMove_ = flowMoveWithStep_; std::cout << "PathBasedFlowMoveWithStep created" << std::endl; } return flowMoveWithStep_; };
virtual void run() override { auto clock = std::make_shared<SteppableClock>(3E-3f); ClockFactory::get(clock); std::unique_ptr<MultiRotorParams> params = MultiRotorParamsFactory::createConfig("SimpleFlight"); MultiRotor vehicle; std::unique_ptr<Environment> environment; vehicle.initialize(params.get(), Pose(), GeoPoint(), environment); std::vector<UpdatableObject*> vehicles = { &vehicle }; std::unique_ptr<PhysicsEngineBase> physics_engine(new FastPhysicsEngine()); PhysicsWorld physics_world(physics_engine.get(), vehicles, static_cast<uint64_t>(clock->getStepSize() * 1E9)); DroneControllerBase* controller = params->getController(); testAssert(controller != nullptr, "Controller was null"); std::string message; testAssert(controller->isAvailable(message), message); clock->sleep_for(0.04f); Utils::getSetMinLogLevel(true, 100); DirectCancelableBase cancellable(controller, &vehicle); controller->enableApiControl(true); controller->armDisarm(true, cancellable); controller->takeoff(10, cancellable); clock->sleep_for(2.0f); Utils::getSetMinLogLevel(true); controller->moveToPosition(-5, -5, -5, 5, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0, cancellable); clock->sleep_for(2.0f); while (true) { clock->sleep_for(0.1f); controller->getStatusMessages(messages_); for (const auto& status_message : messages_) { std::cout << status_message << std::endl; } messages_.clear(); } }
PathBasedFlowMove* ObjectManager::getPathBasedFlowMove(){ if (flowMove_ == NULL) { std::cout << "Creating PathBasedFlowMove" << std::endl; PathAlgoType algo = getPathAlgoType(); assert(algo != Nothing); PathApp app = getPathAlgoApp(); if (app == APP3) { if (algo == PE) { flowMove_ = new PathBasedFlowMovePE(getDescDirectionPath()); } else if (algo == GP) { flowMoveGP_ = new PathBasedFlowMoveGP(getFloatValue("ALPHA"), getDescDirectionPath()); flowMove_ = flowMoveGP_; } else { throw Error("Approach 3 is not implemented for this algorithm"); } } else { flowMoveWithStep_ = new PathBasedFlowMoveWithStep(getStepSize(), getDescDirectionPath(), getFloatValue("ZERO_FLOW")); flowMove_ = flowMoveWithStep_; } std::cout << "PathBasedFlowMove created" << std::endl; } return flowMove_; };
void BeamColumnJoint3d::getGlobalDispls(Vector &dg) { // local variables that will be used in this method int converge = 0; int linesearch = 0; int totalCount = 0; int dtConverge = 0; int incCount = 0; int count = 0; int maxTotalCount = 1000; int maxCount = 20; double loadStep = 0.0; double dLoadStep = 1.0; double stepSize; Vector uExtOld(24); uExtOld.Zero(); Vector uExt(12); uExt.Zero(); Vector duExt(12); duExt.Zero(); Vector uIntOld(4); uIntOld.Zero(); Vector uInt(4); uInt.Zero(); Vector duInt(4); duInt.Zero(); Vector duIntTemp(4); duIntTemp.Zero(); Vector intEq(4); intEq.Zero(); Vector intEqLast(4); intEqLast.Zero(); Vector Uepr(24); Uepr.Zero(); Vector UeprInt(4); UeprInt.Zero(); Vector Ut(24); Ut.Zero(); Vector duExtTemp(24); duExtTemp.Zero(); Vector disp1 = nodePtr[0]->getTrialDisp(); Vector disp2 = nodePtr[1]->getTrialDisp(); Vector disp3 = nodePtr[2]->getTrialDisp(); Vector disp4 = nodePtr[3]->getTrialDisp(); for (int i = 0; i < 6; i++) { Ut(i) = disp1(i); Ut(i+6) = disp2(i); Ut(i+12) = disp3(i); Ut(i+18) = disp4(i); } Uepr = Uecommit; UeprInt = UeIntcommit; uExtOld = Uepr; duExtTemp = Ut - Uepr; duExt.addMatrixVector(0.0,Transf,duExtTemp,1.0); uExt.addMatrixVector(0.0,Transf,uExtOld,1.0); uIntOld = UeprInt; uInt = uIntOld; double tol = 1e-12; double tolIntEq = tol; double toluInt = (tol>tol*uInt.Norm())? tol:tol*uInt.Norm(); double tolIntEqdU = tol; double ctolIntEqdU = tol; double ctolIntEq = tol; double normDuInt = toluInt; double normIntEq = tolIntEq; double normIntEqdU = tolIntEqdU; Vector u(16); u.Zero(); double engrLast = 0.0; double engr = 0.0; Vector fSpring(13); fSpring.Zero(); Vector kSpring(13); kSpring.Zero(); Matrix dintEq_du(4,4); dintEq_du.Zero(); Matrix df_dDef(13,13); df_dDef.Zero(); Matrix tempintEq_du (4,13); tempintEq_du.Zero(); while ((loadStep < 1.0) && (totalCount < maxTotalCount)) { count = 0; converge = 0; dtConverge = 0; while ((!converge) && (count < maxCount)) { if (dLoadStep <= 1e-3) { dLoadStep = dLoadStep; } totalCount ++; count ++; for (int ic = 0; ic < 12; ic++ ) { u(ic) = uExt(ic) + duExt(ic); } u(12) = uInt(0); u(13) = uInt(1); u(14) = uInt(2); u(15) = uInt(3); getMatResponse(u,fSpring,kSpring); // performs internal equilibrium intEq(0) = -fSpring(2)-fSpring(3)+fSpring(9)-fSpring(12)/elemHeight; intEq(1) = fSpring(1)-fSpring(5)-fSpring(7)+fSpring(12)/elemWidth; intEq(2) = -fSpring(4)-fSpring(8)+fSpring(10)+fSpring(12)/elemHeight; intEq(3) = fSpring(0)-fSpring(6)-fSpring(11)-fSpring(12)/elemWidth; matDiag(kSpring, df_dDef); //////////////////////// dintEq_du = dg_df*df_dDef*dDef_du tempintEq_du.addMatrixProduct(0.0,dg_df,df_dDef,1.0); dintEq_du.addMatrixProduct(0.0,tempintEq_du,dDef_du,1.0); normIntEq = intEq.Norm(); normIntEqdU = 0.0; for (int jc = 0; jc<4 ; jc++) { normIntEqdU += intEq(jc)*duInt(jc); } normIntEqdU = fabs(normIntEqdU); if (totalCount == 1) { tolIntEq = (tol>tol*normIntEq) ? tol:tol*normIntEq; tolIntEqdU = tol; } else if (totalCount == 2) { tolIntEqdU = (tol>tol*normIntEqdU) ? tol:tol*normIntEqdU; } ctolIntEqdU = (tolIntEqdU*dLoadStep > tol) ? tolIntEqdU*dLoadStep:tol; ctolIntEq = (tolIntEq*dLoadStep > tol) ? tolIntEq*dLoadStep:tol; // check for convergence starts if ((normIntEq < tol) || ((normIntEqdU < tol) && (count >1)) || (normDuInt < toluInt) || (dLoadStep < 1e-3)) { if ((normIntEq > ctolIntEq) || (normIntEqdU > tolIntEqdU) || (normDuInt > toluInt)) { dtConverge = 1; } else { dtConverge = 0; } converge = 1; loadStep = loadStep + dLoadStep; if (fabs(1.0 - loadStep) < tol) { loadStep = 1.0; } } else { ////////////// duInt = -dintEq_du/intEq dintEq_du.Solve(intEq,duInt); duInt *= -1; normDuInt = duInt.Norm(); if (!linesearch) { uInt = uInt + duInt; } else { engrLast = 0.0; engr = 0.0; for (int jd = 0; jd<4 ; jd++) { engrLast += duInt(jd)*intEqLast(jd); engr += duInt(jd)*intEq(jd); } if (fabs(engr) > tol*engrLast) { duIntTemp = duInt; duIntTemp *= -1; // lineSearch algorithm requirement stepSize = getStepSize(engrLast,engr,uExt,duExt,uInt,duIntTemp,tol); if (fabs(stepSize) > 0.001) { uInt = uInt + stepSize*duInt; } else { uInt = uInt + duInt; } } else { uInt = uInt + duInt; } intEqLast = intEq; } } } if (!converge && loadStep < 1.0) { incCount = 0; maxCount = 25; if (!linesearch) { linesearch = 1; uInt = uIntOld; duInt.Zero(); } else { opserr << "WARNING : BeamColumnJoint::getGlobalDispls() - convergence problem in state determination" << endln; uInt = uIntOld; duInt.Zero(); duExt = duExt*0.1; dLoadStep = dLoadStep*0.1; } } else if (loadStep < 1.0) { maxCount = 10; incCount ++; normDuInt = toluInt; if ((incCount < maxCount) || dtConverge) { uExt = uExt + duExt; if (loadStep + dLoadStep > 1.0) { duExt = duExt*(1.0 - loadStep)/dLoadStep; dLoadStep = 1.0 - loadStep; incCount = 9; } } else { incCount = 0; uExt = uExt + duExt; dLoadStep = dLoadStep*10; if (loadStep + dLoadStep > 1.0) { uExt = uExt + duExt*(1.0 - loadStep)/dLoadStep; dLoadStep = 1.0 - loadStep; incCount = 9; } } } } // determination of stiffness matrix and the residual force vector for the element formR(fSpring); formK(kSpring); for (int ig = 0; ig < 25; ig++ ) { if (ig<24) { dg(ig) = Ut(ig); } } dg(24) = uInt(0); dg(25) = uInt(1); dg(26) = uInt(2); dg(27) = uInt(3); }
void PhysicsHandler::handleUpdate(EventDetails* const details) { commitChanges(); //Start the physics timing statistic StatTimeElem *PhysicsTimeStatElem = StatCollector::getGlobalElem(statPhysicsTime); if(PhysicsTimeStatElem) { PhysicsTimeStatElem->start(); } _TimeSinceLast += dynamic_cast<UpdateEventDetails* const>(details)->getElapsedTime(); if(osgFloor(_TimeSinceLast/getStepSize()) > getMaxStepsPerUpdate()) { SWARNING << "Physics Simulation slowing: dropping " << osgFloor(_TimeSinceLast/getStepSize())-getMaxStepsPerUpdate() << " steps" << std::endl; _TimeSinceLast = getMaxStepsPerUpdate()*getStepSize(); } StatIntElem *NPhysicsStepsStatElem = StatCollector::getGlobalElem(statNPhysicsSteps); StatTimeElem *CollisionTimeStatElem = StatCollector::getGlobalElem(statCollisionTime); StatTimeElem *DynamicsTimeStatElem = StatCollector::getGlobalElem(statSimulationTime); while(_TimeSinceLast > getStepSize()) { //Increment the steps statistic if(NPhysicsStepsStatElem) { NPhysicsStepsStatElem->inc(); } //*********** Collision Checks ************* //Start the collision timing statistic if(CollisionTimeStatElem) { CollisionTimeStatElem->start(); } //Do collision checks for(UInt32 i(0) ; i<getMFSpaces()->size() ; ++i) { getSpaces(i)->Collide(getWorld()); } //Stop the collision timing statistic if(CollisionTimeStatElem) { CollisionTimeStatElem->stop(); } //*********** Simulation step ************* //Start the simulation timing statistic if(DynamicsTimeStatElem) { DynamicsTimeStatElem->start(); } //Step the dynamics simulation getWorld()->worldQuickStep(getStepSize()); //Stop the simulation timing statistic if(DynamicsTimeStatElem) { DynamicsTimeStatElem->stop(); } //Decrease the time since last simulation step _TimeSinceLast -= getStepSize(); } StatRealElem *NCollisionTestsStatElem = StatCollector::getGlobalElem(statNCollisionTests); if(NCollisionTestsStatElem) { NCollisionTestsStatElem->set(NCollisionTestsStatElem->get()/static_cast<Real32>(NPhysicsStepsStatElem->get())); } StatRealElem *NCollisionsStatElem = StatCollector::getGlobalElem(statNCollisions); if(NCollisionsStatElem) { NCollisionsStatElem->set(NCollisionsStatElem->get()/static_cast<Real32>(NPhysicsStepsStatElem->get())); } StatTimeElem *TransformUpdateTimeStatElem = StatCollector::getGlobalElem(statTransformUpdateTime); //Start the transform update timing statistic if(TransformUpdateTimeStatElem) { TransformUpdateTimeStatElem->start(); } //update matrices updateWorld(getUpdateNode()); //Start the transform update timing statistic if(TransformUpdateTimeStatElem) { TransformUpdateTimeStatElem->stop(); } //Stop the physics timing statistic if(PhysicsTimeStatElem) { PhysicsTimeStatElem->stop(); } }
string Solver::convet2CaffeFormat() { string outStr = ""; string netStrStart = "net: \""; string netStrEnd = "\"\n"; string trainNetStrStart = "train_net: \""; string trainNetStrEnd = "\"\n"; string testNetStrStart = "test_net: \""; string testNetStrEnd = "\"\n"; string testIterStrStart = "test_iter: "; string testIterStrEnd = "\n"; string testIntervalStrStart = "test_interval: "; string testIntervalStrEnd = "\n"; string baseLrStrStart = "base_lr: "; string baseLrStrEnd = "\n"; string momentumStrStart = "momentum: "; string momentumStrEnd = "\n"; string weightDecayStrStart = "weight_decay: "; string weightDecayStrEnd = "\n"; string lrPolicyStrStart = "lr_policy: \""; string lrPolicyStrEnd = "\"\n"; string stepSizeStrStart = "stepsize: "; string stepSizeStrEnd = "\n"; string gammaStrStart = "gamma: "; string gammaStrEnd = "\n"; string powerStrStart = "power: "; string powerStrEnd = "\n"; string stepValueStrStart = "stepvalue: "; string stepValueStrEnd = "\n"; string displayStrStart = "display: "; string displayStrEnd = "\n"; string maxIterStrStart = "max_iter: "; string maxIterStrEnd = "\n"; string snapshotStrStart = "snapshot: "; string snapshotStrEnd = "\n"; string snapshotPrefixStrStart = "snapshot_prefix: \""; string snapshotPrefixStrEnd = "\"\n"; string typeStrStart = "type: \""; string typeStrEnd = "\"\n"; string solveModeStrStart = "solver_mode: "; string solveModeStrEnd= "\n"; if(getNet() != "") { outStr += netStrStart + getNet() + netStrEnd; } if(getTainNet() != "") { outStr += trainNetStrStart + getTainNet() + trainNetStrEnd; } if(getTestNet() != "") { outStr += testNetStrStart + getTestNet() + testNetStrEnd; } outStr += testIterStrStart + to_string(getTestIter()) + testIterStrEnd + testIntervalStrStart + to_string(getTestInterval()) + testIntervalStrEnd + baseLrStrStart + to_string(getBaseLr()) + baseLrStrEnd + momentumStrStart + to_string(getMomentum()) + momentumStrEnd + weightDecayStrStart + to_string(getWeightDecay()) + weightDecayStrEnd; switch(getLrPolicy()) { case LrPolicy::LRPOLICY_FIXED: { outStr += lrPolicyStrStart + "fixed" + lrPolicyStrEnd; break; } case LrPolicy::LRPOLICY_STEP: { outStr += lrPolicyStrStart + "step" + lrPolicyStrEnd + stepSizeStrStart + to_string(getStepSize()) + stepSizeStrEnd + gammaStrStart + to_string(getGamma()) + gammaStrEnd; break; } case LrPolicy::LRPOLICY_EXP: { outStr += lrPolicyStrStart + "exp" + lrPolicyStrEnd + gammaStrStart + to_string(getGamma()) + gammaStrEnd; break; } case LrPolicy::LRPOLICY_INV: { outStr += lrPolicyStrStart + "inv" + lrPolicyStrEnd + gammaStrStart + to_string(getGamma()) + gammaStrEnd + powerStrStart + to_string(getPower()) + powerStrEnd; break; } case LrPolicy::LRPOLICY_MULTISTEP: { outStr += lrPolicyStrStart + "multistep" + lrPolicyStrEnd; for(int i = 0 ; i < mParam->mStepValue.size(); i++) { outStr += stepValueStrStart + to_string(getStepValue(i)) + stepValueStrEnd; } outStr += gammaStrStart + to_string(getGamma()) + gammaStrEnd; break; } case LrPolicy::LRPOLICY_POLY: { outStr += lrPolicyStrStart + "poly" + lrPolicyStrEnd + powerStrStart + to_string(getPower()) + powerStrEnd; break; } case LrPolicy::LRPOLICY_SIGMOID: { outStr += lrPolicyStrStart + "sigmoid" + lrPolicyStrEnd + stepSizeStrStart + to_string(getStepSize()) + stepSizeStrEnd + gammaStrStart + to_string(getGamma()) + gammaStrEnd; break; } } outStr += displayStrStart + to_string(getDisplay()) + displayStrEnd + maxIterStrStart + to_string(getMaxIter()) + maxIterStrEnd + snapshotStrStart + to_string(getSnapshot()) + snapshotStrEnd; if(getSnapshotPrefix() != "") { outStr += snapshotPrefixStrStart + getSnapshotPrefix() + snapshotPrefixStrEnd; } switch(getType()) { case SolverType::SGD: { outStr += typeStrStart + "SGD" + typeStrEnd; break; } case SolverType::NESTEROV: { outStr += typeStrStart + "Nesterov" + typeStrEnd; break; } case SolverType::ADAGRAD: { outStr += typeStrStart + "AdaGrad" + typeStrEnd; break; } case SolverType::RMSPROP: { outStr += typeStrStart + "RMSProp" + typeStrEnd; break; } case SolverType::ADADELTA: { outStr += typeStrStart + "AdaDelta" + typeStrEnd; break; } case SolverType::ADAM: { outStr += typeStrStart + "Adam" + typeStrEnd; break; } } switch(getSolverMode()) { case SolverMode::CPU: { outStr += solveModeStrStart + "CPU" + solveModeStrEnd; break; } case SolverMode::GPU: { outStr += solveModeStrStart + "GPU" + solveModeStrEnd; break; } } return outStr; }