void poseCB(const geometry_msgs::PoseStamped& poseMsg)
{
if (!mocapOn)
{
std::cout << "Bebop pose is publishing. THE WALL IS UP!" << std::endl;
mocapOn = true;
}
tf::Vector3 desBodyLinVel, desBodyAngVel;
if (autonomy)
{
// Bebop pose
tf::Transform bebopPoseTF(tf::Quaternion(poseMsg.pose.orientation.x,poseMsg.pose.orientation.y,poseMsg.pose.orientation.z,poseMsg.pose.orientation.w),
tf::Vector3(poseMsg.pose.position.x,poseMsg.pose.position.y,poseMsg.pose.position.z));
// Target pose
tf::StampedTransform targetPoseTF;
try { tfl.lookupTransform("world",target,ros::Time(0),targetPoseTF); }
catch(tf::TransformException ex) {}
// Desired pose
tf::Vector3 desPos, desForward;
if (lazy)
{
tf::Vector3 unitRelPos = (bebopPoseTF.getOrigin() - targetPoseTF.getOrigin());
unitRelPos.setZ(0);
unitRelPos.normalize();
desPos = targetPoseTF.getOrigin() + radius*unitRelPos;
desPos.setZ(desPos.getZ() + 1);
desForward = -unitRelPos;
}
else
{
desPos = targetPoseTF.getOrigin();
desPos.setZ(desPos.getZ() + 1);
desForward = targetPoseTF.getBasis()*tf::Vector3(1,0,0);
}
// Desired velocity
tf::Vector3 desLinVel = kp*(desPos - bebopPoseTF.getOrigin()) + kpd*(targetLinVel - bebopLinVel);
tf::Vector3 desAngVel = kw*((bebopPoseTF.getBasis()*tf::Vector3(1,0,0)).cross(desForward));
desBodyLinVel = sat(bebopPoseTF.getBasis().inverse()*desLinVel,0.4) + tf::Vector3(0,0.4*joyAngVel.getZ(),0);
desBodyAngVel = sat(bebopPoseTF.getBasis().inverse()*desAngVel,0.4) + tf::Vector3(0,0,-0.5*joyAngVel.getZ());
}
else
{
desBodyLinVel = joyLinVel;
desBodyAngVel = joyAngVel;
}
// Publish
geometry_msgs::Twist twistMsg;
twistMsg.linear.x = desBodyLinVel.getX();
twistMsg.linear.y = desBodyLinVel.getY();
twistMsg.linear.z = desBodyLinVel.getZ();
twistMsg.angular.x = desBodyAngVel.getX();
twistMsg.angular.y = desBodyAngVel.getY();
twistMsg.angular.z = -1*desBodyAngVel.getZ();
velCmdPub.publish(twistMsg);
}
void Solver::bindNeckYaw(const double min_deg, const double max_deg)
{
double min_rad=sat(CTRL_DEG2RAD*min_deg,neckYawMin,neckYawMax);
double max_rad=sat(CTRL_DEG2RAD*max_deg,neckYawMin,neckYawMax);
mutex.lock();
(*chainNeck)(2).setMin(min_rad);
(*chainNeck)(2).setMax(max_rad);
mutex.unlock();
yInfo("neck yaw constrained in [%g,%g] deg",min_deg,max_deg);
}
void calculo_borda(Imagem* cop, Imagem* img, int i, int j) {
int acr, acb, acg;
int k;
int m;
int ii;
int jj;
int produto;
float filtro[3][3];
filtro[0][0] = -1;
filtro[0][1] = -1;
filtro[0][2] = -1;
filtro[1][0] = -1;
filtro[1][1] = 8;
filtro[1][2] = -1;
filtro[2][0] = -1;
filtro[2][1] = -1;
filtro[2][2] = -1;
acr = 0;
acg = 0;
acb = 0;
k = i - 1;
ii = 0;
while (k <= i + 1) {
m = j - 1;
jj = 0;
while (m <= j + 1) {
acr += img->pixels[k][m].r * filtro[ii][jj];
acg += img->pixels[k][m].g * filtro[ii][jj];
acb += img->pixels[k][m].b * filtro[ii][jj];
++m;
++jj;
}
++k;
++ii;
}
cop->pixels[i][j].r = sat(acr);
cop->pixels[i][j].g = sat(acg);
cop->pixels[i][j].b = sat(acb);
}
// HERE IS THE BIG DEAL: USE DESIRED AND ACTUAL STATE TO COMPUTE AND PUBLISH CMD_VEL
void SteeringController::lin_steering_algorithm() {
double controller_speed;
double controller_omega;
//Eigen::Vector2d pos_err_xy_vec_;
//Eigen::Vector2d t_vec; //tangent of desired path
//Eigen::Vector2d n_vec; //normal to desired path, pointing to the "left"
double tx = cos(des_state_phi_);
double ty = sin(des_state_phi_);
double nx = -ty;
double ny = tx;
double heading_err;
double lateral_err;
double trip_dist_err; // error is scheduling...are we ahead or behind?
// have access to: des_state_vel_, des_state_omega_, des_state_x_, des_state_y_, des_state_phi_ and corresponding odom values
double dx = des_state_x_- odom_x_;
double dy = des_state_y_ - odom_y_;
//pos_err_xy_vec_ = des_xy_vec_ - odom_xy_vec_; // vector pointing from odom x-y to desired x-y
//lateral_err = n_vec.dot(pos_err_xy_vec_); //signed scalar lateral offset error; if positive, then desired state is to the left of odom
lateral_err = dx*nx + dy*ny;
trip_dist_err = dx*tx + dy*ty;
//trip_dist_err = t_vec.dot(pos_err_xy_vec_); // progress error: if positive, then we are behind schedule
heading_err = min_dang(des_state_phi_ - odom_phi_); // if positive, should rotate +omega to align with desired heading
// DEBUG OUTPUT...
// ROS_INFO("des_state_phi, odom_phi, heading err = %f, %f, %f", des_state_phi_,odom_phi_,heading_err);
// ROS_INFO("lateral err, trip dist err = %f, %f",lateral_err,trip_dist_err);
// DEFINITELY COMMENT OUT ALL cout<< OPERATIONS FOR REAL-TIME CODE
//std::cout<<des_xy_vec_<<std::endl;
//std::cout<<odom_xy_vec_<<std::endl;
// let's put these in a message to publish, for rqt_plot to display
//steering_errs_.data.clear();
//steering_errs_.data.push_back(lateral_err);
//steering_errs_.data.push_back(heading_err);
//steering_errs_.data.push_back(trip_dist_err);
//steering_errs_publisher_.publish(steering_errs_); // suitable for plotting w/ rqt_plot
//END OF DEBUG STUFF
// do something clever with this information
controller_speed = des_state_vel_ + K_TRIP_DIST*trip_dist_err; //you call that clever ?!?!?!? should speed up/slow down to null out
//controller_omega = des_state_omega_; //ditto
controller_omega = des_state_omega_ + K_PHI*heading_err + K_DISP*lateral_err;
controller_omega = MAX_OMEGA*sat(controller_omega/MAX_OMEGA); // saturate omega command at specified limits
// send out our very clever speed/spin commands:
twist_cmd_.linear.x = controller_speed;
twist_cmd_.angular.z = controller_omega;
twist_cmd2_.twist = twist_cmd_; // copy the twist command into twist2 message
twist_cmd2_.header.stamp = ros::Time::now(); // look up the time and put it in the header
cmd_publisher_.publish(twist_cmd_);
cmd_publisher2_.publish(twist_cmd2_);
}
void Model::bf_psy()
{
int index = 0;
cout << "step " << index++ << endl;
NNC_Polyhedron base = base_cvx();
invar_cvx(com->init, base);
base.time_elapse_assign(com->init->time_elapse);
invar_cvx(com->init, base);
if (base.is_empty()) {
cout << "Empty initial states\n";
exit(0);
}
auto root = make_shared<pair_sc>(com->init->name, base, known_param_list,com->init->signature);
passing_map.find(root->signature)->second->push_back(root);
forward_a_step();
while(size(next_map) != 0) {
cout << " ------------------------------------ \n";
cout << "step " << index++ << endl;
sat();
forward_a_step();
cout << " ------------------------------------ \n";
}
cout << "The next is empty\n";
}
void test_Solver()
{
std::cout << "\n---- Testing Solver ----" << std::endl;
cnf::VariableSet v;
cnf::Formula unsat(&v), sat(&v);
cnf::Solver * s = new cnf::Solver("minisat", {});
v.add_subset("x", {3});
unsat.add_clauses({
cnf::Clause{ v.var("x", {1}) , v.var("x", {2})},
cnf::Clause{ v.var("x", {1}) ,cnf::no(v.var("x", {2}))},
cnf::Clause{cnf::no(v.var("x", {1})), v.var("x", {0})},
cnf::Clause{cnf::no(v.var("x", {1})),cnf::no(v.var("x", {0}))}
});
sat.add_clauses({
cnf::Clause{ v.var("x", {1}) , v.var("x", {2})},
cnf::Clause{ v.var("x", {1}) ,cnf::no(v.var("x", {2}))},
cnf::Clause{cnf::no(v.var("x", {1})), v.var("x", {0})}
});
if (!s->solve(unsat, &v))
std::cout << "UNSAT correctly identified" << std::endl;
else
std::cout << "Problem with UNSAT formula" << std::endl;
if (s->solve(sat, &v))
std::cout << "SAT correctly identified" << std::endl;
else
std::cout << "Problem with SAT formula" << std::endl;
}
Imagem* binarizacao_imagem(Imagem* padrao, int limiar) {
Imagem* copia = criar_imagem(padrao->width, padrao->height);
int i;
int j;
int esc_C, a=0;
for (i = 0; i < padrao->height; i++) {
for (j = 0; j < padrao->width;j++) {
esc_C = 0;
esc_C = esc_C + padrao->pixels[i][j].r;
esc_C = esc_C + padrao->pixels[i][j].g;
esc_C = esc_C + padrao->pixels[i][j].b;
esc_C = sat(esc_C/3);
if(esc_C > limiar){
a=000;
}else{
a=255;
}
copia->pixels[i][j].r = a;
copia->pixels[i][j].g = a;
copia->pixels[i][j].b = a;
}
}
return copia;
}
static void
fz_saturation_rgb(int *bdr, int *bdg, int *bdb, int sr, int sg, int sb)
{
int tr, tg, tb;
setsat(*bdr, *bdg, *bdb, sat(sr, sg, sb), &tr, &tg, &tb);
setlum(tr, tg, tb, lum(*bdr, *bdg, *bdb), bdr, bdg, bdb);
}
// Add ephemeris information from a Rinex3NavData object.
bool GloEphemerisStore::addEphemeris(const Rinex3NavData& data)
{
// If enabled, check SV health before entering here (health = 0 -> OK)
if( (data.health == 0) || (!onlyHealthy) )
{
// Get a GloEphemeris object from Rinex3NavData object
GloEphemeris gloEphem(data);
CommonTime t( data.time);
t.setTimeSystem(TimeSystem::GLO); // must be GLONASS time
SatID sat( data.sat );
pe[sat][t] = gloEphem; // find or add entry
if (t < initialTime)
initialTime = t;
else if (t > finalTime)
finalTime = t;
return true;
} // End of 'if( (data.health == 0) || (!onlyHealthy) )'
return false;
} // End of method 'GloEphemerisStore::addEphemeris()'
Collision BoxCircleCollider::collide(EntityW::EntitySp entity1, EntityW::EntitySp entity2)
{
auto transform1 = entity1->get<TransformComponent>();
auto transform2 = entity2->get<TransformComponent>();
auto collision1 = entity1->get<CollisionComponent>();
auto collision2 = entity2->get<CollisionComponent>();
auto collisionShape1 = std::static_pointer_cast<RectCollisionShape>( collision1->shape);
auto collisionShape2 = std::static_pointer_cast<CircleCollisionShape>(collision2->shape);
Vector2 circleCenter = transform2->position + collisionShape2->center();
std::vector<Vector2> vertices = collisionShape1->calculateVertices(transform1);
Vector2 axis;
float minDistance = 10000000000;
for (int i = 0; i < 4; i++)
{
Vector2 vertex = vertices[i];
Vector2 distanceVector = circleCenter - vertex;
float distance = fabs(glm::length(distanceVector));
if (distance < minDistance)
{
minDistance = distance;
axis = distanceVector;
}
}
axis = glm::normalize(axis);
return sat(entity1, entity2, { Vector2(1., 0.), Vector2(0., 1.), axis });
}
// Synchronize the CS flag of input GDS object with the
// SourceSatDataMap object
void GeneralEquations::synchronizeCSFlag( const SourceSatDataMap& dataMap,
gnssRinex& gRin )
{
SourceID source = gRin.header.source;
SourceSatDataMap::const_iterator it = dataMap.find(source);
if(it==dataMap.end()) return;
for(size_t i = 0; i< it->second.satellite.size(); i++)
{
SatID sat(it->second.satellite[i]);
double csValue = it->second.csflag[i]?1.0:0.0;
satTypeValueMap::iterator its = gRin.body.find(sat);
if(its!=gRin.body.end())
{
gRin.body[sat][TypeID::CSL1] = csValue;
gRin.body[sat][TypeID::CSL2] = csValue;
}
} // End of 'for(int i = 0...'
} // End of method 'GeneralEquations::synchronizeCSFlag()'
void FaceClassifier::nonZeroHueSaturationStatistics(cv::Mat face, FaceData &data) {
/* Calculates the average hue and saturation of the face's skin, ignoring
* all zeroed pixels while doing this. This is necessary because the mask
* we applied zeroed all non-face pixels.
*/
//split the HSV face image in its 3 channels
cv::vector<cv::Mat> faceChannels;
cv::split(face, faceChannels);
acc::accumulator_set< double, acc::stats<acc::tag::variance> > hue; //accumulates hue
acc::accumulator_set< double, acc::stats<acc::tag::variance> > sat; //accumulates saturation
acc::accumulator_set< double, acc::stats<acc::tag::variance> > val; //accumulates value
/*
acc::accumulator_set< double, acc::stats<acc::tag::variance> > blue; //accumulates blue
acc::accumulator_set< double, acc::stats<acc::tag::variance> > green; //accumulates green
acc::accumulator_set< double, acc::stats<acc::tag::variance> > red; //accumulates red
*/
double pixelAreaRatio = 0;
{
int validPixelCount = 0;
const int nRows = face.rows;
const int nCols = face.cols;
for( int i = 0; i < nRows; ++i) {
uchar* p_h = faceChannels[0].ptr<uchar>(i);
uchar* p_s = faceChannels[1].ptr<uchar>(i);
uchar* p_v = faceChannels[2].ptr<uchar>(i);
for ( int j = 0; j < nCols; ++j) {
if ( p_h[j] ) hue( p_h[j] );
if ( p_s[j] ) sat( p_s[j] );
if ( p_v[j] ) val( p_v[j] );
if ( p_h[j] ) validPixelCount++;
}
}
pixelAreaRatio = (double)validPixelCount / (double)(nRows * nCols);
}
//only if the face is "made of" non black pixels we have a face
if (pixelAreaRatio >= 0.5) {
data.skinHueMean = acc::mean(hue);
data.skinHueVariance = acc::variance(hue);
data.skinSaturationMean = acc::mean(sat);
data.skinSaturationVariance = acc::variance(sat);
data.skinValueMean = acc::mean(val);
data.skinValueVariance = acc::variance(val);
} else {
data.faceCount = 0;
data.skinHueMean = 0;
data.skinHueVariance = 0;
data.skinSaturationMean = 0;
data.skinSaturationVariance = 0;
data.skinValueMean = 0;
data.skinValueVariance = 0;
}
}
/* Function: mdlDerivatives */
static void mdlDerivatives(SimStruct *S)
{
double wn = ( mxGetPr(ssGetSFcnParam(S,0)) )[0];
double E = ( mxGetPr(ssGetSFcnParam(S,1)) )[0];
double In = ( mxGetPr(ssGetSFcnParam(S,2)) )[0];
double Kva = ( mxGetPr(ssGetSFcnParam(S,3)) )[0];
double Kvb = ( mxGetPr(ssGetSFcnParam(S,4)) )[0];
double KvlkA = ( mxGetPr(ssGetSFcnParam(S,5)) )[0];
double KvlkB = ( mxGetPr(ssGetSFcnParam(S,6)) )[0];
double ps = ( mxGetPr(ssGetSFcnParam(S,7)) )[0];
double pt = ( mxGetPr(ssGetSFcnParam(S,8)) )[0];
double up_dz = ( mxGetPr(ssGetSFcnParam(S,9)) )[0];
double lw_dz = ( mxGetPr(ssGetSFcnParam(S,10)) )[0];
double beta = ( mxGetPr(ssGetSFcnParam(S,11)) )[0];
double Va0 = ( mxGetPr(ssGetSFcnParam(S,12)) )[0];
double Vb0 = ( mxGetPr(ssGetSFcnParam(S,13)) )[0];
double Aa = ( mxGetPr(ssGetSFcnParam(S,14)) )[0];
double Ab = ( mxGetPr(ssGetSFcnParam(S,15)) )[0];
double L = ( mxGetPr(ssGetSFcnParam(S,16)) )[0];
double leak = ( mxGetPr(ssGetSFcnParam(S,17)) )[0];
real_T *dx = ssGetdX(S);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
dx[0]= x[1];
dx[1]= -wn*wn*x[0] - 2*E*wn*x[1] + wn*wn*sat(U(0),-In,In);
if ((sat(U(0),-In,In) >= up_dz) && (sat(U(0),-In,In) <= In))
{
dx[2] = (beta/(Va0 + Aa*U(1))) * (((Kva *(x[0]/In) + KvlkA) * sgn(ps-x[2]) * sqrt(abso(ps-x[2])) - KvlkA * sgn(x[2]-pt) * sqrt(abso(x[2]-pt))) - Aa*U(2)- leak*(x[2]-x[3]));
dx[3] = (beta/(Vb0 + Ab*(L - U(1)))) * (Ab*U(2) - ((Kvb *(x[0]/In) + KvlkB) * sgn(x[3]-pt) * sqrt(abso(x[3]-pt)) - KvlkB * sgn(ps-x[3]) * sqrt(abso(ps-x[3]))) + leak*(x[2]-x[3]));
}
else if ((sat(U(0),-In,In) >= -In) && (sat(U(0),-In,In) <= lw_dz))
{
dx[2] = (beta/(Va0 + Aa*U(1))) * ((-(Kva *(abso(x[0])/In) + KvlkA) * sgn(x[2]-pt) * sqrt(abso(x[2]-pt)) + KvlkA * sgn(ps-x[2]) * sqrt(abso(ps-x[2]))) - Aa*U(2)- leak*(x[2]-x[3]));
dx[3] = (beta/(Vb0 + Ab*(L - U(1)))) * (Ab*U(2) - (-(Kvb *(abso(x[0])/In) + KvlkB) * sgn(ps-x[3]) * sqrt(abso(ps-x[3])) + KvlkB * sgn(x[3]-pt) * sqrt(abso(x[3]-pt))) + leak*(x[2]-x[3]));
}
else
{
dx[2] = 0;
dx[3] = 0;
}
}
uint8_t DIA_getHue(hue *param, ADM_coreVideoFilter *in)
{
diaElemFloat hue(&(param->hue),QT_TRANSLATE_NOOP("hue","Hue :"),-180,180);
diaElemFloat sat(&(param->saturation),QT_TRANSLATE_NOOP("hue","Sat :"),-180,180);
diaElem *elems[]={&hue,&sat};
return diaFactoryRun("Hue",sizeof(elems)/sizeof(diaElem *),elems);
}
// update the satellite data due to the input GDS object
void GeneralEquations::updateSourceSatDataMap( const gnssDataMap& gdsMap )
{
SourceSatDataMap dataMap;
// Iterate through all items in the gnssDataMap
for( gnssDataMap::const_iterator it = gdsMap.begin();
it != gdsMap.end();
++it )
{
// Look for current SourceID
sourceDataMap::const_iterator sdmIter; //(it->second.find(source));
for( sdmIter=it->second.begin();
sdmIter!=it->second.end();
++sdmIter )
{
SourceID source(sdmIter->first);
SatData data;
// Iterate through corresponding 'satTypeValueMap'
satTypeValueMap::const_iterator stvmIter;
for( stvmIter = sdmIter->second.begin();
stvmIter != sdmIter->second.end();
++stvmIter )
{
SatID sat(stvmIter->first);
typeValueMap::const_iterator itt1 =
stvmIter->second.find(TypeID::elevation);
typeValueMap::const_iterator itt2 =
stvmIter->second.find(TypeID::CSL1);
if( (itt1==stvmIter->second.end()) ||
(itt2==stvmIter->second.end()) )
{
Exception e("Elevation was not found.");
GPSTK_THROW(e);
}
data.addData(sat, itt1->second,
(itt2->second!=0.0)?true:false, false);
} // End of 'for( satTypeValueMap::const_iterator ...'
dataMap[source] = data;
} // End of 'for( sdmIter=it->second.begin();...'
} // End of 'for( gnssDataMap::const_iterator it = ...'
sourceSatDataMap = dataMap;
} // End of method 'void GeneralEquations::updateSourceSatDataMap'
void test_flowsolver(const GI& g, const RI& r, double tol, int kind)
{
typedef typename GI::CellIterator CI;
typedef typename CI::FaceIterator FI;
typedef double (*SolutionFuncPtr)(const Vec&);
//typedef Dune::BasicBoundaryConditions<true, false> FBC;
typedef Dune::FunctionBoundaryConditions<SolutionFuncPtr> FBC;
typedef Dune::IncompFlowSolverHybrid<GI, RI, FBC,
Dune::MimeticIPEvaluator> FlowSolver;
FlowSolver solver;
// FBC flow_bc;
// assign_bc(g, flow_bc);
FBC flow_bc(&u);
typename CI::Vector gravity(0.0);
std::cout << "========== Init pressure solver =============" << std::endl;
Dune::time::StopWatch rolex;
rolex.start();
solver.init(g, r, gravity, flow_bc);
rolex.stop();
std::cout << "========== Time in seconds: " << rolex.secsSinceStart() << " =============" << std::endl;
std::vector<double> src(g.numberOfCells(), 0.0);
assign_src(g, src);
std::vector<double> sat(g.numberOfCells(), 0.0);
std::cout << "========== Starting pressure solve =============" << std::endl;
rolex.start();
solver.solve(r, sat, flow_bc, src, tol, 3, kind);
rolex.stop();
std::cout << "========== Time in seconds: " << rolex.secsSinceStart() << " =============" << std::endl;
typedef typename FlowSolver::SolutionType FlowSolution;
FlowSolution soln = solver.getSolution();
std::vector<typename GI::Vector> cell_velocity;
estimateCellVelocity(cell_velocity, g, solver.getSolution());
// Dune's vtk writer wants multi-component data to be flattened.
std::vector<double> cell_velocity_flat(&*cell_velocity.front().begin(),
&*cell_velocity.back().end());
std::vector<double> cell_pressure;
getCellPressure(cell_pressure, g, soln);
compare_pressure(g, cell_pressure);
Dune::VTKWriter<typename GI::GridType::LeafGridView> vtkwriter(g.grid().leafView());
vtkwriter.addCellData(cell_velocity_flat, "velocity", GI::GridType::dimension);
vtkwriter.addCellData(cell_pressure, "pressure");
vtkwriter.write("testsolution-" + boost::lexical_cast<std::string>(0),
Dune::VTKOptions::ascii);
}
SEXP do_sat(SEXP sx1, SEXP sy1, SEXP sx2, SEXP sy2) {
polygon_t p1, p2;
p1.n = LENGTH(sx1);
p1.x = REAL(sx1);
p1.y = REAL(sy1);
p2.n = LENGTH(sx2);
p2.x = REAL(sx2);
p2.y = REAL(sy2);
return ScalarLogical(sat(p1, p2));
}
la::Mat<double> & update(const la::Mat<double> & x, la::Mat<double> & dx_, double cost, int iterationCount)
{
// get latest gradient
const la::Mat<double> & new_grad = func->gradient();
// Loop to set step
for( int i = 0; i < new_grad.size().rows; i++ ){
double s = new_grad[i]*prev_grad[i];
if( s > 0 ){ // when gradients are same sign
alpha[i] = eta_plus*alpha[i];
prev_grad[i] = new_grad[i];
dx[i] = -sign(new_grad[i])*sat(alpha[i]);
}else if( s < 0 ){ // when gradient changes sign
alpha[i] = eta_minus*alpha[i];
if( cost > lcost ){ // if current cost larger than last
// go back to old spot
dx[i] = -dx[i];
prev_grad[i] = 0;
}else{ // else do normal update
dx[i] = -sign(new_grad[i])*sat(alpha[i]);
prev_grad[i] = new_grad[i];
}
}else{ // when gradient product is 0
prev_grad[i] = new_grad[i];
dx[i] = -sign(new_grad[i])*sat(alpha[i]);
}
}
// update x
xn = x + dx;
dx_ = dx;
// do swapping/updating for internal data
prev_grad = new_grad;
lcost = cost;
// return update
return xn;
}
inline typename UpscalerBase<Traits>::permtensor_t
UpscalerBase<Traits>::upscaleEffectivePerm(const FluidInterface& fluid)
{
int num_cells = ginterf_.numberOfCells();
// No source or sink.
std::vector<double> src(num_cells, 0.0);
// Just water.
std::vector<double> sat(num_cells, 1.0);
// Gravity.
Dune::FieldVector<double, 3> gravity(0.0);
// gravity[2] = -Dune::unit::gravity;
permtensor_t upscaled_K(3, 3, (double*)0);
for (int pdd = 0; pdd < Dimension; ++pdd) {
setupUpscalingConditions(ginterf_, bctype_, pdd, 1.0, 1.0, twodim_hack_, bcond_);
if (pdd == 0) {
// Only on first iteration, since we do not change the
// structure of the system, the way the flow solver is
// implemented.
flow_solver_.init(ginterf_, res_prop_, gravity, bcond_);
}
// Run pressure solver.
bool same_matrix = (bctype_ != Fixed) && (pdd != 0);
flow_solver_.solve(fluid, sat, bcond_, src, residual_tolerance_,
linsolver_verbosity_,
linsolver_type_, same_matrix,
linsolver_maxit_, linsolver_prolongate_factor_,
linsolver_smooth_steps_);
double max_mod = flow_solver_.postProcessFluxes();
std::cout << "Max mod = " << max_mod << std::endl;
// Compute upscaled K.
double Q[Dimension] = { 0 };
switch (bctype_) {
case Fixed:
Q[pdd] = computeAverageVelocity(flow_solver_.getSolution(), pdd, pdd);
break;
case Linear:
case Periodic:
for (int i = 0; i < Dimension; ++i) {
Q[i] = computeAverageVelocity(flow_solver_.getSolution(), i, pdd);
}
break;
default:
OPM_THROW(std::runtime_error, "Unknown boundary type: " << bctype_);
}
double delta = computeDelta(pdd);
for (int i = 0; i < Dimension; ++i) {
upscaled_K(i, pdd) = Q[i] * delta;
}
}
return upscaled_K;
}
/* Edit the dataset, removing data outside the indicated time interval
*
* @param[in] tmin Defines the beginning of the time interval
* @param[in] tmax Defines the end of the time interval
*/
void GloEphemerisStore::edit( const CommonTime& tmin,
const CommonTime& tmax )
{
// Create a working copy
GloEphMap bak;
// Reset the initial and final times
initialTime = CommonTime::END_OF_TIME;
finalTime = CommonTime::BEGINNING_OF_TIME;
// Iterate through all items in the 'bak' GloEphMap
for( GloEphMap::const_iterator it = pe.begin();
it != pe.end();
++it )
{
// Then, iterate through corresponding 'TimeGloMap'
for( TimeGloMap::const_iterator tgmIter = (*it).second.begin();
tgmIter != (*it).second.end();
++tgmIter )
{
CommonTime t( (*tgmIter).first );
// Check if the current record is within the given time interval
if( ( tmin <= t ) && ( t <= tmax ) )
{
// If we are within the proper boundaries, let's add the data
GloEphemeris data( (*tgmIter).second );
SatID sat( (*it).first );
bak[sat][t] = data; // Add entry
// Update 'initialTime' and 'finalTime', if necessary
if (t < initialTime)
initialTime = t;
else if (t > finalTime)
finalTime = t;
} // End of 'if ( ( (*tgmIter).first >= tmin ) && ...'
} // End of 'for( TimeGloMap::const_iterator tgmIter = ...'
} // End of 'for( GloEphMap::const_iterator it = pe.begin(); ...'
// Update the data map before returning
pe = bak;
return;
}; // End of method 'GloEphemerisStore::edit()'
void PSatD_dStep(PIDBase* self, float Ts, float derivative)
{
//Proportional term
self->internalState = sat(self->Kp*(self->desired - self->state),
-self->outputLimit, self->outputLimit);
//Derivative term
self->internalState += self->Kd*derivative;
//Set final output
self->output = self->internalState;
self->lastError = (self->desired - self->state);
}
inline void export_templates(){
sizeof( boost::posix_time::time_period );
sizeof( boost::gregorian::date_period );
sizeof( boost::gregorian::weeks );
sizeof( boost::posix_time::milliseconds );
sizeof( boost::posix_time::microseconds );
//In order to get nanoseconds boost::date_time should be compiled with BOOST_DATE_TIME_HAS_NANOSECONDS
//sizeof( boost::posix_time::nanoseconds );
sizeof( boost::gregorian::day_clock );
sizeof( boost::gregorian::gregorian_calendar::day_of_year_type(1) );
sizeof( boost::gregorian::greg_weekday_rep(1) );
sizeof( boost::gregorian::greg_day_rep(1) );
sizeof( boost::gregorian::greg_month_rep(1) );
sizeof( boost::gregorian::greg_year_rep(1) );
sizeof( boost::date_time::year_based_generator<boost::gregorian::date> );
sizeof( boost::date_time::partial_date<boost::gregorian::date> );
sizeof( boost::date_time::nth_kday_of_month<boost::gregorian::date> );
sizeof( boost::date_time::first_kday_of_month<boost::gregorian::date> );
sizeof( boost::date_time::last_kday_of_month<boost::gregorian::date> );
sizeof( boost::date_time::first_kday_after<boost::gregorian::date> );
sizeof( boost::date_time::first_kday_before<boost::gregorian::date> );
sizeof( boost::date_time::time_zone_names_base<char> );
//exporting 4 functions
boost::gregorian::date sunday(2003, boost::date_time::Feb,2);
boost::gregorian::greg_weekday sat(boost::date_time::Saturday);
boost::gregorian::days_until_weekday(sunday, sat);
boost::gregorian::days_before_weekday(sunday, sat);
boost::gregorian::next_weekday(sunday, sat);
boost::gregorian::previous_weekday(sunday, sat);
//exporting static function
boost::date_time::int_adapter<int>::from_special( boost::date_time::not_a_date_time );
boost::date_time::int_adapter<long int>::from_special( boost::date_time::not_a_date_time );
boost::date_time::int_adapter<long unsigned int>::from_special( boost::date_time::not_a_date_time );
sizeof( boost::local_time::first_last_dst_rule );
sizeof( boost::local_time::last_last_dst_rule );
sizeof( boost::local_time::nth_day_of_the_week_in_month_dst_rule );
sizeof( boost::local_time::posix_time_zone );
sizeof( boost::local_time::tz_database );
sizeof( boost::local_time::partial_date_dst_rule );
sizeof( boost::local_time::nth_last_dst_rule );
sizeof( boost::local_time::nth_day_of_the_week_in_month_dst_rule );
sizeof( boost::local_time::local_time_period );
sizeof( boost::local_time::local_sec_clock );
sizeof( boost::local_time::local_microsec_clock );
sizeof( boost::posix_time::second_clock );
sizeof( boost::posix_time::microsec_clock );
sizeof( boost::posix_time::no_dst );
sizeof( boost::posix_time::us_dst );
}
void Controller::doSaccade(const Vector &ang, const Vector &vel)
{
LockGuard guard(mutexCtrl);
if (ctrlInhibited)
return;
setJointsCtrlMode();
// enforce joints bounds
Vector ang_(3);
ang_[0]=CTRL_RAD2DEG*sat(ang[0],lim(eyesJoints[0],0),lim(eyesJoints[0],1));
ang_[1]=CTRL_RAD2DEG*sat(ang[1],lim(eyesJoints[1],0),lim(eyesJoints[1],1));
ang_[2]=CTRL_RAD2DEG*sat(ang[2],lim(eyesJoints[2],0),lim(eyesJoints[2],1));
posHead->setRefSpeeds(eyesJoints.size(),eyesJoints.getFirst(),vel.data());
posHead->positionMove(eyesJoints.size(),eyesJoints.getFirst(),ang_.data());
if (commData->debugInfoEnabled && (port_debug.getOutputCount()>0))
{
Bottle info;
for (size_t i=0; i<ang_.length(); i++)
{
ostringstream ss;
ss<<"pos_"<<eyesJoints[i];
info.addString(ss.str().c_str());
info.addDouble(ang_[i]);
}
port_debug.prepare()=info;
txInfo_debug.update(q_stamp);
port_debug.setEnvelope(txInfo_debug);
port_debug.writeStrict();
}
saccadeStartTime=Time::now();
commData->saccadeUnderway=true;
unplugCtrlEyes=true;
notifyEvent("saccade-onset");
}
void PID_UpdateCentroid(PID * pid)
{
int P, I, D;
float refreshRate;
//------Read Encoder and clock
uint32 nowClocks = ClockTime();
//------Time since last function call in seconds
refreshRate = refresh_rate(nowClocks, pid->lastClockTicks_c);
pid->lastClockTicks_c = nowClocks;
//------Calculate Error
pid->error_c = pid->desiredCentroidPID - pid->currentCentroid;
///Wireless_ControlLog(pid->currentCentroid, pid->desiredCentroidPID);
//------Update Derivative
//pid->differentiator_c = ((((2*pid->Tau)-refreshRate)/((2*pid->Tau)+refreshRate))*pid->differentiator_c) + ((2/((2*pid->Tau)+refreshRate))*(pid->currentCentroid - pid->lastCurrentCentroid));
pid->differentiator_c = ((((2*pid->Tau)-refreshRate)/((2*pid->Tau)+refreshRate))*pid->differentiator_c) + ((2/((2*pid->Tau)+refreshRate))*(pid->error_c - pid->lastError_c));
//------Update integrator - AntiWindup(only use the integrator if we are close, but not too close)
if( abs(pid->error_c) < 75)
pid->integrator_c = pid->integrator_c + ((refreshRate/2)*(pid->error_c + pid->lastError_c))*abs(pid->currentVelocity)/1500;
else
pid->integrator_c = 0;
//------Scale Kp based on current velocity
float Kp = pid->Kp_c * 1000 / abs(pid->currentVelocity);
//------Output Calculation
P = Kp * pid->error_c;
I = pid->Ki_c * pid->integrator_c;
D = pid->Kd_c * pid->differentiator_c;
pid->outputPID_unsat_c = (P) + (I) - (D);
pid->outputPID_c = sat(pid->outputPID_unsat_c, 40);
//pid->integrator_c = pid->integrator_c + (refreshRate/pid->Ki_c)*(pid->outputPID_c - pid->outputPID_unsat_c);
//------Save states and send PWM to motors
pid->lastCurrentCentroid = pid->currentCentroid;
pid->lastError_c = pid->error_c;
//------Condition output on current velocity
if (pid->currentVelocity < 0)
pid->outputPID_c = -pid->outputPID_c;
if (pid->currentVelocity > -20 && pid->currentVelocity < 20)
pid->outputPID_c = 0;
if (pid->outputPID_c == 0)
pid->outputPID_c = 0;
SetServo(RC_STR_SERVO, pid->outputPID_c);
}
std::ostream& RinexObsID::dumpCheck(std::ostream& s) throw(Exception)
{
try {
const std::string types("CLDS");
std::map<char,std::string>::const_iterator it;
for(size_t i=0; i<ObsID::validRinexSystems.size(); i++) {
char csys = ObsID::validRinexSystems[i];
std::string sys = ObsID::validRinexSystems.substr(i,1);
RinexSatID sat(sys);
std::string system(sat.systemString());
s << "System " << sys << " = " << system << ", frequencies ";
for(it = ObsID::validRinexTrackingCodes[sys[0]].begin();
it != ObsID::validRinexTrackingCodes[sys[0]].end(); ++it)
s << it->first;
s << std::endl;
for(it = ObsID::validRinexTrackingCodes[sys[0]].begin();
it != ObsID::validRinexTrackingCodes[sys[0]].end(); ++it)
{
s << " " << system << "(" << sys << "), freq " << it->first
<< ", codes '" << it->second << "'" << std::endl;
std::string codes(it->second), str;
for(size_t j=0; j<codes.size(); ++j) {
std::ostringstream oss1;
for(size_t k=0; k<types.size(); ++k) {
str = std::string(1,types[k]) + std::string(1,it->first)
+ std::string(1,codes[j]);
std::ostringstream oss;
if(!isValidRinexObsID(str,csys))
oss << str << " " << "-INVALID-";
else {
RinexObsID robsid(sys+str);
oss << str << " " << robsid;
}
oss1 << " " << StringUtils::leftJustify(oss.str(),34);
}
s << StringUtils::stripTrailing(oss1.str()) << std::endl;
}
}
}
}
catch(Exception& e) {
s << "Exception: " << e.what() << std::endl;
GPSTK_RETHROW(e);
}
return s;
}
// Synchronize the CS flag of input GDS object with the
// SourceSatDataMap object
void GeneralEquations::synchronizeCSFlag( const SourceSatDataMap& dataMap,
gnssDataMap& gdsMap )
{
// Iterate through the gnssDatamap
for( gnssDataMap::iterator it = gdsMap.begin();
it != gdsMap.end();
++it )
{
// Look for current SourceID
for( sourceDataMap::iterator sdmIter = it->second.begin();
sdmIter != it->second.end();
++sdmIter)
{
SourceID source(sdmIter->first);
// Iterate through corresponding 'satTypeValueMap'
for( satTypeValueMap::iterator stvmIter = sdmIter->second.begin();
stvmIter != sdmIter->second.end();
++stvmIter )
{
SatID sat(stvmIter->first);
SourceSatDataMap::const_iterator its = dataMap.find(source);
if( its!=dataMap.end() )
{
int index = its->second.indexOfSat(sat);
if( index>=0 )
{
double csValue = its->second.csflag[index]?1.0:0.0;
stvmIter->second[TypeID::CSL1] = csValue;
stvmIter->second[TypeID::CSL2] = csValue;
} // End of 'if( index>=0 )'
} // End of 'if( its!=dataMap.end() )'
} // End of 'for( satTypeValueMap::const_iterator ...'
} // End of 'for(sourceDataMap::iterator '
} // End of 'for( gnssDataMap::const_iterator it = ...'
} // End of method 'GeneralEquations::synchronizeCSFlag()'
SEXP sat_match(SEXP sItems, SEXP sTable) {
int ni = LENGTH(sItems), nt = LENGTH(sTable), i, j;
SEXP res = allocVector(INTSXP, ni);
int *mid = INTEGER(res);
for (i = 0; i < ni; i++) {
polygon_t p = list2polygon(VECTOR_ELT(sItems, i));
for (j = 0; j < nt; j++) {
polygon_t pt = list2polygon(VECTOR_ELT(sTable, j));
if (sat(p, pt)) break;
}
mid[i] = (j < nt) ? (j + 1) : NA_INTEGER;
}
return res;
}
/// <summary>
/// A function which determines weither the player and a powerup collided.
/// </summary>
/// <param name="player">The player.</param>
/// <returns>Bool value which determines a pickup or not</returns>
bool Powerup::hitDetection(std::shared_ptr<Player>& player)
{
// CHeck hit detection
bool retVal = sat(this->sprite, player->sprite);
// Check if there was a hit collision
if (retVal)
{
// Set status to deleted
resourceHandler->getSound(ResourceHandler::Sound::FX_PICKUP_HEALTH).play();
setDeleted(true);
}
return retVal;
}
void readControl(Istream& is, std::vector<double>& saturations, Opm::SparseTable<double>& all_pdrops)
{
int num_sats;
is >> num_sats;
std::vector<double> sat(num_sats);
Opm::SparseTable<double> ap;
std::vector<double> tmp_pd;
for (int i = 0; i < num_sats; ++i) {
is >> sat[i];
int num_pdrops;
is >> num_pdrops;
tmp_pd.resize(num_pdrops);
for (int j = 0; j < num_pdrops; ++j) {
is >> tmp_pd[j];
}
ap.appendRow(tmp_pd.begin(), tmp_pd.end());
}
all_pdrops.swap(ap);
saturations.swap(sat);
}
void PIDFF_dwffStep(PIDBase* self, float Ts, float error, float ff, float ds)
{
//Perform windup test if automatic mode is enabled.
if (self->autoWindup == 1)
{
self->windup = (self->internalState > self->output) && (error>0);
self->windup = (self->windup) ||
((self->internalState < self->output) && (error<0));
}
else
{
//Experimental
//self->windup = ((self->windup >0) && (error>0)) ||
// ((self->windup <0) && (error<0));
}
//Proportional term
self->internalState += self->Kp*(error-self->lastError);
//Integral term
//Disabled if windup is in progress.
if (!self->windup) self->internalState += self->Ki*Ts*error;
//Derivative
self->internalState += self->Kd*(self->lastDerivative - ds);
//Feed forward term
self->internalState += ff - self->lastFF;
//Set final output
self->output = self->internalState;
if (self->autoWindup == 1)
{
self->output = sat(self->output,-self->outputLimit, self->outputLimit);
}
self->lastDerivative = ds;
self->llastError = self->lastError;
self->lastError = error;
self->lastRef = self->desired;
self->lastFF = ff;
}