int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init,
const KDL::Frame& p_in,
KDL::JntArray &q_out)
{
Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
std::vector<std::vector<double> > solution_ik;
if(free_angle_ == 0)
{
ROS_DEBUG("Solving with %f",q_init(0));
pr2_arm_ik_.computeIKShoulderPan(b,q_init(0),solution_ik);
}
else
{
pr2_arm_ik_.computeIKShoulderRoll(b,q_init(2),solution_ik);
}
if(solution_ik.empty())
return -1;
double min_distance = 1e6;
int min_index = -1;
for(int i=0; i< (int) solution_ik.size(); i++)
{
ROS_DEBUG("Solution : %d",(int)solution_ik.size());
for(int j=0; j < (int)solution_ik[i].size(); j++)
{
ROS_DEBUG("%d: %f",j,solution_ik[i][j]);
}
ROS_DEBUG(" ");
ROS_DEBUG(" ");
double tmp_distance = computeEuclideanDistance(solution_ik[i],q_init);
if(tmp_distance < min_distance)
{
min_distance = tmp_distance;
min_index = i;
}
}
if(min_index > -1)
{
q_out.resize((int)solution_ik[min_index].size());
for(int i=0; i < (int)solution_ik[min_index].size(); i++)
{
q_out(i) = solution_ik[min_index][i];
}
return 1;
}
else
return -1;
}
bool KNN::predict(VectorDouble inputVector,UINT K){
if( !trained ){
errorLog << "predict(VectorDouble inputVector,UINT K) - KNN model has not been trained" << endl;
return false;
}
if( inputVector.size() != numFeatures ){
errorLog << "predict(VectorDouble inputVector) - the size of the input vector " << inputVector.size() << " does not match the number of features " << numFeatures << endl;
return false;
}
if( K > trainingData.getNumSamples() ){
errorLog << "predict(VectorDouble inputVector,UINT K) - K Is Greater Than The Number Of Training Samples" << endl;
return false;
}
if( useScaling ){
for(UINT i=0; i<inputVector.size(); i++){
inputVector[i] = scale(inputVector[i], ranges[i].minValue, ranges[i].maxValue, 0, 1);
}
}
//TODO - need to build a kdtree of the training data to allow better realtime prediction
const UINT M = trainingData.getNumSamples();
vector< IndexedDouble > neighbours;
for(UINT i=0; i<M; i++){
double dist = 0;
UINT classLabel = trainingData[i].getClassLabel();
VectorDouble trainingSample = trainingData[i].getSample();
switch( distanceMethod ){
case EUCLIDEAN_DISTANCE:
dist = computeEuclideanDistance(inputVector,trainingSample);
break;
case COSINE_DISTANCE:
dist = computeCosineDistance(inputVector,trainingSample);
break;
case MANHATTAN_DISTANCE:
dist = computeManhattanDistance(inputVector, trainingSample);
break;
default:
errorLog << "predict(vector< double > inputVector) - unkown distance measure!" << endl;
return false;
break;
}
if( neighbours.size() < K ){
neighbours.push_back( IndexedDouble(classLabel,dist) );
}else{
//Find the maximum value in the neighbours buffer
double maxValue = neighbours[0].value;
UINT maxIndex = 0;
for(UINT n=1; n<neighbours.size(); n++){
if( neighbours[n].value > maxValue ){
maxValue = neighbours[n].value;
maxIndex = n;
}
}
//If the dist is less than the maximum value in the buffer, then replace that value with the new dist
if( dist < maxValue ){
neighbours[ maxIndex ] = IndexedDouble(classLabel,dist);
}
}
}
//Predict the class ID using the labels of the K nearest neighbours
if( classLikelihoods.size() != numClasses ) classLikelihoods.resize(numClasses,0);
else for(UINT i=0; i<classLikelihoods.size(); i++){ classLikelihoods[i] = 0; }
if( classDistances.size() != numClasses ) classDistances.resize(numClasses,0);
else for(UINT i=0; i<classDistances.size(); i++){ classDistances[i] = 0; }
//Count the classes
for(UINT k=0; k<neighbours.size(); k++){
UINT classLabel = neighbours[k].index;
if( classLabel == 0 ){
errorLog << "predict(VectorDouble inputVector) - Class label of training example can not be zero!" << endl;
return false;
}
//Find the index of the classLabel
UINT classLabelIndex = 0;
for(UINT j=0; j<numClasses; j++){
if( classLabel == classLabels[j] ){
classLabelIndex = j;
break;
}
}
classLikelihoods[ classLabelIndex ] += 1;
classDistances[ classLabelIndex ] += neighbours[k].value;
}
//Get the max count
double maxCount = classLikelihoods[0];
UINT maxIndex = 0;
for(UINT i=1; i<classLikelihoods.size(); i++){
if( classLikelihoods[i] > maxCount ){
maxCount = classLikelihoods[i];
maxIndex = i;
}
}
//Compute the average distances per class
for(UINT i=0; i<classDistances.size(); i++){
if( classLikelihoods[i] > 0 ) classDistances[i] /= classLikelihoods[i];
else classDistances[i] = BIG_DISTANCE;
}
//Normalize the likelihoods
for(UINT i=0; i<classLikelihoods.size(); i++){
classLikelihoods[i] /= double( neighbours.size() );
}
//Set the maximum likelihood value
maxLikelihood = classLikelihoods[ maxIndex ];
if( useNullRejection ){
if( classDistances[ maxIndex ] <= rejectionThresholds[ maxIndex ] ){
predictedClassLabel = classLabels[maxIndex];
}else{
predictedClassLabel = GRT_DEFAULT_NULL_CLASS_LABEL; //Set the gesture label as the null label
}
}else{
predictedClassLabel = classLabels[maxIndex];
}
return true;
}
void SonarMap::addScan(double sonar_x, double sonar_y, double sonar_theta, double fov, double max_range, double distance, double uncertainty)
{
int cx=0, cy=0;
double wx = 0.0, wy = 0.0;
convertToCell(sonar_x, sonar_y, cx, cy);
mapstore::MapStoreCone msc(double(cx), double(cy), sonar_theta, fov, distance/resolution_);
std::list<int> occxs;
std::list<int> occys;
std::list<double> occ_vals;
double sum_occupied = 0;
if (distance<uncertainty/2) {
//fixes crashing on wall entering
return;
}
while (msc.nextCell(cx,cy)) {
if (convertToMap(cx, cy, wx, wy)) {
double tmp_lin_dist = computeEuclideanDistance(wx, wy, sonar_x, sonar_y);
double tmp_ang_dist = computeAngularDistance(sonar_theta, atan2(wy-sonar_y, wx-sonar_x));
double p_linear;
double p_angular = this->Ea(fov, tmp_ang_dist);
double tmp_occ = 0;
double tmp_free = 0;
if (abs(distance-max_range)<uncertainty) {
//beams do not hit any obstacle
p_linear = this->ErFree(distance, tmp_lin_dist, uncertainty);
tmp_free = p_linear*p_angular;
} else {
if (tmp_lin_dist<distance) {
p_linear = this->ErFree(distance, tmp_lin_dist, uncertainty);
tmp_free = p_linear*p_angular;
} else {
double p_linear = this->ErOcc(distance, tmp_lin_dist, uncertainty);
tmp_occ = p_linear*p_angular;
}
}
double free_val_prev = map_free_.get(cx,cy);
double occ_val_prev = map_occupied_.get(cx,cy);
double free_val_new = free_val_prev + tmp_free - free_val_prev*tmp_free;
double occ_val_new = tmp_occ*(1-free_val_new);
sum_occupied += occ_val_new;
occ_vals.push_front(occ_val_new);
occxs.push_front(cx);
occys.push_front(cy);
map_free_.set(cx,cy, free_val_new);
}
}
//Normalization pass over stored x and y's
while (!occ_vals.empty()) {
cx = occxs.back();
cy = occys.back();
double tmp_val = occ_vals.back();
if (sum_occupied>0.05) {
//normalization not possible otherwise
double normalized_occ = tmp_val/sum_occupied;
double occ_prev = map_occupied_.get(cx,cy);
map_occupied_.set(cx,cy, occ_prev + normalized_occ - occ_prev*normalized_occ);
}
double occ = map_occupied_.get(cx,cy);
double free = map_free_.get(cx,cy);
if (occ>free) {
map_.set(cx, cy, occ);
} else {
map_.set(cx,cy, -free);
}
occxs.pop_back();
occys.pop_back();
occ_vals.pop_back();
}
}
