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;
}
Trajectory Trajectory::generatePolynomialTrajectory(const VectorXd& ts, const VectorXd& y_from, const VectorXd& yd_from, const VectorXd& ydd_from,
const VectorXd& y_to, const VectorXd& yd_to, const VectorXd& ydd_to)
{
VectorXd a0 = y_from;
VectorXd a1 = yd_from;
VectorXd a2 = ydd_from / 2;
VectorXd a3 = -10 * y_from - 6 * yd_from - 2.5 * ydd_from + 10 * y_to - 4 * yd_to + 0.5 * ydd_to;
VectorXd a4 = 15 * y_from + 8 * yd_from + 2 * ydd_from - 15 * y_to + 7 * yd_to - ydd_to;
VectorXd a5 = -6 * y_from - 3 * yd_from - 0.5 * ydd_from + 6 * y_to - 3 * yd_to + 0.5 * ydd_to;
int n_time_steps = ts.size();
int n_dims = y_from.size();
MatrixXd ys(n_time_steps,n_dims), yds(n_time_steps,n_dims), ydds(n_time_steps,n_dims);
for (int i = 0; i < ts.size(); i++)
{
double t = (ts[i] - ts[0]) / (ts[n_time_steps - 1] - ts[0]);
ys.row(i) = a0 + a1 * t + a2 * pow(t, 2) + a3 * pow(t, 3) + a4 * pow(t, 4) + a5 * pow(t, 5);
yds.row(i) = a1 + 2 * a2 * t + 3 * a3 * pow(t, 2) + 4 * a4 * pow(t, 3) + 5 * a5 * pow(t, 4);
ydds.row(i) = 2 * a2 + 6 * a3 * t + 12 * a4 * pow(t, 2) + 20 * a5 * pow(t, 3);
}
yds /= (ts[n_time_steps - 1] - ts[0]);
ydds /= pow(ts[n_time_steps - 1] - ts[0], 2);
return Trajectory(ts, ys, yds, ydds);
}
Trajectory Trajectory::generateMinJerkTrajectory(const VectorXd& ts, const VectorXd& y_from, const VectorXd& y_to)
{
int n_time_steps = ts.size();
int n_dims = y_from.size();
MatrixXd ys(n_time_steps,n_dims), yds(n_time_steps,n_dims), ydds(n_time_steps,n_dims);
double D = ts[n_time_steps-1];
ArrayXd tss = (ts/D).array();
ArrayXd A = y_to.array()-y_from.array();
for (int i_dim=0; i_dim<n_dims; i_dim++)
{
// http://noisyaccumulation.blogspot.fr/2012/02/how-to-decompose-2d-trajectory-data.html
ys.col(i_dim) = y_from[i_dim] + A[i_dim]*( 6*tss.pow(5) -15*tss.pow(4) +10*tss.pow(3));
yds.col(i_dim) = (A[i_dim]/D)*( 30*tss.pow(4) -60*tss.pow(3) +30*tss.pow(2));
ydds.col(i_dim) = (A[i_dim]/(D*D))*(120*tss.pow(3) -180*tss.pow(2) +60*tss );
}
return Trajectory(ts,ys,yds,ydds);
}
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);
}
bool CActions::AttackNearest(int unit){ // Attack nearby enemies...
const UnitDef* ud = G->GetUnitDef(unit);
if(ud == 0) return false;
if(G->positions.empty()==false){
float3 p = G->GetUnitPos(unit);
if(G->Map->CheckFloat3(p)==false){
return false;
}
// iterate through them all generating a score, if the score is the highest encountered
float highest_score=0.01f;
int target=0;
float tscore=0;
float3 tpos=UpVector;
int frame= G->GetCurrentFrame()-10;
bool mobile = false;
for(map<int,Global::temp_pos>::const_iterator i = G->positions.begin(); i != G->positions.end(); ++i){
if(i->second.enemy == false){
continue;
}
if(i->second.last_update < frame){
continue;
}
float3 pos = i->second.pos;
float d = pos.distance2D(p);
const UnitDef* ude = G->GetUnitDef(i->first);
if(ude != 0){
float eff = G->GetTargettingWeight(ud->name,ude->name);
tscore = eff/max(d,0.1f);
if(tscore > highest_score){
if(ud->highTrajectoryType == 2){
if((ude->movedata != 0)||(ude->canfly)){
if(G->info->hardtarget == false) tscore = tscore/4;
mobile = true;
}else{
mobile = false;
}
}
target = i->first;
tpos =pos;
highest_score=tscore;
tscore=0;
continue;
}
}
}
//
if(target > 0){
if(ud->highTrajectoryType == 2){
Trajectory(unit,(int)mobile);
}
return MoveToStrike(unit,tpos);
}
}
return false;
}
Trajectory PlanningProblem::GRRTsolve()
{
this->planningResult = false;
randomTree.clear();
float start_time = currentTimeMSec();
int tryExtensionCounter = 0;
randomTree.appendNewStation(NULL, initialState);
for(int step=0; step < MAX_RRT_STEP_TRY/5; step++)
{
Station target;
float toss = uni_rand(0, 1);
if(toss < GOAL_PROB)
target.setPosition(goal.goal_point.getPosition());
else {
Station tempSt = SampleStateUniform();
target.setPosition(tempSt.getPosition());
}
if( !target.isValid() )
continue;
// throw "can not sampled!";
SpatialVertex* near_ver = randomTree.getNearestVertex(target);
if(near_ver == NULL)
continue;
int greedyCounter = 0;
while(greedyCounter < 5){
tryExtensionCounter ++;
Station extended = RRTExtend(near_ver->state, target, agent->radius() * 2);
if(!extended.isValid())
break;
randomTree.appendNewStation(near_ver, extended);
if(Station::dubinDistance(extended, target) < agent->radius() ) {
if((target.getPosition() - goal.goal_point.getPosition()).lenght2D() < agent->radius() /2)
planningResult = true;
break;
}
// if(target != goal.goal_point) break;
greedyCounter ++;
}
cout << "Step = " << step << endl;
}
if(planningResult)
{
float finish_time = currentTimeMSec();
this->planningTime = finish_time - start_time;
// cout << "Greedy RRT Planning succeed in " << planningTime << "mili seconds" << endl;
return buildTrajectoryFromRandomTree();
}
return Trajectory();
}
Trajectory Trajectory::readFromFile(string filename, int n_dims_misc)
{
MatrixXd traj_matrix;
if (!loadMatrix(filename,traj_matrix))
{
cerr << __FILE__ << ":" << __LINE__ << ":";
cerr << "Cannot open filename '"<< filename <<"'." << endl;
return Trajectory();
}
int n_dims = (traj_matrix.cols()-1-n_dims_misc)/3;
int n_time_steps = traj_matrix.rows();
VectorXd ts;
MatrixXd ys, yds, ydds, misc;
ts = traj_matrix.block(0,0+0*n_dims,n_time_steps,1);
ys = traj_matrix.block(0,1+0*n_dims,n_time_steps,n_dims);
yds = traj_matrix.block(0,1+1*n_dims,n_time_steps,n_dims);
ydds = traj_matrix.block(0,1+2*n_dims,n_time_steps,n_dims);
misc = traj_matrix.block(0,1+3*n_dims,n_time_steps,n_dims_misc);
return Trajectory(ts,ys,yds,ydds,misc);
}
bool CActions::AttackNear(int unit,float LOSmultiplier){
NLOG("CActions::AttackNear");
const UnitDef* ud = G->GetUnitDef(unit);
if(ud == 0) return false;
int* en = new int[5000];
int e = G->GetEnemyUnits(en,G->GetUnitPos(unit),max(G->cb->GetUnitMaxRange(unit),ud->losRadius)*LOSmultiplier);
if(e>0){
float best_score = 0;
int best_target = 0;
float tempscore = 0;
bool mobile = true;
for(int i = 0; i < e; i++){
if(en[i] < 1) continue;
const UnitDef* endi = G->GetEnemyDef(en[i]);
if(endi == 0){
continue;
}else{
bool tmobile = false;
tempscore = G->GetTargettingWeight(ud->name,endi->name);
if((endi->movedata != 0)||(endi->canfly)){
if(G->info->hardtarget == false) tempscore = tempscore/4;
tmobile = true;
}else if(endi->weapons.empty()==false){
tempscore /= 2;
}
if(tempscore > best_score){
best_score = tempscore;
best_target = en[i];
mobile = tmobile;
}
tempscore = 0;
}
}
if(ud->highTrajectoryType == 2){
Trajectory(unit,(int)mobile);
}
if(best_target > 0){
delete [] en;
return Attack(unit,best_target,mobile);
}
delete [] en;
return true;
}else{
delete [] en;
return false;
}
}
// *************************************************
// ************** under construction ***************
// *************************************************
Trajectory PlanningProblem::RRTConnectSolve(double arg1)
{
double start_time = currentTimeMSec();
randomTree.clear();
backward_tree.clear();
randomTree.appendNewStation(NULL, initialState);
backward_tree.appendNewStation(NULL, goal.goal_point);
for(uint i=0; i< MAX_RRT_STEP_TRY ; ++i)
{
Station randSt = SampleStateUniform();
if(!randSt.isValid())
continue;
SpatialVertex* near_ver = randomTree.getNearestVertex(randSt);
if(near_ver == NULL)
continue;
Station extended_ = RRTExtend(near_ver->state, randSt, arg1);
if(!extended_.isValid())
continue;
randomTree.appendNewStation(near_ver, extended_);
for(int j=0; j<10; j++) {
SpatialVertex* back_near = backward_tree.getNearestVertex(extended_);
Station back_extended = RRTExtend(back_near->state, extended_, arg1);
if(!back_extended.isValid())
break;
backward_tree.appendNewStation(back_near, back_extended);
if(Station::euclideanDistance(back_extended, extended_) < agent->radius()) {
while(back_near != NULL) {
Station on_back_tree_st = back_near->state;
on_back_tree_st.setPosition(Vector3D(back_near->state.getPosition().to2D(),
back_near->state.getPosition().Teta() + M_PI));
randomTree.appendNewStation(randomTree.lastAddedVertex(), on_back_tree_st);
back_near = back_near->parent;
}
planningTime = currentTimeMSec() - start_time;
return buildTrajectoryFromRandomTree();
}
}
}
planningTime = currentTimeMSec() - start_time;
return Trajectory();
}
Trajectory PlanningProblem::RRTsolve(float arg1, float max_len)
{
// if(tree.count() > MAX_TREE_SIZE * 0.75)
randomTree.clear();
this->planningResult = false;
double start_time = currentTimeMSec();
randomTree.appendNewStation(NULL, initialState);
for(uint i=0; i< MAX_RRT_STEP_TRY ; ++i)
{
if(RRTStep(arg1, max_len) == eReached)
{
this->planningResult = true;
double finish_time = currentTimeMSec();
this->planningTime = finish_time - start_time;
return buildTrajectoryFromRandomTree();
break;
}
}
return Trajectory();
}
Trajectory RobotControl::adaptTrajectory(Trajectory aTrajectory)
{
return Trajectory(Vector(aTrajectory.position.y - (1400 / 2), aTrajectory.position.z - 350, 0), aTrajectory.time);
}
/**
* @function followDualTrajectory
*/
bool BaseDualControl::followDualTrajectory( const std::list<Eigen::VectorXd> &_pathLeft,
const std::list<Eigen::VectorXd> &_pathRight,
const Eigen::VectorXd &_maxAccel,
const Eigen::VectorXd &_maxVel ) {
std::list<Eigen::VectorXd> path[2];
path[0] = _pathLeft;
path[1] = _pathRight;
// Safety check
while( !this->update() ) {}
for( int i = 0; i < 2; ++i ) {
if( ( *(path[i].begin()) - mBc[i].get_q() ).norm() > mDq_thresh ) {
printf("\t [followTrajectory] First point of trajectory [%d] is too far from init path point \n", i);
return false;
}
}
// Create trajectory
Trajectory trajectory[2] = { Trajectory(Path(path[0], mMaxDev), _maxVel, _maxAccel),
Trajectory(Path(path[1], mMaxDev), _maxVel, _maxAccel) };
double duration[2];
double maxDuration;
for( size_t i = 0; i < 2; ++i ) {
//trajectory[i] = Trajectory( Path(path[i], mMaxDev), _maxVel, _maxAccel );
trajectory[i].outputPhasePlaneTrajectory();
if( !trajectory[i].isValid() ) {
printf("\t [followTrajectory] ERROR: Trajectory is not valid \n");
return false;
}
duration[i] = trajectory[i].getDuration();
}
if( duration[0] > duration[1] ) { maxDuration = duration[0]; } else { maxDuration = duration[1]; }
printf("Duration left: %f Duration right: %f \n", duration[0], duration[1] );
double start_time;
double current_time;
Eigen::VectorXd vel_cmd[2];
Eigen::VectorXd zeros(mN); for( int i = 0; i < mN; ++i ) { zeros(i) = 0; }
printf("\t [followDualTraj] DEBUG: Max. duration of trajectory: %f \n", maxDuration );
// Update current state and time
double tn;
while( !this->update() ) {}
start_time = mNow.tv_sec + (mNow.tv_nsec)/1.0e9;
current_time = start_time;
tn = current_time - start_time;
// Send velocity commands
while( tn < maxDuration ) {
// Get current time and state
while( !this->update() ) {}
current_time = mNow.tv_sec + mNow.tv_nsec / (1.0e9);
tn = current_time - start_time;
// Build velocity command
for( int i = 0; i < 2; ++i ) {
if( tn < duration[i] ) {
vel_cmd[i] = trajectory[i].getVelocity( tn );
//std::cout << "["<<i<<": "<< tn << "]: Sent velocity"<< vel_cmd[i].transpose() << std::endl;
//std::cout << " Expected pos: "<< trajectory[i].getPosition(tn).transpose() << std::endl;
//std::cout << "Current pos: " << mq.transpose() << std::endl;
//std::cout << "Current velocity: " << mdq.transpose() << std::endl;
// Send command to robot
if( !mBc[i].control_n( mN, vel_cmd[i], 2*mDt, mBc[i].get_refChan(), SNS_MOTOR_MODE_VEL ) ) {
printf("\t[followTrajectory - %d] Sending vel msg error \n", i);
return false;
}
} else {
//printf("Sending 0 vel for arm %d \n", i);
mBc[i].control_n( mN, zeros, mDt, mBc[i].get_refChan(), SNS_MOTOR_MODE_VEL );
}
} // end for
// Sleep and clean up
usleep( (useconds_t)(1e6*mDt)) ;
aa_mem_region_local_release();
} // end while
printf("\t * [trajectoryFollowing] DEBUG: Finish and sending zero vel for good measure \n");
for( size_t i = 0; i < 2; ++i ) {
mBc[i].control_n( mN, zeros, mDt, mBc[i].get_refChan(), SNS_MOTOR_MODE_VEL );
}
return true;
}
Trajectory PlanningProblem::ERRTsolve()
{
cout << "ERRT method is not implemented yet!!!" << endl;
assert(0);
return Trajectory();
}
