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;
}
void XTCReader::read(Trajectory &trajectory) {
while (true) {
SmartPointer<Positions> positions = new Positions;
if (!read(*positions, trajectory.getTopology().get())) break;
trajectory.add(positions);
}
}
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);
}
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);
}
}
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";
}
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);
}
}
}
// --------------------------------------------------------------------------
// 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;
}
// --------------------------------------------------------------------------
// 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;
}
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;
}
/**
* 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);
}
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);
}
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;
}
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"));
}
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");
}
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;
}
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;
}
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++)
{
}
}
}
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);
}
}
}
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;
}
// --------------------------------------------------------------------------
// 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;
}
//*************************************************************
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;
}
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());
}
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 );
}
// 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;
}
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;
}
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;
}
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();
}
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;
}
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;
}
}
*/
}
bool IsTailNumber(Trajectory &trajectory)
{
std::string s = trajectory.object_id();
if ((s[0] != 'N') || (s[1] < '0') || (s[1] > '9'))
return false;
return true;
}
