コード例 #1
0
ファイル: Trajectory.cpp プロジェクト: humm/dovecot
Trajectory Trajectory::generatePolynomialTrajectoryThroughViapoint(const VectorXd& ts, const VectorXd& y_from, const VectorXd& y_yd_ydd_viapoint, double viapoint_time, const VectorXd& y_to)
{

    int n_dims = y_from.size();
    assert(n_dims==y_to.size());
    assert(3*n_dims==y_yd_ydd_viapoint.size()); // Contains y, yd and ydd, so *3

    int n_time_steps = ts.size();

    int viapoint_time_step = 0;
    while (viapoint_time_step<n_time_steps && ts[viapoint_time_step]<viapoint_time)
        viapoint_time_step++;

    if (viapoint_time_step>=n_time_steps)
    {
        cerr << __FILE__ << ":" << __LINE__ << ":";
        cerr << "ERROR: the time vector does not contain any time smaller than " << viapoint_time << ". Returning min-jerk trajectory WITHOUT viapoint." <<  endl;
        return Trajectory();
    }

    VectorXd yd_from        = VectorXd::Zero(n_dims);
    VectorXd ydd_from       = VectorXd::Zero(n_dims);

    VectorXd y_viapoint     = y_yd_ydd_viapoint.segment(0*n_dims,n_dims);
    VectorXd yd_viapoint    = y_yd_ydd_viapoint.segment(1*n_dims,n_dims);
    VectorXd ydd_viapoint   = y_yd_ydd_viapoint.segment(2*n_dims,n_dims);

    VectorXd yd_to          = VectorXd::Zero(n_dims);
    VectorXd ydd_to         = VectorXd::Zero(n_dims);

    Trajectory traj = Trajectory::generatePolynomialTrajectory(ts.segment(0, viapoint_time_step + 1), y_from, yd_from, ydd_from, y_viapoint, yd_viapoint, ydd_viapoint);
    traj.append(Trajectory::generatePolynomialTrajectory(ts.segment(viapoint_time_step, n_time_steps - viapoint_time_step), y_viapoint, yd_viapoint, ydd_viapoint, y_to, yd_to, ydd_to));

    return traj;
}
コード例 #2
0
ファイル: XTCReader.cpp プロジェクト: jchodera/fah-viewer
void XTCReader::read(Trajectory &trajectory) {
  while (true) {
    SmartPointer<Positions> positions = new Positions;
    if (!read(*positions, trajectory.getTopology().get())) break;
    trajectory.add(positions);
  }
}
コード例 #3
0
void CalibrateAction::visualize_trajectory(Trajectory &traj)
{
    // publishing to rviz
    geometry_msgs::PoseArray poses;
    for(unsigned int i=0; i<traj.getPointsSize();i++)
    {
        geometry_msgs::Pose temp_pose;
        double x_, y_,th_;
        traj.getPoint(i, x_, y_, th_);
        tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose.orientation);
        temp_pose.position.x = x_;
        temp_pose.position.y = y_;
        poses.poses.push_back(temp_pose);
        //ROS_INFO("Pose: x: %f y: %f th: %f", x_, y_, th_);
    }
    geometry_msgs::Pose temp_pose, temp_pose2;
    double x_, y_,th_;
    traj.getPoint(0, x_, y_, th_);
    tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose.orientation);
    temp_pose.position.x = x_;
    temp_pose.position.y = y_;
    traj.getEndpoint(x_, y_, th_);
    tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose2.orientation);
    temp_pose2.position.x = x_;
    temp_pose2.position.y = y_;
    //ROS_INFO("Visualize trajectory with %i points. First Point (%2.1f,%2.1f), last Point (%2.1f,%2.1f)", traj.getPointsSize(),temp_pose.position.x,temp_pose.position.y,temp_pose2.position.x,temp_pose2.position.y);
    poses.header.frame_id = cost_map->getGlobalFrameID();;
    estTraj_pub.publish(poses);
}
コード例 #4
0
ファイル: Dmp.cpp プロジェクト: humm/dovecot
void Dmp::train(const Trajectory& trajectory, string save_directory, bool overwrite)
{
  
  // Set tau, initial_state and attractor_state from the trajectory 
  set_tau(trajectory.duration());
  set_initial_state(trajectory.initial_y());
  set_attractor_state(trajectory.final_y());
  
  VectorXd fa_input_phase;
  MatrixXd f_target;
  computeFunctionApproximatorInputsAndTargets(trajectory, fa_input_phase, f_target);
  
  // Some checks before training function approximators
  assert(!function_approximators_.empty());
  
  for (unsigned int dd=0; dd<function_approximators_.size(); dd++)
  {
    // This is just boring stuff to figure out if and where to store the results of training
    string save_directory_dim;
    if (!save_directory.empty())
    {
      if (function_approximators_.size()==1)
        save_directory_dim = save_directory;
      else
        save_directory_dim = save_directory + "/dim" + boost::lexical_cast<string>(dd);
    }
    
    // Actual training is happening here.
    VectorXd fa_target = f_target.col(dd);
    if (function_approximators_[dd]->isTrained())
      function_approximators_[dd]->reTrain(fa_input_phase,fa_target,save_directory_dim,overwrite);
    else
      function_approximators_[dd]->train(fa_input_phase,fa_target,save_directory_dim,overwrite);
  }
}
コード例 #5
0
void TrajectoryAnalyzer::analyze(Trajectory & trajectory,
                                 std::vector<std::shared_ptr<ProteinSegment>> & protein_segments,
                                 const double & temperature,
                                 const int ensemble_size) {

    std::vector<ProteinSegmentEnsemble> protein_segment_ensembles;

    LOGD << "Fitting protein segments with trajectory frames and computing force constants.";
    while (trajectory.has_next()) {
        for (std::shared_ptr<ProteinSegment> const & protein_segment : protein_segments) {
            protein_segment_ensembles.push_back(ProteinSegmentEnsemble(protein_segment));
        }

        LOGD << "adding frames to protein segment ensembles";
        int frame_nr = 0;
        while (trajectory.has_next() && ++frame_nr <= ensemble_size) {
            Frame frame = trajectory.get_next_frame();

            #pragma omp parallel for
            for (size_t i = 0; i < protein_segment_ensembles.size(); i++) {
                protein_segment_ensembles[i].add_frame(frame);
            }

            LOGV_IF(frame_nr % 100 == 0) << "read " << frame_nr << " frames";
        }
        LOGD << "read total number of " << frame_nr << " frames";

        LOGD << "computing force constants for protein segment ensembles.";
        #pragma omp parallel for
        for (size_t i = 0; i < protein_segment_ensembles.size(); i++) {
            protein_segment_ensembles[i].compute_force_constant(temperature);
        }
    }
    LOGD << "Finished analyzing trajectory";
}
コード例 #6
0
Trajectory::Trajectory(const Trajectory& source_traj, const std::string& planning_group):
  planning_group_name_(planning_group),
  discretization_(source_traj.discretization_)
{
	ROS_INFO("Pezzotto safe at ChomTrajectory constructor");
  num_joints_ = source_traj.getNumJoints();

  num_points_ = source_traj.num_points_;
  start_index_ = source_traj.getStartIndex();
  end_index_ = (num_points_ - 1);
  duration_ = (num_points_ - 1)*discretization_;

  // allocate the memory:
  init();

  full_trajectory_index_.resize(num_points_);

  // now copy the trajectories over:
  for (int i=0; i<num_points_; i++)
  {
    full_trajectory_index_[i] = i;
    for (int j=0; j<num_joints_; j++)
    {
      (*this)(i,j) = source_traj(i, j);
    }
  }
}
コード例 #7
0
// --------------------------------------------------------------------------
// commence le movement qui va vers l'entree d'un pont juste un peu decalle en y (~15cm)
// --------------------------------------------------------------------------
bool StrategyAttackCL::gotoBridgeEntryNear(Millimeter y)
{
    Trajectory t;
    disableBridgeCaptors();
    LOG_COMMAND("gotoBridgeEntry Hard Near:%d\n", (int)y);
    // on ne va pas loin en y donc on fait un joli mouvement en S
    // met les servos en position
    Millimeter y2 = RobotPos->y();
    if (y>1800) { // pont du bord
      Move->go2Target(Point(BRIDGE_ENTRY_NAVIGATE_X, y2), 3, 30);
    } else if (y<1500) { //  pont du bord ou pont contre le milieu
      Move->go2Target(Point(BRIDGE_ENTRY_NAVIGATE_X, y2), 3, 30);
    } else { // ponts au milieu
      t.push_back(Point(BRIDGE_DETECT_BUMP_X-100, y2));
      t.push_back(Point(BRIDGE_ENTRY_NAVIGATE_X, (2*y+y2)/3)); 
      Move->followTrajectory(t, TRAJECTORY_RECTILINEAR, 3, 30);
    }
     
    Events->wait(evtEndMove);
    Move->enableAccelerationController(false);
    if (checkEndEvents()) return false; 
    if(!Events->isInWaitResult(EVENTS_MOVE_END)) {
      // collision
      Move->backward(100);
      Events->wait(evtEndMoveNoCollision);
      return false;
    }
    
    // va en face du pont
    enableBridgeCaptors();
    MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
    Move->go2Target(BRIDGE_DETECT_BUMP_X, y, 2, 30);
    // dont need to wait, it is done by the upper function
    return true;
}
コード例 #8
0
// --------------------------------------------------------------------------
// commence le movement qui va vers l'entree d'un pont tres decalle en y (>20cm)
// --------------------------------------------------------------------------
bool StrategyAttackCL::gotoBridgeEntryFar(Millimeter y)
{
    disableBridgeCaptors();
    Millimeter y2 = RobotPos->y();
    LOG_COMMAND("gotoBridgeEntry Hard Far:%d\n", (int)y);
    // on va loin: on recule bettement puit on prend un point cible
    // on s'eloigne un peu du bord, pour aller en x qui nous permet
    //de nous promener tranquillement le long de la riviere
    Move->go2Target(BRIDGE_ENTRY_NAVIGATE_X, y2, 2, 40); // norotation
    Events->wait(evtEndMove);
    if (checkEndEvents() || 
        !Events->isInWaitResult(EVENTS_MOVE_END)) {
        Move->enableAccelerationController(false);
        return false;
    }
    
    // va en face du pont
    enableBridgeCaptors();
    Trajectory t;
    // Pour eviter les rotations finales on 
    if (y2 < y)
      t.push_back(Point(BRIDGE_ENTRY_NAVIGATE_X, y-75));
    else
      t.push_back(Point(BRIDGE_ENTRY_NAVIGATE_X, y+75));
    t.push_back(Point(BRIDGE_DETECT_BUMP_X, y));
    MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
    Move->followTrajectory(t, TRAJECTORY_RECTILINEAR, 2, 30, true); // norotation
    // dont wait events, it is done by the upper function
    return true;
}
コード例 #9
0
double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
  double cost = 0;
  double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
  double px, py, pth;
  if (footprint_spec_.size() == 0) {
    // Bug, should never happen
    ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
    return -9;
  }

  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
    traj.getPoint(i, px, py, pth);
    double f_cost = footprintCost(px, py, pth,
        scale, footprint_spec_,
        costmap_, world_model_);

    if(f_cost < 0){
        return f_cost;
    }

    if(sum_scores_)
        cost +=  f_cost;
    else
        cost = f_cost;
  }
  return cost;
}
コード例 #10
0
  /**
   * return a score for trajectory traj
   */
  virtual double scoreTrajectory(Trajectory &traj)
  { 
      if(traj.getPointsSize()==0)
        return 0.0;

      double px, py, pth;
      traj.getPoint(0, px, py, pth);
      
      double dist_to_goal = hypot(px-goal_x_, py-goal_y_);
      if(dist_to_goal < .2){
        return 0.0;
      }

      
      double start_diff;
      if(!getAngleDiff(px,py,pth,&start_diff))
        return -4.0;

      if(fabs(start_diff) < max_trans_angle_){
	return 0.0;
      }

      if(fabs(traj.xv_) > 0.0 || fabs(traj.yv_) > 0.0)
        return -3.0;
      else if( sign(start_diff) != sign(traj.thetav_) )
        return -2.0;
        
      traj.getPoint(traj.getPointsSize()-1, px, py, pth);
      double end_diff;
      if(!getAngleDiff(px,py,pth,&end_diff))
        return -1.0;
        
      return fabs(end_diff);
  }
