void PlanningProblem::computeCost(Trajectory &plan_)
{
if(!this->checkPlanValidity(plan_)) {
plan_.cost.safety = INFINITY;
plan_.cost.smoothness = INFINITY;
plan_.cost.length = INFINITY;
}
else {
plan_.computeCost();
plan_.cost.safety = 0;
ObstacleSet desired_ob_set = stat_obstacles;
for(int i=0; i< plan_.length(); i++) {
Station st = plan_.getStation(i);
float min_dist = INFINITY;
b2Vec2 tmpv1, tmpv2;
for(int j=0; j<desired_ob_set.size(); j++) {
Obstacle* ob = desired_ob_set[j];
float dist_ = distToObstacle(st, *ob, tmpv1, tmpv2);
min_dist = min(min_dist, dist_);
}
st.cost.min_dist_to_obs = min_dist;
plan_.EditStation(i, st);
plan_.cost.safety += plan_.getStation(i).cost.safety_penalty() / plan_.length();
}
}
}
Station PlanningProblem::getNextStation(const Station &st, Trajectory &path)
{
if(path.isEmpty())
return this->goal.goal_point;
if(path.length() <= 2)
return path.getLastStation();
float max_memberance = 0;
int nearest_segment_index = -1;
for(uint i=1; i<path.length(); i++) { // it doesnt use the last segment (go towards goal on last segment)
Vector2D pnt_1 = path.getStation(i-1).getPosition().to2D();
Vector2D pnt_2 = path.getStation(i).getPosition().to2D();
float dist_st_segment = (st.getPosition().to2D() - pnt_1).lenght() +
(st.getPosition().to2D() - pnt_2).lenght();
float segment_len = (pnt_1 - pnt_2).lenght();
float segment_mem = segment_len /dist_st_segment;
if(segment_mem > max_memberance) {
max_memberance = segment_mem;
nearest_segment_index = i;
}
}
return path.getStation(nearest_segment_index);
}
bool PlanningProblem::replan(const ObstacleSet &ob_set, Trajectory &trajec)
{
if(trajec.length() < 2)
return false;
Vector2D goals_diff = (goal.goal_point.getPosition() - trajec.getLastStation().getPosition()).to2D();
if(goals_diff.lenght() > agent->radius())
return false;
Vector2D inits_dist = (initialState.getPosition() - trajec.getFirstStation().getPosition()).to2D();
if(inits_dist.lenght() > agent->radius())
return false;
trajec.EditStation(0, initialState);
trajec.EditStation(trajec.length() -1, goal.goal_point);
return true;
}
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;
}
Trajectory PlanningProblem::PruneTrajectory(Trajectory &input_plan, const ObstacleSet& ob_set)
{
if(input_plan.length() < 3)
return input_plan;
Trajectory prunned_plan;
int st_index = 1;
prunned_plan.appendState(input_plan.getStation(0));
while((input_plan.length() - st_index) > 1) {
Station st_A = prunned_plan.getLastStation();
Station st_B = input_plan.getStation(st_index +1);
if(pathHasCollision(st_A, st_B, ob_set)) {
Station new_inserted_st = input_plan.getStation(st_index);
float new_teta = (new_inserted_st.getPosition().to2D() -
prunned_plan.getLastStation().getPosition().to2D()).arctan();
new_inserted_st.setPosition(Vector3D(new_inserted_st.getPosition().to2D(), new_teta));
prunned_plan.appendState(new_inserted_st);
}
st_index ++;
}
prunned_plan.appendState(input_plan.getLastStation());
return prunned_plan;
// for(int i=0; i< p.length(); i++) {
// if(i == 0) {
// opt_plan.appendState(p.getStation(0));
// continue;
// }
// Station _st = p.getStation(i);
// float min_dist_to_ob;
// b2Vec2 st_colid_point;
// b2Vec2 ob_colid_point;
// Obstacle* ob_ = nearestObstacle(_st, stat_obstacles, min_dist_to_ob, st_colid_point, ob_colid_point);
// if(ob_ != NULL && min_dist_to_ob < agent->radius() * 1.5) {
// Vector2D bad_direc = (Vector2D(ob_colid_point) - Vector2D(st_colid_point)).normalized();
// _st.setPosition(_st.getPosition() - bad_direc.to3D() * 0.5);
// }
// if(CheckValidity(_st))
// opt_plan.appendState(_st);
// else
// opt_plan.appendState(p.getStation(i));
// }
}
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);
}
}
}
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();
}
void Trajectory::append(const Trajectory& trajectory)
{
assert(dim() == trajectory.dim());
assert(ts_[length() - 1] == trajectory.ts()[0]);
if (ys_.row(length() - 1).isZero() || trajectory.ys().row(0).isZero())
assert(ys_.row(length() - 1).isZero() && trajectory.ys().row(0).isZero());
else
assert(ys_.row(length() - 1).isApprox(trajectory.ys().row(0)));
if (yds_.row(length() - 1).isZero() || trajectory.yds().row(0).isZero())
assert(yds_.row(length() - 1).isZero() && trajectory.yds().row(0).isZero());
else
assert(yds_.row(length() - 1).isApprox(trajectory.yds().row(0)));
if (ydds_.row(length() - 1).isZero() || trajectory.ydds().row(0).isZero())
assert(ydds_.row(length() - 1).isZero() && trajectory.ydds().row(0).isZero());
else
assert(ydds_.row(length() - 1).isApprox(trajectory.ydds().row(0)));
int new_size = length() + trajectory.length() - 1;
VectorXd new_ts(new_size);
new_ts << ts_, trajectory.ts().segment(1, trajectory.length() - 1);
ts_ = new_ts;
MatrixXd new_ys(new_size, dim());
new_ys << ys_, trajectory.ys().block(1, 0, trajectory.length() - 1, dim());
ys_ = new_ys;
MatrixXd new_yds(new_size, dim());
new_yds << yds_, trajectory.yds().block(1, 0, trajectory.length() - 1, dim());
yds_ = new_yds;
MatrixXd new_ydds(new_size, dim());
new_ydds << ydds_, trajectory.ydds().block(1, 0, trajectory.length() - 1, dim());
ydds_ = new_ydds;
assert(dim_misc() == trajectory.dim_misc());
if (dim_misc()==0)
{
misc_.resize(new_size,0);
}
else
{
MatrixXd new_misc(new_size, dim_misc());
new_misc << misc_, trajectory.misc().block(1, 0, trajectory.length() - 1, dim_misc());
misc_ = new_misc;
}
}
vector<Segment> generateSegments(Trajectory &t, double epsilon, int index){
//generate graph.
vector<vector<int>> g(t.length());
for(int i = 0; i<t.length()-1; i++){
const Point &p = t.points[i];
const Point &q = t.points[i+1];
const Point e1(1,0);
double thetamin;
double thetamax;
double theta = acos((q-p).dotProd(e1)/(rootDistance(p,q)));
double dtheta = atan(epsilon/rootDistance(p,q));
thetamin = theta-dtheta;
thetamax = theta+dtheta;
g[i].push_back(i+1);
int j = i+2;
if (j<t.length()){
theta = acos((t.points[j]-p).dotProd(e1)/(rootDistance(p,t.points[j])));
}
while(j<t.length() && theta>=thetamin && theta<= thetamax){
g[i].push_back(j);
double dtheta = atan(epsilon/rootDistance(p,t.points[j]));
if (thetamin<theta-dtheta) thetamin = theta-dtheta;
if (thetamax>theta+dtheta) thetamax = theta+dtheta;
j++;
if (j<t.length()){
theta = acos((t.points[j]-p).dotProd(e1)/(rootDistance(p,t.points[j])));
}
}
}
//do DFS.
queue<int> Q;
Q.push(0);
bool added[t.length()];
for (int i; i< t.length(); i++) added[i] = false;
added[0]=true;
int dist[t.length()];
for (int i; i< t.length(); i++) dist[i] = t.length();
dist[0] = 0;
int parent[t.length()];
for (int i; i < t.length(); i++) parent[i] = -1;
while(!Q.empty()){
int ver = Q.front();
Q.pop();
for(auto n : g[ver]){
if (!added[n]){
parent[n] = ver;
dist[n] = dist[ver]+1;
added[n] = true;
Q.push(n);
}
}
}
int n = t.length()-1;
vector<Segment> ret;
while(n!=0){
ret.push_back(Segment(t.points[parent[n]],t.points[n], index, n-parent[n]));
n = parent[n];
}
return ret;
}
