/* public static */
void
DistanceToPoint::computeDistance(const geom::Geometry& geom,
const geom::Coordinate& pt,
PointPairDistance& ptDist)
{
if ( const LineString* ls = dynamic_cast<const LineString*>(&geom) )
{
computeDistance(*ls, pt, ptDist);
}
else if ( const Polygon* pl = dynamic_cast<const Polygon*>(&geom) )
{
computeDistance(*pl, pt, ptDist);
}
else if ( const GeometryCollection* gc = dynamic_cast<const GeometryCollection*>(&geom) )
{
for (size_t i = 0; i < gc->getNumGeometries(); i++)
{
const Geometry* g = gc->getGeometryN(i);
computeDistance(*g, pt, ptDist);
}
}
else
{
// assume geom is Point
ptDist.setMinimum(*(geom.getCoordinate()), pt);
}
}
inline virtual double operator()(const RenderItem * r1, const RenderItem * r2) const {
if (supported(r1, r2))
return computeDistance(dynamic_cast<const R1*>(r1), dynamic_cast<const R2*>(r2));
else if (supported(r2,r1))
return computeDistance(dynamic_cast<const R1*>(r2), dynamic_cast<const R2*>(r1));
else
return NOT_COMPARABLE_VALUE;
}
/* public static */
void
DistanceToPoint::computeDistance(const geom::Polygon& poly,
const geom::Coordinate& pt,
PointPairDistance& ptDist)
{
computeDistance(*(poly.getExteriorRing()), pt, ptDist);
for (size_t i=0, n=poly.getNumInteriorRing(); i<n; ++i)
{
computeDistance(*(poly.getInteriorRingN(i)), pt, ptDist);
}
}
void countANCodingUndetectableErrors(uintll_t n, uintll_t A, uint128_t* counts, uintll_t count_counts)
{
double shardsDone = 0.0;
#pragma omp parallel
{
const uintll_t CNT_MESSAGES = 0x1ull << n;
const uintll_t CNT_SLICES = CNT_MESSAGES / SZ_SHARDS;
const uintll_t CNT_SHARDS = CNT_SLICES * CNT_SLICES;
uintll_t* counts_local = new uintll_t[count_counts];
memset(counts_local, 0, count_counts*sizeof(uintll_t));
#pragma omp for schedule(dynamic,1)
for (uintll_t shardX = 0; shardX < CNT_MESSAGES; shardX += SZ_SHARDS)
{
// 1) Triangle for main diagonale
uintll_t m1, m2;
for (uintll_t x = 0; x < SZ_SHARDS; ++x) {
m1 = (shardX + x) * A;
++counts_local[computeDistance(m1, m1)];
for (uintll_t y = (x + 1); y < SZ_SHARDS; ++y) {
m2 = (shardX + y) * A;
counts_local[computeDistance(m1, m2)]+=2;
}
}
// 2) Remainder of the slice
for (uintll_t shardY = shardX + SZ_SHARDS; shardY < CNT_MESSAGES; shardY += SZ_SHARDS) {
for (uintll_t x = 0; x < SZ_SHARDS; ++x) {
m1 = (shardX + x) * A;
for (uintll_t y = 0; y < SZ_SHARDS; ++y) {
m2 = (shardY + y) * A;
counts_local[computeDistance(m1, m2)]+=2;
}
}
}
uintll_t shardsComputed = CNT_SLICES - (shardX / SZ_SHARDS);
float inc = static_cast<double>(shardsComputed * 2 - 1) / CNT_SHARDS * 100;
#pragma omp atomic
shardsDone += inc;
} // for
// 3) Sum the counts
for (uintll_t i = 0; i < count_counts; ++i) {
#pragma omp atomic
counts[i] += counts_local[i];
}
delete[] counts_local;
} // parallel
}
// add a obstacle square to the map, marking its distance in the relevant heading bins
void FovObstacleMap::putObstacle(GridSquare pt)
{
if(!inFieldOfView(pt)) {
//std::cerr << "out of FOV: d = " << computeDistance(pt) << ", range = (" << minRange << ", " << maxRange << "), hdiff = " << computeHeadingDiff(computeHeading(pt), robotHeading) << ", fov/2 = " << fov/2.0 << std::endl;
return; // we don't want to consider this point
}
CellData& cellData = map.cell(pt.x, pt.y);
cellData.isKnownObstacle = true;
Length distance = computeDistance(pt);
// if the difference between max and min is greater than pi, the cell lies
// across the discontinuity in heading so array access must be separate
Length *minObstacleDistances;
int N;
if(cellData.maxHeading - cellData.minHeading > wykobi::PI)
{
N = closestObstacleByHeading.cells(-wykobi::PI, cellData.minHeading, minObstacleDistances);
setMinObstacleDistance(distance, minObstacleDistances, N);
N = closestObstacleByHeading.cells(cellData.minHeading, wykobi::PI, minObstacleDistances);
setMinObstacleDistance(distance, minObstacleDistances, N);
}
else
{
N = closestObstacleByHeading.cells(cellData.minHeading, cellData.maxHeading, minObstacleDistances);
setMinObstacleDistance(distance, minObstacleDistances, N);
}
}
float Action::reachGrasp(geometry_msgs::Pose pose_target, const std::string surface_name)
{
if (!reachPregrasp(pose_target, surface_name))
return std::numeric_limits<float>::max();
float dist = computeDistance(move_group_->getCurrentPose().pose, move_group_->getPoseTarget().pose);
ROS_INFO_STREAM("Reaching distance to the target = " << dist);
/*moveit_msgs::PickupGoal goal;
collision_detection::AllowedCollisionMatrixPtr approach_grasp_acm(new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix()));
std::cout << "attach_object_msg.link_name " << attach_object_msg.link_name << std::endl;
std::cout << "attach_object_msg.object.id " << attach_object_msg.object.id << std::endl;
std::cout << "attach_object_msg.object.operation " << attach_object_msg.object.operation << std::endl;
std::cout << "attach_object_msg.touch_links.size() " << attach_object_msg.touch_links.size() << std::endl;
// we are allowed to touch the target object
approach_grasp_acm->setEntry(goal.target_name, attach_object_msg.touch_links, true);*/
if (dist < 0.2)
//if (dist > grasp_data_.approach_retreat_min_dist_)
posture.poseHandClose(end_eff);
return dist;
}
void findBestNMatches( uint nBest,
const cv::Mat &descriptor,
const cv::Mat &candidateDescriptors,
const uchar *mask,
std::list<cv::DMatch> &bestNMatches )
{
bestNMatches.clear();
double minDistance = -1.0L;
for (int i = 0; i < candidateDescriptors.rows; ++i)
{
if (!mask || (mask && mask[i]))
{
double distance = computeDistance(descriptor, candidateDescriptors, i);
if (distance < minDistance || bestNMatches.size() < 2)
{
minDistance = minDistance < 0 ? distance : MIN(minDistance, distance);
bestNMatches.push_front(cv::DMatch(0, i, static_cast<float>(distance)));
if (bestNMatches.size() > nBest)
{
bestNMatches.pop_back();
}
}
}
}
}
/**
* CCamera::setCamera
* date Modified March 13, 2006
*/
void CCamera::setCamera(const D3DXVECTOR3 &pos, const D3DXVECTOR3 &targ, D3DXVECTOR3 &vPTwoPos)
{
// do this
if(vPTwoPos == D3DXVECTOR3(0,0,0))
vPTwoPos = targ;
// store the position of the camera
m_Position = pos;
// store the target position of the camera
m_Target = (targ+vPTwoPos)*0.5f;
// store the vector from the target to the position
m_TargToPos = m_Position - m_Target;
D3DXVec3Normalize(&m_UnitTargPos, &(m_Target - m_Position));
// set the up vector based on the world's up
m_UpVector = D3DXVECTOR3(0,1,0);
// compute the at and right of the camera
m_AtVector = m_Target - m_Position;
m_AtVector.y = 0;
D3DXVec3Normalize(NULL, &m_AtVector, &m_AtVector);
D3DXVec3Cross(&m_RightVector, &m_UpVector, &m_AtVector);
D3DXVec3Normalize(NULL, &m_RightVector, &m_RightVector);
// set the initial rotation of the camera
D3DXVECTOR3 vec = -m_AtVector;
m_fRotation = acosf(D3DXVec3Dot(&vec, &D3DXVECTOR3(0,0,1)));
if(D3DXVec3Dot(&vec, &D3DXVECTOR3(1,0,0)) < 0) m_fRotation = -m_fRotation;
if (targ == vPTwoPos)
m_fInitDist = 0.0f;
else
// compute the initial distance between the characters
m_fInitDist = computeDistance(targ, vPTwoPos);
}
void kmeans::startClustering() {
kcenter = new double[k];
for(int i = 0; i < k; i++) kcenter[i] = 0;
center = new average[k];
for(int i = 0; i < k; i++) {
center[i].setx(0);
center[i].sety(0);
}
imageArray = new int*[numRow];
for(int i = 0; i < numRow; ++i) {
imageArray[i] = new int[numCol];
}
for(int i = 0; i < numRow; i++) {
for(int j = 0; j < numCol; j++) {
imageArray[i][j] = 0;
}
}
labelChecker = true;
while (labelChecker) {
labelChecker = false;
computeCenter();
computeDistance();
}
listHead.putonImage(imageArray);
}
//--------------------------------------------- IS IN WEAPON RANGE -----------------------------------------
bool UnitImpl::isInWeaponRange(Unit *target) const
{
// Preliminary checks
if ( !exists() || !target || !target->exists() || this == target )
return false;
// Store the types as locals
UnitType thisType = getType();
UnitType targType = target->getType();
// Obtain the weapon type
WeaponType wpn = thisType.groundWeapon();
if ( targType.isFlyer() || target->isLifted() )
wpn = thisType.airWeapon();
// Return if there is no weapon type
if ( wpn == WeaponTypes::None || wpn == WeaponTypes::Unknown )
return false;
// Retrieve the min and max weapon ranges
int minRange = wpn.minRange();
int maxRange = getPlayer()->weaponMaxRange(wpn);
// Check if the distance to the unit is within the weapon range
int distance = computeDistance(this, target);
return (minRange ? minRange < distance : true) && maxRange >= distance;
}
void Shooter::run () {
while(true) {
computeTurn();
// for the speed of the shooter
if(getoShooterSpeed != -1)
pid->SetSetpoint(getoShooterSpeed/-60.);
else {
//pid->SetSetpoint(0);
pid->SetSetpoint(computeSpeed(computeDistance())/-60.);
//printf("encoder %f %f\n", encoder.GetRate()*60, distance.GetVoltage()/.0098);
}
cout << encoder.GetRate() << endl;
/*double intPart;
turret->Set(modf(TurretLocation, &intPart));
if(LimitTurret.Get() == 1) {
// I am rezeroing the turret because it might slip on the turning and this will reset it as it should be
turret->EnableControl(0);
// I think that this has to get reset to the jaguar after it is enabled
turret->ConfigEncoderCodesPerRev((int)(500 * config->GetValue("turretMotorTurns")));
turret->SetPID(config->GetValue("turretP"), config->GetValue("turretI"), config->GetValue("turretD"));
}*/
Wait(.05);
}
}
static inline float minusSimilarity(
const int distancefunction,
const Mat& points1, int idx1,
const Mat& points2, int idx2)
{
return -computeDistance(distancefunction, points1, idx1, points2, idx2);
}
Math::Matrix
AAKR::estimate(Math::Matrix query, double variance)
{
Math::Matrix mean;
Math::Matrix std;
normalize(mean, std);
// Use normalized query vector.
computeDistance((query - mean) / std);
computeWeights(variance);
double s = sum(m_weights);
// Avoid division by zero.
if (!s)
return query;
// Combine with weights.
Math::Matrix result = ((transpose(m_weights) * m_norm) / s);
// Normalize.
for (unsigned i = 0; i < sampleSize(); i++)
result(i) = result(i) * std(i) + mean(i);
return result;
}
/**
* CCamera::updateCameraMP
* date Modified March 13, 2006
*/
void CCamera::updateCameraMP(const D3DXVECTOR3 &one, const D3DXVECTOR3 &two)
{
// check whether to update or not
if(!m_bUpdateMP)
return;
// set the target to the point b/w the two players'
D3DXVec3Scale(&m_Target, &(one + two), 0.5f);
// compute the distance b/w the characters
float fDist = computeDistance(one, two);
// move the camera forward/back based on the change in distance b/w characters
m_Position = m_Target + m_TargToPos;
m_Position += m_UnitTargPos * ((m_fInitDist - fDist)/m_fInitDist*m_fMoveDist);
m_TargToPos = m_Position - m_Target;
m_fInitDist = fDist;
// if the camera moves too far away, stop it
if((fDist = D3DXVec3Length(&m_TargToPos)) > 150.0f)
{
m_TargToPos *= (1.0f/fDist);
m_TargToPos *= 150.0f;
}
}
/*
* The inexact GiST distance method for geometric types that store bounding
* boxes.
*
* Compute lossy distance from point to index entries. The result is inexact
* because index entries are bounding boxes, not the exact shapes of the
* indexed geometric types. We use distance from point to MBR of index entry.
* This is a lower bound estimate of distance from point to indexed geometric
* type.
*/
Datum
gist_bbox_distance(PG_FUNCTION_ARGS)
{
GISTENTRY *entry = (GISTENTRY *) PG_GETARG_POINTER(0);
StrategyNumber strategy = (StrategyNumber) PG_GETARG_UINT16(2);
bool *recheck = (bool *) PG_GETARG_POINTER(4);
double distance;
StrategyNumber strategyGroup = strategy / GeoStrategyNumberOffset;
/* Bounding box distance is always inexact. */
*recheck = true;
switch (strategyGroup)
{
case PointStrategyNumberGroup:
distance = computeDistance(false,
DatumGetBoxP(entry->key),
PG_GETARG_POINT_P(1));
break;
default:
elog(ERROR, "unknown strategy number: %d", strategy);
distance = 0.0; /* keep compiler quiet */
}
PG_RETURN_FLOAT8(distance);
}
static inline float heuristicSimilarity(
const int distancefunction,
const float alpha,
const Mat& points1, int idx1,
const Mat& points2, int idx2)
{
return 1 / (alpha + computeDistance(distancefunction, points1, idx1, points2, idx2));
}
void CostMap2D::enqueue(unsigned int source, unsigned int mx, unsigned int my){
// If the cell is not marked for cost propagation
unsigned int ind = MC_IND(mx, my);
if(!marked(ind)){
QueueElement* c = new QueueElement(computeDistance(source, ind), source, ind);
queue_.push(c);
mark(ind);
}
}
static inline float gaussianSimilarity(
const int distancefunction,
const float alpha,
const Mat& points1, int idx1,
const Mat& points2, int idx2)
{
float distance = computeDistance(distancefunction, points1, idx1, points2, idx2);
return exp(-alpha * distance * distance);
}
int point_proche(int16 table[][2]) {
int x1, y1, i, x, y, p;
int d1 = 1000;
_vm->_polyStructs = &_vm->_polyStructNorm;
if (nclick_noeud == 1) {
x = x_mouse;
y = y_mouse;
x1 = table_ptselect[0][0];
y1 = table_ptselect[0][1];
_vm->_polyStructs = &_vm->_polyStructExp;
getPixel(x, y);
if (!flag_obstacle) {
_vm->_polyStructs = &_vm->_polyStructNorm;
getPixel(x, y);
if (flag_obstacle) {
polydroite(x1, y1, x, y);
}
_vm->_polyStructs = &_vm->_polyStructExp;
}
if (!flag_obstacle) { /* dans flag_obstacle --> couleur du point */
x1 = table_ptselect[0][0];
y1 = table_ptselect[0][1];
poly2(x, y, x1, y1);
x_mouse = X;
y_mouse = Y;
}
}
_vm->_polyStructs = &_vm->_polyStructNorm;
p = -1;
for (i = 0; i < ctp_routeCoordCount; i++) {
x = table[i][0];
y = table[i][1];
int pointDistance = computeDistance(x_mouse, y_mouse, x, y);
if (pointDistance < d1) {
polydroite(x_mouse, y_mouse, x, y);
if (!flag_obstacle && ctp_routes[i][0] > 0) {
d1 = pointDistance;
p = i;
}
}
}
return (p);
}
void GestureRecognizer::setupPinchData(const NIXTouchEvent& event)
{
const NIXTouchPoint& first = event.touchPoints[0];
const NIXTouchPoint& second = event.touchPoints[1];
m_initialPinchDistance = computeDistance(WKPointMake(first.globalX, first.globalY), WKPointMake(second.globalX, second.globalY));
m_initialPinchScale = m_client->scale();
m_initialPinchContentCenter = computeCenter(WKPointMake(first.x, first.y), WKPointMake(second.x, second.y));
m_previousPinchGlobalCenter = computeCenter(WKPointMake(first.globalX, first.globalY), WKPointMake(second.globalX, second.globalY));
}
bool QgsLayoutItemPolyline::_addNode( const int indexPoint,
QPointF newPoint,
const double radius )
{
const double distStart = computeDistance( newPoint, mPolygon[0] );
const double distEnd = computeDistance( newPoint, mPolygon[mPolygon.size() - 1] );
if ( indexPoint == ( mPolygon.size() - 1 ) )
{
if ( distEnd < radius )
mPolygon.append( newPoint );
else if ( distStart < radius )
mPolygon.insert( 0, newPoint );
}
else
mPolygon.insert( indexPoint + 1, newPoint );
return true;
}
int EuclidDistHeuristic::GetGoalHeuristic(int state_id)
{
if (state_id == planningSpace()->getGoalStateID()) {
return 0;
}
if (m_pose_ext) {
Affine3 p;
if (!m_pose_ext->projectToPose(state_id, p)) {
return 0;
}
auto& goal_pose = planningSpace()->goal().pose;
const double dist = computeDistance(p, goal_pose);
const int h = FIXED_POINT_RATIO * dist;
double Y, P, R;
angles::get_euler_zyx(p.rotation(), Y, P, R);
SMPL_DEBUG_NAMED(LOG, "h(%0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f) = %d", p.translation()[0], p.translation()[1], p.translation()[2], Y, P, R, h);
return h;
} else if (m_point_ext) {
Vector3 p;
if (!m_point_ext->projectToPoint(state_id, p)) {
return 0;
}
auto& goal_pose = planningSpace()->goal().pose;
Vector3 gp(goal_pose.translation());
double dist = computeDistance(p, gp);
const int h = FIXED_POINT_RATIO * dist;
SMPL_DEBUG_NAMED(LOG, "h(%d) = %d", state_id, h);
return h;
} else {
return 0;
}
}
void TargetReport::OutputScores() {
for (unsigned int i = 0; i < reports->size(); ++i) {
ParticleAnalysisReport * report = &(reports->at(i));
if(scoreCompare(scores[i], false)) {
printf("particle: %d is a High Goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
printf("Distance: %f \n", computeDistance(thresholdImage, report, false));
printf("Scores: rect(%.2f) arOuter(%.2f) arInner(%.2f) xEdge(%.2f) yEdge (%.2f)\n",
scores[i].rectangularity,
scores[i].aspectRatioOuter,
scores[i].aspectRatioInner,
scores[i].xEdge,
scores[i].yEdge);
} else if (scoreCompare(scores[i], true)) {
printf("particle: %d is a Middle Goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
printf("Distance: %f \n", computeDistance(thresholdImage, report, true));
printf("Scores: rect(%.2f) arOuter(%.2f) arInner(%.2f) xEdge(%.2f) yEdge (%.2f)\n",
scores[i].rectangularity,
scores[i].aspectRatioOuter,
scores[i].aspectRatioInner,
scores[i].xEdge,
scores[i].yEdge);
} else {
printf("particle: %d is not a goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
printf("Scores: rect(%.2f) arOuter(%.2f) arInner(%.2f) xEdge(%.2f) yEdge (%.2f)\n",
scores[i].rectangularity,
scores[i].aspectRatioOuter,
scores[i].aspectRatioInner,
scores[i].xEdge,
scores[i].yEdge);
}
printf("rect: %f ARinner: %f \n", scores[i].rectangularity, scores[i].aspectRatioInner);
printf("ARouter: %f xEdge: %f yEdge: %f \n", scores[i].aspectRatioOuter, scores[i].xEdge, scores[i].yEdge);
}
printf("\n");
}
// this function process path finding coordinates
void computeAllDistance(int16 table[][10], short int coordCount) {
for (int i = 0; i < coordCount; i++) {
int x1 = ctp_routeCoords[i][0];
int y1 = ctp_routeCoords[i][1];
for (int j = 0; j < ctp_routes[i][0]; j++) {
int p = ctp_routes[i][j+1];
int x2 = ctp_routeCoords[p][0];
int y2 = ctp_routeCoords[p][1];
table[i][p] = computeDistance(x1, y1, x2, y2);
}
}
}
// Input: 2D array of prototypes, 1D array of unknown data
// Output: double representing category for this unknown
// The category is determined by the closest prototype to the unknown
double categorize(std::vector<std::vector<double>> prototypes, std::vector<double> unknown)
{
double minDist = INT_MAX;
double category;
for(int i = 0; i < prototypes.size(); i++)
{
std::vector<double> prototype = prototypes[i];
double dist = computeDistance(prototype, unknown);
if(dist < minDist)
{
minDist = dist;
category = prototype[prototype.size() - 1];
}
}
return category;
}
int EuclidDistHeuristic::GetFromToHeuristic(int from_id, int to_id)
{
if (m_pose_ext) {
if (from_id == planningSpace()->getGoalStateID()) {
auto& gp = planningSpace()->goal().pose;
Affine3 p;
if (!m_pose_ext->projectToPose(to_id, p)) {
return 0;
}
return (int)(FIXED_POINT_RATIO * computeDistance(gp, p));
} else if (to_id == planningSpace()->getGoalStateID()) {
auto& gp = planningSpace()->goal().pose;
Affine3 p;
if (!m_pose_ext->projectToPose(from_id, p)) {
return 0;
}
return (int)(FIXED_POINT_RATIO * computeDistance(p, gp));
} else {
Affine3 a, b;
if (!m_pose_ext->projectToPose(from_id, a) ||
!m_pose_ext->projectToPose(to_id, b))
{
return 0;
}
return (int)(FIXED_POINT_RATIO * computeDistance(a, b));
}
} else if (m_point_ext) {
if (from_id == planningSpace()->getGoalStateID()) {
Vector3 gp(planningSpace()->goal().pose.translation());
Vector3 p;
if (!m_pose_ext->projectToPoint(to_id, p)) {
return 0;
}
return (int)(FIXED_POINT_RATIO * computeDistance(gp, p));
} else if (to_id == planningSpace()->getGoalStateID()) {
Vector3 gp(planningSpace()->goal().pose.translation());
Vector3 p;
if (!m_pose_ext->projectToPoint(from_id, p)) {
return 0;
}
return (int)(FIXED_POINT_RATIO * computeDistance(p, gp));
} else {
Vector3 a, b;
if (!m_pose_ext->projectToPoint(from_id, a) ||
!m_pose_ext->projectToPoint(to_id, b))
{
return 0;
}
return (int)(FIXED_POINT_RATIO * computeDistance(a, b));
}
} else {
return 0;
}
}
void GestureRecognizer::updatePinchData(double timestamp, const NIXTouchEvent& event)
{
assert(m_initialPinchScale > 0);
const NIXTouchPoint& first = event.touchPoints[0];
const NIXTouchPoint& second = event.touchPoints[1];
WKPoint currentCenter = computeCenter(WKPointMake(first.globalX, first.globalY), WKPointMake(second.globalX, second.globalY));
WKPoint delta = WKPointMake((currentCenter.x - m_previousPinchGlobalCenter.x) / m_client->scale(),
(currentCenter.y - m_previousPinchGlobalCenter.y) / m_client->scale());
double currentDistance = computeDistance(WKPointMake(first.globalX, first.globalY), WKPointMake(second.globalX, second.globalY));
double newScale = m_initialPinchScale * (currentDistance / m_initialPinchDistance);
m_client->handlePinch(timestamp, delta, newScale, m_initialPinchContentCenter);
m_previousPinchGlobalCenter = currentCenter;
}
FovObstacleMap::GridSquareState FovObstacleMap::getState(GridSquare pt) const
{
if(!inFieldOfView(pt)) { return UNKNOWN; }
const CellData& cellData = map.cell(pt.x, pt.y);
if(cellData.isKnownObstacle) {
return OBSTACLE;
}
// if this is not an obstacle, it's considered free if there are no obstacles
// between this and the robot (it's not occluded)
else if(computeDistance(pt) < closestObstacleByHeading.cell(computeHeading(pt))) {
return FREE;
}
else {
return UNKNOWN;
}
}
float Action::reachGrasp(MetaBlock *block, const std::string surface_name)
{
//if (verbose_)
ROS_INFO_STREAM("Reaching at distance = " << block->start_pose.position.x << " " << block->start_pose.position.y << " " << block->start_pose.position.z);
//clean object temporally or allow to touch it
/*visual_tools_->cleanupCO(block->name);
ros::Duration(1.0).sleep();*/
/*moveit_msgs::PickupGoal goal;
collision_detection::AllowedCollisionMatrixPtr approach_grasp_acm(new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix()));
std::cout << "attach_object_msg.link_name " << attach_object_msg.link_name << std::endl;
std::cout << "attach_object_msg.object.id " << attach_object_msg.object.id << std::endl;
std::cout << "attach_object_msg.object.operation " << attach_object_msg.object.operation << std::endl;
std::cout << "attach_object_msg.touch_links.size() " << attach_object_msg.touch_links.size() << std::endl;
// we are allowed to touch the target object
approach_grasp_acm->setEntry(goal.target_name, attach_object_msg.touch_links, true);
*/
geometry_msgs::Pose pose = block->start_pose;
pose.position.z += block->size_l/2.0;
//reach the object
if (!reachPregrasp(pose, surface_name))
return std::numeric_limits<float>::max();
//compute the distance to teh object
float dist = computeDistance(move_group_->getCurrentPose().pose, move_group_->getPoseTarget().pose);
if (verbose_)
ROS_INFO_STREAM("Reached at distance = " << dist);
//close the hand, if the object is close enough
/*if (dist < 0.2)
//if (dist > grasp_data_.approach_retreat_min_dist_)
{
//visual_tools_->processCollisionObjectMsg(wrapToCollisionObject(block));
visual_tools_->attachCO(block->name, grasp_data_.ee_group_);
posture.poseHandClose(end_eff);
}*/
return dist;
}
void
SphericalAverage::execute()
{
// loop over quadrature points
for (_qp = 0; _qp < _qrule->n_points(); ++_qp)
{
// compute target bin
auto bin = computeDistance() / _deltaR;
// add the volume contributed by the current quadrature point
if (bin >= 0 && bin < static_cast<int>(_nbins))
{
for (auto j = decltype(_nvals)(0); j < _nvals; ++j)
(*_average[j])[bin] += (*_values[j])[_qp];
_counts[bin]++;
}
}
}