コード例 #11
0
ファイル: Scene.cpp プロジェクト: SPICEFI/MissionsVisualizer
void Scene::AddTrajectoryAsSpaceObject(const SpaceObject& obj, Frame frame, Date startingDate, const SpaceObject& relativeTo)
{
	Trajectory tj = Trajectory(obj, frame, Units::Metric::kilometers, distanceScale);
	tj.SetRelativeTo(relativeTo);
	standaloneTrajectories.push_back(/*Trajectory(obj, frame, Units::Metric::kilometers)*/tj);
	Time time(10000, Units::Common::days);
	standaloneTrajectories.at(standaloneTrajectories.size() - 1).SetIncrementalParams(startingDate, time, 10000);
}
コード例 #12
0
QPainterPath getTrajectoryPath(const Trajectory& traj, double timespanMs, double timeLCMs) {
    QPainterPath p;
    p.moveTo(traj.x(0)*fieldXConvert, traj.y(0)*fieldXConvert);
    for (int i = 1; i*timeLCMs < timespanMs; i++) {
        p.lineTo(traj.x(i*timeLCMs*0.001)*fieldXConvert, traj.y(i*timeLCMs*0.001)*fieldXConvert);
    }
    return p;
}
コード例 #13
0
bool HasMajorAirports(Trajectory &trajectory, 
 std::vector<std::string> &airports)
{
  return std::binary_search(airports.begin(),airports.end(),
   trajectory.front().string_property("arr")) &&
   std::binary_search(airports.begin(),airports.end(),
   trajectory.front().string_property("dep"));
}
コード例 #14
0
 void traj_cb(const occgrid_planner::TrajectoryConstPtr & msg) {
     frame_id_ = msg->header.frame_id;
     delay_ = 0.0;
     traj_.clear();
     for (unsigned int i=0;i<msg->Ts.size();i++) {
         traj_.insert(Trajectory::value_type(msg->Ts[i].header.stamp.toSec(), msg->Ts[i]));
     }
     ROS_INFO("Trajectory received");
 }
