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;
}
示例#2
0
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;
}
示例#3
0
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();
	}
}