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;
}