コード例 #15
0
ファイル: trajectory.cpp プロジェクト: amiryanj/cyrus_ssl
void Trajectory::copyFrom(Trajectory &other)
{
    m_states_vec.clear();
    for(int i=0; i<other.length(); i++) {
//        other.getStation(i).getPosition().print(std::cout);
        Station st = other.getStation(i);
        this->appendState(st);
    }
    this->cost = other.cost;
}
コード例 #16
0
bool TrajectoryTest::equal(const Trajectory<CvPoint>& t1, const Trajectory<CvPoint>& t2) const
{
	if (t1.getId() != t2.getId())
	{
		CPPUNIT_ASSERT (t1.getId() == t2.getId());
		return false;
	}

	if (t1.size() != t2.size())
	{
		return false;
	}

	for (unsigned i = 0; i < t1.size(); ++i)
	{
		const TrajectoryElement<CvPoint> e1 = t1.getTrajectoryElement(i);
		const TrajectoryElement<CvPoint> e2 = t2.getTrajectoryElement(i);
		if (e1 != e2)
		{
			return false;
		}
	}

	return true;
}
コード例 #17
0
double FutureObstacleCost::cost(const Trajectory& trajectory)
{
    const int num_milestones = trajectory.getNumMilestones();
    const int num_interpolation_samples = trajectory.getNumInterpolationSamples();
    
    for (int i=0; i<num_milestones; i++)
    {
        for (int j = -1; j < num_interpolation_samples; j++)
        {
        }
    }
}
コード例 #18
0
void DmpWithGainSchedules::train(const Trajectory& trajectory, std::string save_directory, bool overwrite)
{
  // First, train the DMP
  Dmp::train(trajectory,save_directory,overwrite);
  
  // Get phase from trajectory
  // Integrate analytically to get phase states
  MatrixXd xs_ana;
  MatrixXd xds_ana;
  Dmp::analyticalSolution(trajectory.ts(),xs_ana,xds_ana);
  MatrixXd xs_phase  = xs_ana.PHASEM(trajectory.length());


  // Get targets from trajectory
  MatrixXd targets = trajectory.misc();
  
  // The dimensionality of the extra variables in the trajectory must be the same as the number of
  // function approximators.
  assert(targets.cols()==(int)function_approximators_gains_.size());
  
  // Train each fa_gains, see below
  // Some checks before training function approximators
  assert(!function_approximators_gains_.empty());
  
  for (unsigned int dd=0; dd<function_approximators_gains_.size(); dd++)
  {
    // This is just boring stuff to figure out if and where to store the results of training
    string save_directory_dim;
    if (!save_directory.empty())
    {
      if (function_approximators_gains_.size()==1)
        save_directory_dim = save_directory;
      else
        save_directory_dim = save_directory + "/gains" + to_string(dd);
    }
    
    // Actual training is happening here.
    VectorXd cur_target = targets.col(dd);
    if (function_approximators_gains_[dd]==NULL)
    {
      cerr << __FILE__ << ":" << __LINE__ << ":";
      cerr << "WARNING: function approximator cannot be trained because it is NULL." << endl;
    }
    else
    {
      if (function_approximators_gains_[dd]->isTrained())
        function_approximators_gains_[dd]->reTrain(xs_phase,cur_target,save_directory_dim,overwrite);
      else
        function_approximators_gains_[dd]->train(xs_phase,cur_target,save_directory_dim,overwrite);
    }

  }
}
コード例 #19
0
ファイル: generate_map.cpp プロジェクト: RaresAmbrus/clams
int main(int argc, char** argv)
{
  namespace bpo = boost::program_options;
  namespace bfs = boost::filesystem;
  bpo::options_description opts_desc("Allowed options");
  bpo::positional_options_description p;

  string sseq_path;
  string traj_path;
  string map_path;
  double resolution;
  double max_range;
  opts_desc.add_options()
    ("help,h", "produce help message")
    ("sseq", bpo::value(&sseq_path)->required(), "StreamSequence, i.e. asus data.")
    ("traj", bpo::value(&traj_path)->required(), "Trajectory from slam in CLAMS format.")
    ("map", bpo::value(&map_path)->required(), "Where to save the output map.")
    ("resolution", bpo::value(&resolution)->default_value(0.01), "Resolution of the voxel grid used for filtering.")
    ("max-range", bpo::value(&max_range)->default_value(MAX_RANGE_MAP), "Maximum range to use when building the map from the given trajectory, in meters.")
    ;

  p.add("sseq", 1);
  p.add("traj", 1);
  p.add("map", 1);
  
  bpo::variables_map opts;
  bool badargs = false;
  try {
    bpo::store(bpo::command_line_parser(argc, argv).options(opts_desc).positional(p).run(), opts);
    bpo::notify(opts);
  }
  catch(...) { badargs = true; }
  if(opts.count("help") || badargs) {
    cout << "Usage: " << bfs::basename(argv[0]) << " [OPTS] SSEQ TRAJ MAP" << endl;
    cout << "  A map will be generated from SSEQ and TRAJ.  It will be saved to MAP." << endl;
    cout << endl;
    cout << opts_desc << endl;
    return 1;
  }

  Trajectory traj;
  traj.load(traj_path);
  StreamSequenceBase::ConstPtr sseq = StreamSequenceBase::initializeFromDirectory(sseq_path);
  cout << "Building map from " << endl;
  cout << "  " << sseq_path << endl;
  cout << "  " << traj_path << endl;
  cout << "Saving to " << map_path << endl;
  Cloud::Ptr map = SlamCalibrator::buildMap(sseq, traj, max_range, resolution);
  pcl::io::savePCDFileBinary(map_path, *map);
  
  return 0;
}
コード例 #20
0
// --------------------------------------------------------------------------
// va a l'endroit ou on detecte les pont par capteurs sharps 
// --------------------------------------------------------------------------
bool StrategyAttackCL::gotoBridgeDetection(bool gotoSiouxFirst)
{
  Events->disable(EVENTS_NO_BRIDGE_BUMP_LEFT);
  Events->disable(EVENTS_NO_BRIDGE_BUMP_RIGHT);
  while(true) {
      LOG_INFO("gotoBridgeDetection(%s)\n",
               gotoSiouxFirst?"Passe par le milieu":"Passe par un pont normal");
      Trajectory t;
      t.push_back(Point(RobotPos->x(), RobotPos->y()));
      // if looping (while(true)..) be careful not to go too close.
      if (RobotPos->x() < BRIDGE_DETECT_SHARP_X - 500)
	  t.push_back(Point(RobotPos->x()+250, RobotPos->y()));
      if (gotoSiouxFirst) {
          // va vers le pont du milieu
          t.push_back(Point(BRIDGE_DETECT_SHARP_X-250, BRIDGE_ENTRY_SIOUX_Y));
          t.push_back(Point(BRIDGE_DETECT_SHARP_X,     BRIDGE_ENTRY_SIOUX_Y));
      } else {
          // va vers la gauche du terrain pour detecter la position du pont d'un coup
          t.push_back(Point(BRIDGE_DETECT_SHARP_X-250, BRIDGE_ENTRY_MIDDLE_BORDURE_Y));
          t.push_back(Point(BRIDGE_DETECT_SHARP_X,     BRIDGE_ENTRY_MIDDLE_BORDURE_Y));
      }
      Move->followTrajectory(t, TRAJECTORY_RECTILINEAR, -1, 40); // gain, vitesse max
      Events->wait(evtEndMove);
      if (checkEndEvents()) return false;
      if (Events->isInWaitResult(EVENTS_MOVE_END)) {
	Move->rotate(0);
	Move->stop();

         Events->wait(evtEndMove);
	 if (checkEndEvents()) return false;
	 if (Events->isInWaitResult(EVENTS_MOVE_END)) {
	   return true;
	 }
	 return true;
      }
      if (RobotPos->x()>600 || Timer->time() > 6000) {
	// collision: on recule et on essaye de repartir par un autre endroit...
	Move->backward(150);
	Events->wait(evtEndMoveNoCollision);
	gotoSiouxFirst = !gotoSiouxFirst;
      }
      if (isZeroAngle(RobotPos->thetaAbsolute() - M_PI, M_PI_2) &&
	  RobotPos->x() < 600)
      {
	Move->backward(700);
	Events->wait(evtEndMoveNoCollision);
	gotoSiouxFirst = !gotoSiouxFirst;
      }
  }
  return false;
}
コード例 #21
0
ファイル: fitter.cpp プロジェクト: sonia3994/SaRoMaN
//*************************************************************
int fitter::GetQ(const Trajectory& traj){
  //*************************************************************
  
  if (_model.compare("particle/helix")!=0) return 0;
  double q;
  
  /// 
  q = traj.state(traj.last_fitted_node()).vector()[dim-1];
  
  if (q<0) q=-1; else q=1;
  
  return (int) q;
  
}
コード例 #22
0
void TrajectoryTest::testSizeInc(void)
{
	Trajectory<CvPoint> *t = new Trajectory<CvPoint> ();

	for (unsigned i = 0; i < sampleTrajectory->size(); ++i)
	{
		CPPUNIT_ASSERT_EQUAL(t->size(), (unsigned int)i);

		CvPoint point = (*sampleTrajectory)[i];
		t->add(point);
	}

	CPPUNIT_ASSERT_EQUAL(t->size(), sampleTrajectory->size());
}
コード例 #23
0
void
MapView2D<SLAM>::pollSlam() {
	if( !this->m_slam )
		return;

	Trajectory 						traj;
	TrajectoryVector				allTraj;
	std::vector<Eigen::Matrix3d>	covs;
	std::vector<size_t>				timePoints;


	this->m_slam->lock();
	size_t	idx 				= this->m_slam->bestParticleIdx( false );
	const	MapType	map			= this->m_slam->map( idx, false );
	this->m_bestParticlePos		= this->m_slam->pose( idx ).pos();

	if( m_showBestPath ) {
		traj = this->m_slam->trajectory( idx, false );

		if( m_showCovariances && traj.size() > 9 ) {
			timePoints.reserve( traj.size() / 3 - 2 );
			for( size_t i = 1; i < traj.size() - 1; i += 3 )
				timePoints.push_back( i );

			covs = this->m_slam->covariances( timePoints, false );
		}
	}

	if( m_showPaths )
		allTraj = this->m_slam->trajectories( false );

	std::vector<Point2w> posVec;
	posVec.reserve( this->m_slam->numParticles() );
	for( size_t i = 0; i < this->m_slam->numParticles(); i++ )
		posVec.push_back( this->m_slam->pose( i ).pos() );
	this->m_slam->unlock();

	// The order of drawing is important in order not to overdraw something
	drawMap( map );
	if( m_showPaths )
		for( const auto &t : allTraj )
			drawTrajectory( t, Qt::darkYellow );
	if( m_showBestPath ) {
		if( m_showCovariances )
			drawCovariances( traj, covs, timePoints, Qt::darkYellow );
		drawTrajectory( traj );
	}
	drawPosVector( posVec );
}
コード例 #24
0
// estimates if, at current position, all space for speed up and continuing calibration run is free up to threshold, given the values in the parameters
bool CalibrateAction::estimateFullPathIsClear(double vx_samp, double vy_samp, double vtheta_samp, double  sim_time_,
                                     double acc_x, double acc_y, double acc_theta)
{
    bool pathClear = false;
    Trajectory tempTraj;
    double x_, y_, theta_;

    pathClear = checkPath(0, 0, 0, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, &tempTraj);

    tempTraj.getEndpoint(x_, y_, theta_);

    pathClear = checkPath(x_, y_, theta_, vx_samp, vy_samp, vtheta_samp, sim_time_, vx_samp, vy_samp, vtheta_samp, 0, 0, 0);

    return pathClear;
}
コード例 #25
0
ファイル: ArduinoComparer.cpp プロジェクト: ReliaSolve/Latent
double ArduinoComparer::computeError(
                const Trajectory &aT
                , const Trajectory &dT
                , double offsetSeconds
  ) const
{
  double sum = 0;

  // Check each reading in the device trajectory.  Find its time and
  // shift it.  Then read the value from the Arduino trace at the
  // shifted time and run its value through the mapping to find the
  // expected device value at that time.  Compute the sum of squared
  // errors.
  //  Because we want a positive offset to correspond to the device
  // measurements being behind the Arduino, we need to offset the
  // Arduino measurements by the negative of the offset.
  for (size_t i = 0; i < dT.m_entries.size(); i++) {
    double deviceValue = dT.m_entries[i].m_value;
    double timeShifted = dT.m_entries[i].m_time - offsetSeconds;
    size_t arduinoValue = static_cast<size_t>(aT.lookup(timeShifted));
    double expectedDeviceValue = m_mappingMean[arduinoValue];
    sum += (expectedDeviceValue - deviceValue) * 
           (expectedDeviceValue - deviceValue);    
  }

  return sum;
}
コード例 #26
0
bool PlanningProblem::checkPlanValidity(Trajectory &plan, float tolerance_coeff)
{
    if(plan.length() == 0)
        return false;
    if(Station::euclideanDistance(plan.getStation(0), initialState) > tolerance_coeff* agent->radius())
        return false;    
    if(goal.minDistTo(plan.getLastStation()) > tolerance_coeff * agent->radius())
        return false;
    if(Station::euclideanDistance(plan.getLastStation(), goal.goal_point) > tolerance_coeff * agent->radius() )
        return false;
//    for(int i=0; i<plan.length(); i++) {
//        if(!CheckValidity(plan.getStation(i)))
//            return false;
//    }
    return true;
}
コード例 #27
0
ファイル: Dmp.cpp プロジェクト: humm/dovecot
void Dmp::computeFunctionApproximatorInputsAndTargets(const Trajectory& trajectory, VectorXd& fa_inputs_phase, MatrixXd& f_target) const
{
  int n_time_steps = trajectory.length();
  double dim_data = trajectory.dim();
  
  if (dim_orig()!=dim_data)
  {
    cout << "WARNING: Cannot train " << dim_orig() << "-D DMP with " << dim_data << "-D data. Doing nothing." << endl;
    return;
  }
  
  // Integrate analytically to get goal, gating and phase states
  MatrixXd xs_ana;
  MatrixXd xds_ana;
  
  // Before, we would make clone of the dmp, and integrate it with the tau, and initial/attractor
  // state of the trajectory. However, Thibaut needed to call this from outside the Dmp as well,
  // with the tau/states of the this object. Therefore, we no longer clone. 
  // Dmp* dmp_clone = static_cast<Dmp*>(this->clone());
  // dmp_clone->set_tau(trajectory.duration());
  // dmp_clone->set_initial_state(trajectory.initial_y());
  // dmp_clone->set_attractor_state(trajectory.final_y());
  // dmp_clone->analyticalSolution(trajectory.ts(),xs_ana,xds_ana);
  analyticalSolution(trajectory.ts(),xs_ana,xds_ana);
  MatrixXd xs_goal   = xs_ana.GOALM(n_time_steps);
  MatrixXd xs_gating = xs_ana.GATINGM(n_time_steps);
  MatrixXd xs_phase  = xs_ana.PHASEM(n_time_steps);
  
  fa_inputs_phase = xs_phase;
  
  // Get parameters from the spring-dampers system to compute inverse
  double damping_coefficient = spring_system_->damping_coefficient();
  double spring_constant     = spring_system_->spring_constant();
  double mass                = spring_system_->mass();
  if (mass!=1.0)
  {
    cout << "WARNING: Usually, spring-damper system of the DMP should have mass==1, but it is " << mass << endl;
  }

  // Compute inverse
  f_target = tau()*tau()*trajectory.ydds() + (spring_constant*(trajectory.ys()-xs_goal) + damping_coefficient*tau()*trajectory.yds())/mass;
  
  //Factor out gating term
  for (unsigned int dd=0; dd<function_approximators_.size(); dd++)
    f_target.col(dd) = f_target.col(dd).array()/xs_gating.array();
 
}
コード例 #28
0
ファイル: Atomtype.cpp プロジェクト: wesbarnett/tpi
double Atomtype::CalcPE(int frame_i, const Trajectory &trj, const coordinates &rand_xyz, const cubicbox_m256 &box, double vol) const
{
    float pe = 0.0;
    int atom_i = 0;

    /* BEGIN SIMD SECTION */
    // This performs the exact same calculation after the SIMD section
    // but doing it on 8 atoms at a time using SIMD instructions.

    coordinates8 rand_xyz8(rand_xyz), atom_xyz;
    __m256 r2_8, mask, r6, ri6, pe_tmp;
    __m256 pe_sum = _mm256_setzero_ps();
    float result[n] __attribute__((aligned (16)));

    for (; atom_i < this->n-8; atom_i+=8)
    {
        atom_xyz = trj.GetXYZ8(frame_i, this->name, atom_i);
        r2_8 = distance2(atom_xyz, rand_xyz8, box);
        mask = _mm256_cmp_ps(r2_8, rcut2_8, _CMP_LT_OS);
        r6 = _mm256_and_ps(mask, _mm256_mul_ps(_mm256_mul_ps(r2_8, r2_8), r2_8));
        ri6 = _mm256_and_ps(mask, _mm256_rcp_ps(r6));
        pe_tmp = _mm256_and_ps(mask, _mm256_mul_ps(ri6, _mm256_sub_ps(_mm256_mul_ps(c12_8, ri6), c6_8)));
        pe_sum = _mm256_add_ps(pe_tmp, pe_sum);
    }
    _mm256_store_ps(result, pe_sum);
    for (int i = 0; i < 8; i++)
    {
        pe += result[i];
    }

    /* END SIMD SECTION */

    for (; atom_i < this->n; atom_i++)
    {
        coordinates atom_xyz = trj.GetXYZ(frame_i, this->name, atom_i);
        float r2 = distance2(atom_xyz, rand_xyz, cubicbox(box));
        if (r2 < this->rcut2)
        {
            float ri6 = 1.0/(pow(r2,3));
            pe += ri6*(this->c12*ri6 - this->c6);
        }
    }

    pe += this->n/vol * this->tail_factor;;

    return pe;
}
コード例 #29
0
bool CalibrateAction::checkTrajectory(Trajectory& traj)
{
    unsigned int size = traj.getPointsSize();
    float smallest_dist = INFINITY;
    bool isValid;
    for(unsigned int i = 0; i< size;i++)
    {
        geometry_msgs::Pose tempPose;
        tf::Pose tfPose;
        traj.getPoint(i,tempPose.position.x, tempPose.position.y, tempPose.orientation.z);
        tf::poseMsgToTF(tempPose, tfPose);
        float dist = getDistanceAtPose(tfPose, &isValid);
        // to be able to inform about closest distance
        if (dist < smallest_dist)
        {
            smallest_dist = dist;
        }
        if ((dist < traj_dist_threshold) && (dist != -INFINITY) && (dist != INFINITY)) // prevent negative infinity value from crashing us
        {
            ROS_INFO("Critical distance was %f at point %i, point valid: %i", dist, i, isValid);
            return false;
        }
    }
    //ROS_INFO("Trajectory check successful, smallest distance was: %f", smallest_dist);
    return true;

    /*
    unsigned int size = traj.getPointsSize();
   float smallest_dist = INFINITY;
    for(unsigned int i = 0; i< size;i++)
    {
        geometry_msgs::Pose testPose;
        traj.getPoint(i,testPose.position.x, testPose.position.y, testPose.orientation.z);
        float dist = voronoi_.getDistance(testPose.position.x, testPose.position.y); // get closest distance from trajectory point to an obstacle
        dist *= costmap_.getResolution(); // distance now in meters
        if (dist < smallest_dist)
        {
            smallest_dist = dist;
        }
        if ((dist < traj_dist_threshold) && (dist != -INFINITY) && (dist != INFINITY)) // prevent negative infinity value from crashing us
        {
            ROS_INFO("Critical distance was %f at point %i", dist, i);
            return false;
        }
    }
    */
}
コード例 #30
0
bool IsTailNumber(Trajectory &trajectory)
{
  std::string s = trajectory.object_id();
  if ((s[0] != 'N') || (s[1] < '0') || (s[1] > '9'))
    return false;

  return true;
}