base::MatrixXd NMPC::calcExpInterpolation(const base::Vector6d ¤t_state, const base::Vector6d &reference) const
{
base::MatrixXd trajectory = Eigen::MatrixXd::Zero(6, kHorizon);
for (uint dof = 0; dof < current_state.size(); dof++)
{
double A; double B; double C;
C = reference[dof];
A = current_state[dof] - C;
if(position_control_)
{
if(dof < 3)
B = (double)-1.5/kHorizon;
else
B = (double)-6.0/kHorizon;
}
else
{
B = (double)-8/kHorizon;
}
for(int horizon = 0; horizon < kHorizon; horizon++)
trajectory(dof, horizon) = A*exp(B*horizon) + C;
}
for(int horizon = 0; horizon < kHorizon; horizon++)
trajectory(4, horizon) = current_state[4] + horizon*(reference[4] - current_state[4])/kHorizon;
return trajectory;
}
TEST(testTrackToFaceBasic, trackFirstFace)
{
Mesh *mesh = readMesh("mesh");
vector<Particle> particles = readParticles("particles", mesh);
// Find Particle with label 1
Particle *particle;
for(int i=0; i<particles.size(); i++) {
if(particles[i].getLabel() == 1) {
particle = &particles[i];
break;
}
}
vector<pair<float, float> > trajectory(particle->getTrajectory());
float lambda;
ASSERT_EQ(1, particle->trackToFaceBasic(0, -1,
trajectory[0].first, trajectory[0].second,
trajectory[1].first, trajectory[1].second,
&lambda)
);
ASSERT_TRUE(lambda >= 0);
ASSERT_TRUE(lambda <= 1);
}
Eigen::MatrixXf BezierQuadraticGenerator::generateGuess(std::vector<double> start, std::vector<double> end, std::vector<double> parameters){
Eigen::Map<Eigen::VectorXd> startVec(start.data(), start.size());
Eigen::Map<Eigen::VectorXd> endVec(end.data(), end.size());
Eigen::Map<Eigen::VectorXd> midVec(parameters.data(), parameters.size());
//Error Checking Crap
// if (startVec.size() != endVec.size() || startVec.size() != midVec.size()){
// std::cout<< "Dimensions for generateGuess are incorrect"<<std::endl;
// std::cout<< "Size of start = " << startVec.size();
// std::cout<< ", Size of mid = " << midVec.size();
// std::cout<< ", Size of end = " << endVec.size();
// std::cout<<std::endl;
// }
Eigen::MatrixXf trajectory(num_waypoints,start.size());
for( int row = 0; row < num_waypoints; row++){
double t = (double) row / (double) (num_waypoints-1);
//Bezier curves are defined as
// B(t) = (1-t)^2*P0 + 2*(1-t)*t*P1 +
Eigen::VectorXd waypoint = ((1-t)*(1-t)*startVec + 2*(1-t)*t*midVec + t*t*endVec);
trajectory.row(row) = waypoint.cast<float>().transpose();
}
return trajectory;
}
void Ball::setTrajectory(QPointF start, QPointF end)
{
QLineF trajectory(start, end);
animation->stop();
animation->setStartValue(start);
animation->setEndValue(end);
animation->setDuration(getDuration());
animation->start();
}
int main(void)
{
char buffer[3];
int selection,done;
done = 0;
while (1)
{
printf("\n\n **** Ballistic Calculator Menu ****\n\n");
printf(" 1. Calculate Trajectory\n");
printf(" 2. Calculate Ballistic Coefficient\n");
printf(" 3. Calculate Distance to Target from Mil-Dots\n");
printf(" 4. Calculate Optimum Twist Rate\n");
printf("\n 99. Exit\n\n");
printf("Enter Selection: ");
sgets(buffer,sizeof buffer);
selection = atoi(buffer);
printf("\n\n");
switch (selection)
{
case 1:
trajectory();
PauseForEnterKey();
break;
case 2:
ballistic_coefficient();
PauseForEnterKey();
break;
case 3:
mildot();
PauseForEnterKey();
break;
case 4:
twist_rate();
PauseForEnterKey();
break;
case 99:
done = 1;
break;
default:
printf("Invalid Entry\n");
break;
}
if (done == 1) break;
}
return 0;
}
void SceneCloudView::addCoordinateSystem(const Eigen::Affine3d& pose, bool keep_last)
{
if(!keep_last && !first_time_)
cloud_viewer_->removeCoordinateSystem();
cloud_viewer_->addCoordinateSystem(0.3, pose.cast<float>());
trajectory("estimate")->color(Color::red()).add(pose);
first_time_ = false;
}
Reach setup_reach(const Config & config, const Protein & protein) {
ModelReduction reduction = load_reduction(config.files.reduction, protein.residue_count());
ProteinSegmentFactory protein_segment_factory;
std::vector<std::shared_ptr<ProteinSegment>> protein_segments = protein_segment_factory.generate_protein_segments_for_analysis(protein);
TrajectoryAnalyzer trajectory_analyzer;
if (config.files.nma_covariance) {
LOGI << "setting up Reach with NMA input";
CSVReader csv_reader;
Eigen::MatrixXd nma_covariance = csv_reader.read_matrix(
*config.files.nma_covariance,
protein.residue_count() * 3);
trajectory_analyzer.analyze(nma_covariance, protein_segments, config.general.temperature);
} else {
LOGI << "setting up Reach with Trajectory";
LOGI << "loading trajectory files";
TrajectoryFileFactory tf;
std::vector<std::shared_ptr<TrajectoryFile>> trajectory_files;
for (auto const & path : config.files.trajectories) {
trajectory_files.push_back(tf.create(path, config.general.crystal_information));
}
Trajectory trajectory(trajectory_files);
trajectory_analyzer.analyze(trajectory,
protein_segments,
config.general.temperature,
config.general.ensemble_size);
}
Reach reach(protein_segments,
config.general.temperature,
config.general.ensemble_size,
config.fitting.average_bin_length,
config.fitting.minimum_length,
config.fitting.maximum_length,
config.fitting.use_slow_fitting,
config.fitting.slow_minimum_length,
config.fitting.slow_maximum_length,
reduction);
return reach;
}
bool KinematicIntegerBands::cd_future_traj(Detection3D* det, double B, double T, bool trajdir, double t,
const TrafficState& ownship, const TrafficState& ac) const {
if (t > T || B > T) return false;
std::pair<Vect3,Velocity> sovot = trajectory(ownship,t,trajdir);
Vect3 sot = sovot.first;
Velocity vot = sovot.second;
Vect3 si = ac.get_s();
Velocity vi = ac.get_v();
Vect3 sit = vi.ScalAdd(t,si);
if (B > t) {
return conflict(det, sot, vot, sit, vi, B-t, T-t);
}
return conflict(det, sot, vot, sit, vi, 0, T-t);
}
bool KinematicIntegerBands::any_los_aircraft(Detection3D* det, bool trajdir, double tsk,
const TrafficState& ownship, const std::vector<TrafficState>& traffic) const {
for (TrafficState::nat i=0; i < traffic.size(); ++i) {
TrafficState ac = traffic[i];
std::pair<Vect3,Velocity> sovot = trajectory(ownship,tsk,trajdir);
Vect3 sot = sovot.first;
Velocity vot = sovot.second;
Vect3 si = ac.get_s();
Velocity vi = ac.get_v();
Vect3 sit = vi.ScalAdd(tsk,si);
if (det->violation(sot, vot, sit, vi))
return true;
}
return false;
}
int main( int argc, char **argv )
{
Trajectory trajectory("TrajectoriesKernel.cl",kTimeMax,kTimeDelta);
if( trajectory.Acquire("Trajectory1") )
{
trajectory.Compute(kTime,kSpeed,kAngle);
trajectory.Log();
} // if
if( trajectory.Acquire("Trajectory2") )
{
trajectory.Compute(kTime,kSpeed,kHeight);
trajectory.Log();
} // if
return 0;
} // main
// Particle stays in the same cell
TEST(testTrackToFaceBasic, trackSame)
{
Mesh *mesh = readMesh("mesh");
vector<Particle> particles = readParticles("particles", mesh);
Particle *particle;
for(int i=0; i<particles.size(); i++) {
if(particles[i].getLabel() == 1) {
particle = &particles[i];
break;
}
}
vector<pair<float, float> > trajectory(particle->getTrajectory());
float lambda;
ASSERT_EQ(-1, particle->trackToFaceBasic(1, -1, 1.5, 3, 2.5, 1.5, &lambda)
);
}
void NMPC::calcTrajectory(const base::Vector6d &reference) const
{
base::Vector6d reference_vctr = base::Vector6d::Zero();
for(int i = 0; i < reference.size(); i++)
{
if(base::isUnset<double>(reference[i]))
reference_vctr[i] = 0;
else
reference_vctr[i] = reference[i];
}
base::Vector6d current_state = base::Vector6d::Zero();
if(position_control_)
{
for(uint i = 0; i < 6; i++)
current_state[i] = acadoVariables.x0[i+6];
}
else
{
for(uint i = 0; i < 6; i++)
current_state[i] = acadoVariables.x0[i];
}
base::MatrixXd trajectory = Eigen::MatrixXd::Zero(6, kHorizon);
if(reference_interpolation_ == NONE)
{
trajectory = calcConstantTrajectory(reference_vctr);
}
else if(reference_interpolation_ == EXPONENTIAL)
{
trajectory = calcExpInterpolation(current_state, reference_vctr);
}
for (int dof = 0; dof < 6; dof++)
{
for (int horizon = 0; horizon < kHorizon; horizon++)
{
acadoVariables.y[ dof + horizon*kNumOutputs ] = trajectory(dof, horizon);
}
acadoVariables.yN[dof] = reference_vctr[dof];
}
}
TEST(testTrackToFaceBasic, anotherTrackingTest)
{
Mesh *mesh = readMesh("mesh");
vector<Particle> particles = readParticles("particles", mesh);
Particle *particle;
for(int i=0; i<particles.size(); i++) {
if(particles[i].getLabel() == 1) {
particle = &particles[i];
break;
}
}
vector<pair<float, float> > trajectory(particle->getTrajectory());
float lambda;
ASSERT_EQ(2, particle->trackToFaceBasic(0, -1, 3.4, 1.79, 3, 3, &lambda));
ASSERT_TRUE(lambda >= 0);
ASSERT_TRUE(lambda <= 1);
}
TEST(testTrackToFaceBasic, trackCase2)
{
Mesh *mesh = readMesh("mesh");
vector<Particle> particles = readParticles("particles", mesh);
Particle *particle;
for(int i=0; i<particles.size(); i++) {
if(particles[i].getLabel() == 2) {
particle = &particles[i];
break;
}
}
vector<pair<float, float> > trajectory(particle->getTrajectory());
float lambda;
int faceHit = particle->trackToFaceBasic
(1, 1, 3.23239, 1.11972, 3.4, 1, &lambda);
ASSERT_EQ(-1, faceHit);
ASSERT_TRUE(lambda >= 0);
ASSERT_TRUE(lambda <= 1);
}
TEST(TestParticleTracker, testTrackParticle2)
{
Mesh *mesh = readMesh("mesh");
vector<Particle> particles = readParticles("particles", mesh);
Particle *particle;
for(int i=0; i<particles.size(); i++) {
if(particles[i].getLabel() == 2) {
particle = &particles[i];
break;
}
}
vector<pair<float, float> > trajectory(particle->getTrajectory());
particle->trackParticle();
vector<pair<int, float> > cellFraction(particle->getCellFraction());
vector<int> stepCells(particle->getStepCells());
ASSERT_TRUE(stepCells.size() == 3);
ASSERT_EQ(6, cellFraction.size());
ASSERT_EQ(2, cellFraction[cellFraction.size()-1].first);
}
int main() {
register int i, j, k;
int ntr = 0, cnt = 0, p0 = 0;
int E0Shift, Corr, MS, MSS;
double RC, h, EKT, pk;
double s, p, q;
double ACT[5];
FILE* fresult;
time_t time_begin;
double time_left;
#ifdef SFMT
init_gen_rand(time(NULL));
#else
srand(time(NULL));
#endif
for (i=1; i<=NN; i++) {
A[i] = 0.;
for (k=1; k<=i; k++) A[i] += 1./k;
}
for (k=1; k<=NN; k++) P[k] = .5/k/A[NN];
printf("Конечное время: %.2f\n", Tmax);
printf("Количество траекторий: %.1e\n", (double) Ntraj);
kappa = V0 * A[NN];
printf("Kappa: %.4f\n", kappa);
m2 = 0.;
for (k=1; k<=NN; k++) m2 += 2*P[k]*k*k;
printf("m2: %.4f\n", m2);
RC = R(Tmax);
printf("Радиус коридора: %.2f\n", RC);
h = (RC + DiamP) / Nmax;
printf("Шаг сетки: %.2f\n", h);
E0Shift = floor(E0*Tmax/h);
printf("Количество узлов сетки в сдвиге от E0: %i\n", E0Shift);
Corr = floor(RC/h);
printf("Число узлов сетки в коридоре: %i\n", Corr);
MS = Corr + floor(DiamP/h) + floor(abs(E0)*Tmax/h);
MSS = 2*MS + 1;
printf("Размерность матрицы, аппроксимирующей разрешающий оператор: %i\n", MSS);
EKT = exp(kappa*Tmax)/Ntraj;
for (i=1; i<=MSS; i++)
for (j=1; j<=MSS; j++)
if (i!=j) Mat[i][j].re = Mat[i][j].im = 0.;
else {
Mat[i][j].re = 1./Ntraj/Ntraj;
Mat[i][j].im = 0.;
}
time_begin = time(NULL);
printf("\nПроцесс выполнения:\n");
while (ntr < Ntraj) {
ntr++;
if ( ntr% (Ntraj/10) == 0 ) {
printf(" %d%%", 100*ntr/Ntraj);
time_left = (time(NULL) - time_begin) / 60. / ntr * (Ntraj-ntr);
if (time_left >= 1.) printf(" [осталось %.2f минут]\n", time_left);
else printf(" [осталось %.0f секунд]\n", time_left*60.);
fflush(stdout);
}
else if ( ntr% (Ntraj/10/10) == 0 ) {
printf("=");
fflush(stdout);
}
trajectory(ACT);
if (ACT[1] > 0.) {
p = ACT[2];
q = ACT[3];
//double delta = p/h - j1;
s = ACT[4];
if (abs(p)>RC) cnt++;
else for (k=1; k<=MSS; k++) {
pk = p0 + (k-MS)*h;
i = (Round(p/h) - E0Shift + k) % MSS;
Mat[k][i].re += //(1 - delta)
cos(s-q*pk+pk*pk*Tmax/2.) * EKT;
Mat[k][i].im += //(1 - delta)
sin(s-q*pk+pk*pk*Tmax/2.) * EKT;
//Mat[k+1][i].re += delta * cos(s-q*pk+pk*pk*Tmax/2.) * EKT;
//Mat[k+1][i].im += delta * sin(s-q*pk+pk*pk*Tmax/2.) * EKT;
}
}
else for (k=1; k<=MSS; k++) {
pk = p0 + (k-MS)*h;
i = (k-E0Shift) % MSS;
Mat[k][i].re += cos( (pk*pk-pk*E0*Tmax+ (E0*Tmax*E0*Tmax) /3. )*Tmax/2. ) * EKT;
Mat[k][i].im += sin( (pk*pk-pk*E0*Tmax+ (E0*Tmax*E0*Tmax) /3. )*Tmax/2. ) * EKT;
}
}
printf("\nВероятность покинуть коридор: %.2f%%\n", (double) cnt/ (double) Ntraj * 100.);
fresult = fopen(result_file,"w");
fprintf(fresult, "{");
for (i=1; i<=MSS; i++) {
fprintf(fresult, "{");
for (j=1; j<=MSS; j++) {
if (fabs(Mat[i][j].im) + fabs(Mat[i][j].re) > 1e-10)
if (Mat[i][j].im < 0)
fprintf(fresult, "%.10f - %.10f*I", Mat[i][j].re, -Mat[i][j].im);
else
fprintf(fresult, "%.10f + %.10f*I", Mat[i][j].re, Mat[i][j].im);
else fprintf(fresult, "0");
if (j<MSS) {
fprintf(fresult, ", ");
}
}
if (i<MSS) fprintf(fresult, "}, ");
else fprintf(fresult, "}");
}
fprintf(fresult, "}");
fclose(fresult);
return 0;
}
SICONOS_EXPORT void controlLaw(double * t, double * q, double * qdot, int * NDOF, int * NCONT, double * torques)
{
int ndof;
int contacts;
char lagrangianModelName[20] = "";
getLagrangianModelName(lagrangianModelName);
getActiveDOFsize(&ndof);
vector<int> activeDOF(ndof);
if (ndof > 0)
getActiveDOF(&(activeDOF[0]));
else
ndof = *NDOF;
///////////////////////////////////////////
// Computation of the Lagrangian model
///////////////////////////////////////////
matrix<double, column_major> M(*NDOF, *NDOF);
vector<double> N(*NDOF);
if (strcmp(lagrangianModelName, "Human36") == 0)
{
double L[31];
double addl[84];
double mass;
GetModelMass(&mass);
GetAnatomicalLengths(L);
GetTag2JointLengths(addl);
InertiaH36(&(M(0, 0)), q, L, addl, mass);
NLEffectsH36(&(N[0]), q, qdot, L, addl, mass);
}
else
{
Inertia(&(M(0, 0)), q);
NLEffects(&(N[0]), q, qdot);
}
//////////////////////////////////////////////////
//Additionnal forces
/////////////////////////////////////////////////
vector<double> fAdditionnal(*NDOF);
if (strcmp(lagrangianModelName, "PA10") == 0)
Friction(&(fAdditionnal[0]), q, qdot);
else if (strcmp(lagrangianModelName, "RX90") == 0)
SpringForce(&(fAdditionnal[0]), q);
else
fAdditionnal.clear();
///////////////////////////////////////////////////
// Limitation to the selected degrees of freedom
///////////////////////////////////////////////////
reduceMatrix(M, activeDOF, activeDOF);
N = reduceVector(N, activeDOF);
fAdditionnal = reduceVector(fAdditionnal, activeDOF);
//////////////////////////////////////////////////
// Proportionnel-derivee in the task space
//////////////////////////////////////////////////
vector<double> sDesired(*NDOF);
vector<double> sdotDesired(*NDOF);
matrix<double, column_major> sddotDesiredMATRIX(*NDOF, 1);
matrix_column<matrix<double, column_major> > sddotDesired(sddotDesiredMATRIX, 1);
//vector<double> sddotDesired(*NDOF);
trajectory(t, &(sDesired[0]), &(sdotDesired[0]), &(sddotDesired[0]), &contacts);
//v = Kp*(s_desiree-TaskFunction(q))+Kv*(sdot_desiree-H*qdot)-h+sddot_desiree;
vector<double> TF(*NDOF);
vector<double> h(*NDOF);
matrix<double, column_major> H(*NDOF, *NDOF);
TaskFunction(&(TF[0]), q);
TaskJacobian(&(H(0, 0)), q);
TaskNLEffects(&(h[0]), q, qdot);
double Kp = 100.0;
double Kv = 30.0;
vector<double, array_adaptor<double> > tmp_cast(*NDOF, array_adaptor<double> (*NDOF, qdot));
sddotDesired += Kp * (sDesired - TF) + Kv * (sdotDesired - prod(H, tmp_cast)) - h;
/////////////////////////////////////////////////////////////
// Generalized Torques to make the realisation of the command
/////////////////////////////////////////////////////////////
//qddot = inv(H)*v;
vector<int> ipiv(*NDOF);
// lapack::getrf(H, ipiv);
// sddotDesired = prod(H, sddotDesired);
lapack::gesv(H, ipiv, sddotDesiredMATRIX);
//D = M*qddot(ActiveDOF) + N + FAdditionnal;
sddotDesired = reduceVector(sddotDesired, activeDOF);
N += prod(M, sddotDesired) + fAdditionnal;
//nmot = sum(ActiveDOF<=size(q, 1)); en fait ici nmot = ndof
//Torques = zeros(q);
//Torques(ActiveDOF(1:nmot)) = D(1:nmot);
for (int i = 0; i < *NDOF; i++)
torques[i] = 0;
expandVector(torques, N, activeDOF);
}
int main( int, char** ){
/*
mlockall(MCL_CURRENT | MCL_FUTURE);
RT_TASK task;
rt_task_shadow( &task, "mtsTrajectoryExample", 20, 0 );
*/
mtsTaskManager* taskManager = mtsTaskManager::GetInstance();
cmnLogger::SetMask( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL );
mtsKeyboard kb;
kb.SetQuitKey( 'q' );
kb.AddKeyWriteFunction( 'E', "Enable", "Enable", true );
taskManager->AddComponent( &kb );
vctMatrixRotation3<double> Rw0( 0.0, 0.0, -1.0,
0.0, 1.0, 0.0,
1.0, 0.0, 0.0 );
vctFixedSizeVector<double,3> tw0(0.0);
vctFrame4x4<double> Rtw0( Rw0, tw0 );
vctDynamicVector<double> qinit( 7, 0.0 );
qinit[1] = -cmnPI_2;
qinit[3] = cmnPI;
qinit[5] = -cmnPI_2;
cmnPath path;
path.AddRelativeToCisstShare("/models/WAM");
std::string fname = path.Find("wam7.rob");
mtsTrajectory trajectory( "trajectory",
0.01,
fname,
Rtw0,
qinit );
taskManager->AddComponent( &trajectory );
SetPoints setpoints( fname, Rtw0, qinit );
taskManager->AddComponent( &setpoints );
taskManager->Connect( trajectory.GetName(), "Control",
kb.GetName(), "Enable" );
taskManager->Connect( trajectory.GetName(), "Input",
setpoints.GetName(), "Output" );
taskManager->CreateAll();
taskManager->StartAll();
pause();
taskManager->KillAll();
taskManager->Cleanup();
return 0;
}
//--------------------------------------------------------------
void ofApp::draw(){
if (path_est.size() > 0)
{
cam.begin();
ofPushStyle();
/// Render points as 3D cubes
for (size_t i = 0; i < point_cloud_est.size(); ++i)
{
cv::Vec3d point = point_cloud_est[i];
cv::Affine3d point_pose(cv::Mat::eye(3, 3, CV_64F), point);
char buffer[50];
sprintf(buffer, "%d", static_cast<int>(i));
ofBoxPrimitive box;
ofSetLineWidth(2.0f);
ofSetColor(ofColor::blue);
box.set(0.1, 0.1, -0.1);
box.setPosition(point[0], point[1], point[2]);
box.drawWireframe();
}
ofPopStyle();
cam.end();
cv::Affine3d cam_pose = path_est[idx];
cv::Matx44d mat44 = cam_pose.matrix;
ofMatrix4x4 m44(mat44(0, 0), mat44(1, 0), mat44(2, 0), mat44(3, 0),
mat44(0, 1), mat44(1, 1), mat44(2, 1), mat44(3, 1),
mat44(0, 2), mat44(1, 2), mat44(2, 2), mat44(3, 2),
mat44(0, 3), mat44(1, 3), mat44(2, 3), mat44(3, 3));
if (camera_pov) {
cam.setPosition(m44.getTranslation());
cam.lookAt(ofVec3f(0,0,1)*m44, ofVec3f(mat44(1,0),mat44(1,1), mat44(1,2)));
}
else
{
std::vector<ofPoint> path;
for (int i = 0; i < path_est.size()-1; i++) {
cv::Vec3d point = path_est[i].translation();
path.push_back(ofPoint(point[0], point[1], point[2]));
}
ofPolyline trajectory(path);
// render complete trajectory
cam.begin();
ofSetColor(ofColor::green);
trajectory.draw();
ofSetColor(ofColor::yellow);
ofPushMatrix();
ofMultMatrix(m44);
ofDrawAxis(0.25);
drawFrustum(f, cx, cy, 0.025, 0.4);
ofPopMatrix();
cam.end();
}
// update trajectory index (spring effect)
forw *= (idx == n-1 || idx == 0) ? -1 : 1; idx += forw;
}
}
inline Velocity<Barycentric> Celestial::current_velocity(
Instant const& current_time) const {
CHECK(is_initialized());
return trajectory().EvaluateVelocity(current_time, current_time_hint());
}
inline Position<Barycentric> Celestial::current_position(
Instant const& current_time) const {
CHECK(is_initialized());
return trajectory().EvaluatePosition(current_time, current_time_hint());
}
inline DegreesOfFreedom<Barycentric> Celestial::current_degrees_of_freedom(
Instant const& current_time) const {
CHECK(is_initialized());
return trajectory().EvaluateDegreesOfFreedom(current_time,
current_time_hint());
}
void Ephemeris<Frame>::ComputeApsides(not_null<MassiveBody const*> const body1,
not_null<MassiveBody const*> const body2,
DiscreteTrajectory<Frame>& apoapsides1,
DiscreteTrajectory<Frame>& periapsides1,
DiscreteTrajectory<Frame>& apoapsides2,
DiscreteTrajectory<Frame>& periapsides2) {
not_null<ContinuousTrajectory<Frame> const*> const body1_trajectory =
trajectory(body1);
not_null<ContinuousTrajectory<Frame> const*> const body2_trajectory =
trajectory(body2);
typename ContinuousTrajectory<Frame>::Hint hint1;
typename ContinuousTrajectory<Frame>::Hint hint2;
// Computes the derivative of the squared distance between |body1| and |body2|
// at time |t|.
auto const evaluate_square_distance_derivative =
[body1_trajectory, body2_trajectory, &hint1, &hint2](
Instant const& t) -> Variation<Square<Length>> {
DegreesOfFreedom<Frame> const body1_degrees_of_freedom =
body1_trajectory->EvaluateDegreesOfFreedom(t, &hint1);
DegreesOfFreedom<Frame> const body2_degrees_of_freedom =
body2_trajectory->EvaluateDegreesOfFreedom(t, &hint2);
RelativeDegreesOfFreedom<Frame> const relative =
body1_degrees_of_freedom - body2_degrees_of_freedom;
return 2.0 * InnerProduct(relative.displacement(), relative.velocity());
};
std::experimental::optional<Instant> previous_time;
std::experimental::optional<Variation<Square<Length>>>
previous_squared_distance_derivative;
for (Instant time = t_min(); time <= t_max(); time += parameters_.step()) {
Variation<Square<Length>> const squared_distance_derivative =
evaluate_square_distance_derivative(time);
if (previous_squared_distance_derivative &&
Sign(squared_distance_derivative) !=
Sign(*previous_squared_distance_derivative)) {
CHECK(previous_time);
// The derivative of |squared_distance| changed sign. Find its zero by
// bisection, this is the time of the apsis. Then compute the apsis and
// append it to one of the output trajectories.
Instant const apsis_time = Bisect(evaluate_square_distance_derivative,
*previous_time,
time);
DegreesOfFreedom<Frame> const apsis1_degrees_of_freedom =
body1_trajectory->EvaluateDegreesOfFreedom(apsis_time, &hint1);
DegreesOfFreedom<Frame> const apsis2_degrees_of_freedom =
body2_trajectory->EvaluateDegreesOfFreedom(apsis_time, &hint2);
if (Sign(squared_distance_derivative).Negative()) {
apoapsides1.Append(apsis_time, apsis1_degrees_of_freedom);
apoapsides2.Append(apsis_time, apsis2_degrees_of_freedom);
} else {
periapsides1.Append(apsis_time, apsis1_degrees_of_freedom);
periapsides2.Append(apsis_time, apsis2_degrees_of_freedom);
}
}
previous_time = time;
previous_squared_distance_derivative = squared_distance_derivative;
}
}
AlphaReal TreeLearnerUCT::run()
{
if ( _numOfCalling == 0 ) {
if (_verbose > 0) {
cout << "Initializing tree..." << endl;
}
InnerNodeUCTSparse::setDepth( _numBaseLearners );
InnerNodeUCTSparse::setBranchOrder( _pTrainingData->getNumAttributes() );
_root.setChildrenNum();
//createUCTTree();
}
_numOfCalling++;
set< int > tmpIdx, idxPos, idxNeg;
_pTrainingData->clearIndexSet();
for( int i = 0; i < _pTrainingData->getNumExamples(); i++ ) tmpIdx.insert( i );
vector< int > trajectory(0);
_root.getBestTrajectory( trajectory );
// for UCT
for(int ib = 0; ib < _numBaseLearners; ++ib)
_baseLearners[ib]->setTrainingData(_pTrainingData);
AlphaReal edge = numeric_limits<AlphaReal>::max();
BaseLearner* pPreviousBaseLearner = 0;
//floatBaseLearner tmpPair, tmpPairPos, tmpPairNeg;
// for storing the inner point (learneres) which will be extended
//vector< floatBaseLearner > bLearnerVector;
InnerNodeType innerNode;
priority_queue<InnerNodeType, deque<InnerNodeType>, greater_first<InnerNodeType> > pq;
//train the first learner
//_baseLearners[0]->run();
pPreviousBaseLearner = _baseLearners[0]->copyState();
dynamic_cast<FeaturewiseLearner*>(pPreviousBaseLearner)->run( trajectory[0] );
//this contains the number of baselearners
int ib = 0;
NodePointUCT tmpNodePoint, nodeLeft, nodeRight;
////////////////////////////////////////////////////////
//set the edge
//tmpPair.first = getEdge( pPreviousBaseLearner, _pTrainingData );
//tmpPair.second.first.first = pPreviousBaseLearner;
// set the pointer of the parent
//tmpPair.second.first.second.first = 0;
// set that this is a neg child
//tmpPair.second.first.second.second = 0;
//tmpPair.second.second = tmpIdx;
//bLearnerVector = calculateChildrenAndEnergies( tmpPair );
///
pPreviousBaseLearner->setTrainingData( _pTrainingData );
tmpNodePoint._edge = pPreviousBaseLearner->getEdge();
tmpNodePoint._learner = pPreviousBaseLearner;
tmpNodePoint._idx = 0;
tmpNodePoint._depth = 0;
tmpNodePoint._learnerIdxSet = tmpIdx;
calculateChildrenAndEnergies( tmpNodePoint, trajectory[1] );
////////////////////////////////////////////////////////
//insert the root into the priority queue
if ( tmpNodePoint._extended )
{
if (_verbose > 2) {
//cout << "Edges: (parent, pos, neg): " << bLearnerVector[0].first << " " << bLearnerVector[1].first << " " << bLearnerVector[2].first << endl << flush;
//cout << "alpha[" << (ib) << "] = " << _alpha << endl << flush;
cout << "Edges: (parent, pos, neg): " << tmpNodePoint._edge << " " << tmpNodePoint._leftEdge << " " << tmpNodePoint._rightEdge << endl << flush;
}
// if the energy is getting higher then we push it into the priority queue
if ( tmpNodePoint._edge < ( tmpNodePoint._leftEdge + tmpNodePoint._rightEdge ) ) {
float deltaEdge = abs( tmpNodePoint._edge - ( tmpNodePoint._leftEdge + tmpNodePoint._rightEdge ) );
innerNode.first = deltaEdge;
innerNode.second = tmpNodePoint;
pq.push( innerNode );
} else {
//delete bLearnerVector[0].second.first.first;
delete tmpNodePoint._leftChild;
delete tmpNodePoint._rightChild;
}
}
if ( pq.empty() ) {
// we don't extend the root
BaseLearner* tmpBL = _baseLearners[0];
_baseLearners[0] = tmpNodePoint._learner;
delete tmpBL;
ib = 1;
}
while ( ! pq.empty() && ( ib < _numBaseLearners ) ) {
//get the best learner from the priority queue
innerNode = pq.top();
if (_verbose > 2) {
cout << "Delta energy: " << innerNode.first << endl << flush;
cout << "Size of priority queue: " << pq.size() << endl << flush;
}
pq.pop();
tmpNodePoint = innerNode.second;
//tmpPair = bLearnerVector[0];
//tmpPairPos = bLearnerVector[1];
//tmpPairNeg = bLearnerVector[2];
nodeLeft._edge = tmpNodePoint._leftEdge;
nodeLeft._learner = tmpNodePoint._leftChild;
nodeLeft._learnerIdxSet = tmpNodePoint._leftChildIdxSet;
nodeRight._edge = tmpNodePoint._rightEdge;
nodeRight._learner = tmpNodePoint._rightChild;
nodeRight._learnerIdxSet = tmpNodePoint._rightChildIdxSet;
//store the baselearner if the deltaenrgy will be higher
if ( _verbose > 3 ) {
cout << "Insert learner: " << ib << endl << flush;
}
int parentIdx = tmpNodePoint._parentIdx;
BaseLearner* tmpBL = _baseLearners[ib];
_baseLearners[ib] = tmpNodePoint._learner;
delete tmpBL;
nodeLeft._parentIdx = ib;
nodeRight._parentIdx = ib;
nodeLeft._leftOrRightChild = 0;
nodeRight._leftOrRightChild = 1;
nodeLeft._depth = tmpNodePoint._depth + 1;
nodeRight._depth = tmpNodePoint._depth + 1;
if ( ib > 0 ) {
//set the descendant idx
_idxPairs[ parentIdx ][ tmpNodePoint._leftOrRightChild ] = ib;
}
ib++;
if ( ib >= _numBaseLearners ) break;
//extend positive node
if ( nodeLeft._learner ) {
calculateChildrenAndEnergies( nodeLeft, trajectory[ nodeLeft._depth + 1 ] );
} else {
nodeLeft._extended = false;
}
//calculateChildrenAndEnergies( nodeLeft );
// if the energie is getting higher then we push it into the priority queue
if ( nodeLeft._extended )
{
if (_verbose > 2) {
//cout << "Edges: (parent, pos, neg): " << bLearnerVector[0].first << " " << bLearnerVector[1].first << " " << bLearnerVector[2].first << endl << flush;
//cout << "alpha[" << (ib) << "] = " << _alpha << endl << flush;
cout << "Edges: (parent, pos, neg): " << nodeLeft._edge << " " << nodeLeft._leftEdge << " " << nodeLeft._rightEdge << endl << flush;
}
// if the energy is getting higher then we push it into the priority queue
if ( nodeLeft._edge < ( nodeLeft._leftEdge + nodeLeft._rightEdge ) ) {
float deltaEdge = abs( nodeLeft._edge - ( nodeLeft._leftEdge + nodeLeft._rightEdge ) );
innerNode.first = deltaEdge;
innerNode.second = nodeLeft;
pq.push( innerNode );
} else {
//delete bLearnerVector[0].second.first.first;
delete nodeLeft._leftChild;
delete nodeLeft._rightChild;
if ( ib >= _numBaseLearners ) {
delete nodeLeft._learner;
break;
} else {
//this will be a leaf, we do not extend further
int parentIdx = nodeLeft._parentIdx;
BaseLearner* tmpBL = _baseLearners[ib];
_baseLearners[ib] = nodeLeft._learner;
delete tmpBL;
_idxPairs[ parentIdx ][ 0 ] = ib;
ib += 1;
}
}
}
//extend negative node
if ( nodeRight._learner ) {
calculateChildrenAndEnergies( nodeRight, trajectory[ nodeRight._depth + 1 ] );
} else {
nodeRight._extended = false;
}
// if the energie is getting higher then we push it into the priority queue
if ( nodeRight._extended )
{
if (_verbose > 2) {
cout << "Edges: (parent, pos, neg): " << nodeRight._edge << " " << nodeRight._leftEdge << " " << nodeRight._rightEdge << endl << flush;
//cout << "alpha[" << (ib) << "] = " << _alpha << endl << flush;
}
// if the energie is getting higher then we push it into the priority queue
if ( nodeRight._edge < ( nodeRight._leftEdge + nodeRight._rightEdge ) ) {
float deltaEdge = abs( nodeRight._edge - ( nodeRight._leftEdge + nodeRight._rightEdge ) );
innerNode.first = deltaEdge;
innerNode.second = nodeRight;
pq.push( innerNode );
} else {
//delete bLearnerVector[0].second.first.first;
delete nodeRight._leftChild;
delete nodeRight._rightChild;
if ( ib >= _numBaseLearners ) {
delete nodeRight._learner;
break;
} else {
//this will be a leaf, we do not extend further
int parentIdx = nodeRight._parentIdx;
BaseLearner* tmpBL = _baseLearners[ib];
_baseLearners[ib] = nodeRight._learner;
delete tmpBL;
_idxPairs[ parentIdx ][ 1 ] = ib;
ib += 1;
}
}
}
}
for(int ib2 = ib; ib2 < _numBaseLearners; ++ib2) delete _baseLearners[ib2];
_numBaseLearners = ib;
if (_verbose > 2) {
cout << "Num of learners: " << _numBaseLearners << endl << flush;
}
//clear the priority queur
while ( ! pq.empty() && ( ib < _numBaseLearners ) ) {
//get the best learner from the priority queue
innerNode = pq.top();
pq.pop();
tmpNodePoint = innerNode.second;
delete tmpNodePoint._learner;
delete tmpNodePoint._leftChild;
delete tmpNodePoint._rightChild;
}
_id = _baseLearners[0]->getId();
for(int ib = 0; ib < _numBaseLearners; ++ib)
_id += "_x_" + _baseLearners[ib]->getId();
//calculate alpha
this->_alpha = 0.0;
float eps_min = 0.0, eps_pls = 0.0;
_pTrainingData->clearIndexSet();
for( int i = 0; i < _pTrainingData->getNumExamples(); i++ ) {
vector< Label> l = _pTrainingData->getLabels( i );
for( vector< Label >::iterator it = l.begin(); it != l.end(); it++ ) {
float result = this->classify( _pTrainingData, i, it->idx );
if ( ( result * it->y ) < 0 ) eps_min += it->weight;
if ( ( result * it->y ) > 0 ) eps_pls += it->weight;
}
}
//update the weights in the UCT tree
double updateWeight = 0.0;
if ( _updateRule == EDGE_SQUARE ) {
double edge = getEdge();
updateWeight = 1 - sqrt( 1 - ( edge * edge ) );
} else if ( _updateRule == ALPHAS ) {
double alpha = this->getAlpha();
updateWeight = alpha;
} else if ( _updateRule == ESQUARE ) {
double edge = getEdge();
updateWeight = edge * edge;
}
_root.updateInnerNodes( updateWeight, trajectory );
if (_verbose > 2) {
cout << "Update weight (" << updateWeight << ")" << "\tUpdate rule index: "<< _updateRule << endl << flush;
}
// set the smoothing value to avoid numerical problem
// when theta=0.
setSmoothingVal( (float)1.0 / (float)_pTrainingData->getNumExamples() * (float)0.01 );
this->_alpha = getAlpha( eps_min, eps_pls );
// calculate the energy (sum of the energy of the leaves
float energy = this->getEnergy( eps_min, eps_pls );
return energy;
}
bool ChompPlannerNode::filterJointTrajectory(arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request &request, arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Response &res)
{
arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request req = request;
ros::WallTime start_time = ros::WallTime::now();
ROS_INFO_STREAM("Received filtering request with trajectory size " << req.trajectory.points.size());
if(req.path_constraints.joint_constraints.size() > 0 ||
req.path_constraints.position_constraints.size() > 0 ||
req.path_constraints.orientation_constraints.size() > 0 ||
req.path_constraints.visibility_constraints.size() > 0) {
if(use_trajectory_filter_) {
ROS_INFO("Chomp can't handle path constraints, passing through to other trajectory filters");
if(!filter_trajectory_client_.call(req,res)) {
ROS_INFO("Pass through failed");
} else {
ROS_INFO("Pass through succeeded");
}
} else {
ROS_INFO("Chomp can't handle path constraints, and not set up to use additional filter");
}
return true;
}
for (unsigned int i=0; i< req.trajectory.points.size(); i++)
{
req.trajectory.points[i].velocities.resize(req.trajectory.joint_names.size(),0.0);
}
getLimits(req.trajectory, req.limits);
trajectory_msgs::JointTrajectory jtraj;
int num_points = req.trajectory.points.size();
if(num_points > maximum_spline_points_) {
num_points = maximum_spline_points_;
} else if(num_points < minimum_spline_points_) {
num_points = minimum_spline_points_;
}
//create a spline from the trajectory
spline_smoother::CubicTrajectory trajectory_solver;
spline_smoother::SplineTrajectory spline;
planning_environment::setRobotStateAndComputeTransforms(req.start_state,
*collision_proximity_space_->getCollisionModelsInterface()->getPlanningSceneState());
trajectory_solver.parameterize(req.trajectory,req.limits,spline);
double smoother_time;
spline_smoother::getTotalTime(spline, smoother_time);
ROS_INFO_STREAM("Total time given is " << smoother_time);
double t = 0.0;
vector<double> times(num_points);
for(int i = 0; i < num_points; i++,t += smoother_time/(1.0*(num_points-1))) {
times[i] = t;
}
spline_smoother::sampleSplineTrajectory(spline, times, jtraj);
//double planner_time = req.trajectory.points.back().time_from_start.toSec();
t = 0.0;
for(unsigned int i = 0; i < jtraj.points.size(); i++, t += smoother_time/(1.0*(num_points-1))) {
jtraj.points[i].time_from_start = ros::Duration(t);
}
ROS_INFO_STREAM("Sampled trajectory has " << jtraj.points.size() << " points with " << jtraj.points[0].positions.size() << " joints");
string group_name;
group_name = req.group_name;
vector<string> linkNames;
vector<string> attachedBodies;
collision_proximity_space_->setupForGroupQueries(group_name,
req.start_state,
linkNames,
attachedBodies);
ChompTrajectory trajectory(robot_model_, group_name, jtraj);
//configure the distance field - this should just use current state
arm_navigation_msgs::RobotState robot_state = req.start_state;
jointStateToArray(arm_navigation_msgs::createJointState(req.trajectory.joint_names, jtraj.points[0].positions), group_name, trajectory.getTrajectoryPoint(0));
//set the goal state equal to start state, and override the joints specified in the goal
//joint constraints
int goal_index = trajectory.getNumPoints()-1;
trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
sensor_msgs::JointState goal_state = arm_navigation_msgs::createJointState(req.trajectory.joint_names, jtraj.points.back().positions);
jointStateToArray(goal_state, group_name,trajectory.getTrajectoryPoint(goal_index));
map<string, KinematicModel::JointModelGroup*> groupMap = robot_model_->getJointModelGroupMap();
KinematicModel::JointModelGroup* modelGroup = groupMap[group_name];
// fix the goal to move the shortest angular distance for wrap-around joints:
for (size_t i = 0; i < modelGroup->getJointModels().size(); i++)
{
const KinematicModel::JointModel* model = modelGroup->getJointModels()[i];
const KinematicModel::RevoluteJointModel* revoluteJoint = dynamic_cast<const KinematicModel::RevoluteJointModel*>(model);
if (revoluteJoint != NULL)
{
if(revoluteJoint->continuous_)
{
double start = trajectory(0, i);
double end = trajectory(goal_index, i);
trajectory(goal_index, i) = start + angles::shortest_angular_distance(start, end);
}
}
}
//sets other joints
trajectory.fillInMinJerk();
trajectory.overwriteTrajectory(jtraj);
// set the max planning time:
chomp_parameters_.setPlanningTimeLimit(req.allowed_time.toSec());
chomp_parameters_.setFilterMode(true);
// optimize!
ChompOptimizer optimizer(&trajectory, robot_model_, group_name, &chomp_parameters_,
vis_marker_array_publisher_, vis_marker_publisher_, collision_proximity_space_);
optimizer.optimize();
// assume that the trajectory is now optimized, fill in the output structure:
vector<double> velocity_limits(trajectory.getNumJoints(), numeric_limits<double>::max());
res.trajectory.points.resize(trajectory.getNumPoints());
// fill in joint names:
res.trajectory.joint_names.resize(trajectory.getNumJoints());
for (size_t i = 0; i < modelGroup->getJointModels().size(); i++)
{
res.trajectory.joint_names[i] = modelGroup->getJointModels()[i]->getName();
velocity_limits[i] = joint_limits_[res.trajectory.joint_names[i]].max_velocity;
}
res.trajectory.header.stamp = ros::Time::now();
res.trajectory.header.frame_id = reference_frame_;
// fill in the entire trajectory
for (size_t i = 0; i < (unsigned int)trajectory.getNumPoints(); i++)
{
res.trajectory.points[i].positions.resize(trajectory.getNumJoints());
res.trajectory.points[i].velocities.resize(trajectory.getNumJoints());
for (size_t j=0; j < res.trajectory.points[i].positions.size(); j++)
{
res.trajectory.points[i].positions[j] = trajectory(i, j);
}
if (i==0)
res.trajectory.points[i].time_from_start = ros::Duration(0.0);
else
{
double duration = trajectory.getDiscretization();
// check with all the joints if this duration is ok, else push it up
for (int j=0; j < trajectory.getNumJoints(); j++)
{
double d = fabs(res.trajectory.points[i].positions[j] - res.trajectory.points[i-1].positions[j]) / velocity_limits[j];
if (d > duration)
duration = d;
}
try {
res.trajectory.points[i].time_from_start = res.trajectory.points[i-1].time_from_start + ros::Duration(duration);
} catch(...) {
ROS_INFO_STREAM("Potentially weird duration of " << duration);
}
}
}
arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request next_req;
arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Response next_res;
if(use_trajectory_filter_) {
next_req = req;
next_req.trajectory = res.trajectory;
next_req.allowed_time=ros::Duration(1.0);//req.allowed_time/2.0;
if(filter_trajectory_client_.call(next_req, next_res)) {
ROS_INFO_STREAM("Filter call ok. Sent trajectory had " << res.trajectory.points.size() << " points. Returned trajectory has " << next_res.trajectory.points.size() << " points ");
} else {
ROS_INFO("Filter call not ok");
}
res.trajectory = next_res.trajectory;
res.error_code = next_res.error_code;
res.trajectory.header.stamp = ros::Time::now();
res.trajectory.header.frame_id = reference_frame_;
} else {
res.error_code.val = res.error_code.val = res.error_code.SUCCESS;
}
// for every point in time:
for (unsigned int i=1; i<res.trajectory.points.size()-1; ++i)
{
double dt1 = (res.trajectory.points[i].time_from_start - res.trajectory.points[i-1].time_from_start).toSec();
double dt2 = (res.trajectory.points[i+1].time_from_start - res.trajectory.points[i].time_from_start).toSec();
// for every (joint) trajectory
for (int j=0; j < trajectory.getNumJoints(); ++j)
{
double dx1 = res.trajectory.points[i].positions[j] - res.trajectory.points[i-1].positions[j];
double dx2 = res.trajectory.points[i+1].positions[j] - res.trajectory.points[i].positions[j];
double v1 = dx1/dt1;
double v2 = dx2/dt2;
res.trajectory.points[i].velocities[j] = 0.5*(v1 + v2);
}
}
ROS_INFO("Serviced filter request in %f wall-seconds, trajectory duration is %f", (ros::WallTime::now() - start_time).toSec(), res.trajectory.points.back().time_from_start.toSec());
return true;
}
void AutoThread::run(){
QTime rate;
qDebug() << "automatic from: " << QThread::currentThreadId();
MavState previous = g::state;
MavState previous_platform = g::platform;
MavState next, next_platform;
land_count = 0;
rot_count = 0;
double vz = 0;
double vx = 0;
double vy = 0;
double e_x = 0;
double e_y = 0;
double e_z = 0;
float vy_platform = 0;
rate.start();
while (true) {
next = g::state;
next_platform = g::platform;
e_x = g::setPoint.x() - g::state.x();
e_y = g::setPoint.y() - g::state.y();
e_z = g::setPoint.z() - g::state.z();
vz = r_auto * (next.z() - previous.z()) ;
vx = r_auto * (next.x() - previous.x()) ;
vy = r_auto * (next.y() - previous.y()) ;
vy_platform = r_auto * (next_platform.y() - previous_platform.y()) ;
//takeoff
if(executioner::take_off::take_off_sig){
takeOff();
}
//land
if(executioner::land::land_sig){
position state;
state.x = g::state.x();
state.y = g::state.y();
state.z = g::state.z();
position p;
p.x = nodeList[actualNode].p.x;
p.y = nodeList[actualNode].p.y;
float vel = nodeList[actualNode].a.params[0];
land(vel,(float)1/r_auto,vz,p,state);
}
//move
if(executioner::move::move_sig){
position target;
target.x = nodeList[actualNode].p.x;
target.y = nodeList[actualNode].p.y;
target.z = nodeList[actualNode].p.z;
target.yaw = nodeList[actualNode].p.yaw;
float alpha = nodeList[actualNode].a.params[0];
position state;
state.x = g::state.x();
state.y = g::state.y();
state.z = g::state.z();
move(alpha,target,state);
}
//Land on moving platform (experimental)
if(executioner::land::land_plat_sig){
land_plat(g::platform,g::state,2);
}
//rotate
if(executioner::rotate::rotate_sig){
rotate();
}
//Trajectory
if(executioner::trajectory::traj_sig){
if(!executioner::trajectory::was_executing){
t_traj = 0;
executioner::trajectory::was_executing = true;
}
else{
t_traj += (float)1/r_auto;
double omega = nodeList[actualNode].a.params[0];
double radius = nodeList[actualNode].a.params[1];
double secs = nodeList[actualNode].a.params[2];
double look = nodeList[actualNode].a.params[3];
double c[2] = {nodeList[actualNode].p.x,nodeList[actualNode].p.y};
trajectory(omega,radius,c,t_traj,secs,look);
}
}
//Writing output file
float roll,pitch,yaw;
g::state.orientation2(&roll,&pitch,&yaw);
output <<g::state.x()<<" "<<g::state.y()<<" "<<g::state.z()<< //Position
" "<<g::setPoint.x()<<" "<<g::setPoint.y()<<" "<<g::setPoint.z()<< //Position SetPoint
" "<<e_x<<" "<<e_y<<" "<<e_z<<" "<< //Position error
vx <<" "<<vy<<" "<<vz<<" "<< //Velocity
roll<<" "<<pitch << " " << yaw //Attitude
<<" "<<plat_error[0]<<" "<<plat_error[1]; //platform allignement error
output << ";\n";
previous = next;
previous_platform = next_platform;
msleep(1000/r_auto - (float)rate.elapsed());
rate.restart();
}
}
bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit_msgs::GetMotionPlan::Request &req,
const chomp::ChompParameters& params,
moveit_msgs::GetMotionPlan::Response &res) const
{
ros::WallTime start_time = ros::WallTime::now();
ChompTrajectory trajectory(planning_scene->getRobotModel(),
3.0,
.03,
req.motion_plan_request.group_name);
jointStateToArray(planning_scene->getRobotModel(),
req.motion_plan_request.start_state.joint_state,
req.motion_plan_request.group_name,
trajectory.getTrajectoryPoint(0));
int goal_index = trajectory.getNumPoints()- 1;
trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
sensor_msgs::JointState js;
for(unsigned int i = 0; i < req.motion_plan_request.goal_constraints[0].joint_constraints.size(); i++) {
js.name.push_back(req.motion_plan_request.goal_constraints[0].joint_constraints[i].joint_name);
js.position.push_back(req.motion_plan_request.goal_constraints[0].joint_constraints[i].position);
ROS_INFO_STREAM("Setting joint " << req.motion_plan_request.goal_constraints[0].joint_constraints[i].joint_name
<< " to position " << req.motion_plan_request.goal_constraints[0].joint_constraints[i].position);
}
jointStateToArray(planning_scene->getRobotModel(),
js,
req.motion_plan_request.group_name,
trajectory.getTrajectoryPoint(goal_index));
const planning_models::RobotModel::JointModelGroup* model_group =
planning_scene->getRobotModel()->getJointModelGroup(req.motion_plan_request.group_name);
// fix the goal to move the shortest angular distance for wrap-around joints:
for (size_t i = 0; i < model_group->getJointModels().size(); i++)
{
const planning_models::RobotModel::JointModel* model = model_group->getJointModels()[i];
const planning_models::RobotModel::RevoluteJointModel* revolute_joint = dynamic_cast<const planning_models::RobotModel::RevoluteJointModel*>(model);
if (revolute_joint != NULL)
{
if(revolute_joint->isContinuous())
{
double start = (trajectory)(0, i);
double end = (trajectory)(goal_index, i);
ROS_INFO_STREAM("Start is " << start << " end " << end << " short " << shortestAngularDistance(start, end));
(trajectory)(goal_index, i) = start + shortestAngularDistance(start, end);
}
}
}
// fill in an initial quintic spline trajectory
trajectory.fillInMinJerk();
// optimize!
planning_models::RobotState *start_state(planning_scene->getCurrentState());
planning_models::robotStateMsgToRobotState(*planning_scene->getTransforms(), req.motion_plan_request.start_state, start_state);
ros::WallTime create_time = ros::WallTime::now();
ChompOptimizer optimizer(&trajectory,
planning_scene,
req.motion_plan_request.group_name,
¶ms,
start_state);
if(!optimizer.isInitialized()) {
ROS_WARN_STREAM("Could not initialize optimizer");
res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
return false;
}
ROS_INFO("Optimization took %f sec to create", (ros::WallTime::now() - create_time).toSec());
ROS_INFO("Optimization took %f sec to create", (ros::WallTime::now() - create_time).toSec());
optimizer.optimize();
ROS_INFO("Optimization actually took %f sec to run", (ros::WallTime::now() - create_time).toSec());
create_time = ros::WallTime::now();
// assume that the trajectory is now optimized, fill in the output structure:
ROS_INFO("Output trajectory has %d joints", trajectory.getNumJoints());
// fill in joint names:
res.trajectory.joint_trajectory.joint_names.resize(trajectory.getNumJoints());
for (size_t i = 0; i < model_group->getJointModels().size(); i++)
{
res.trajectory.joint_trajectory.joint_names[i] = model_group->getJointModels()[i]->getName();
}
res.trajectory.joint_trajectory.header = req.motion_plan_request.start_state.joint_state.header; // @TODO this is probably a hack
// fill in the entire trajectory
res.trajectory.joint_trajectory.points.resize(trajectory.getNumPoints());
for (int i=0; i < trajectory.getNumPoints(); i++)
{
res.trajectory.joint_trajectory.points[i].positions.resize(trajectory.getNumJoints());
for (size_t j=0; j < res.trajectory.joint_trajectory.points[i].positions.size(); j++)
{
res.trajectory.joint_trajectory.points[i].positions[j] = trajectory.getTrajectoryPoint(i)(j);
if(i == trajectory.getNumPoints()-1) {
ROS_INFO_STREAM("Joint " << j << " " << res.trajectory.joint_trajectory.points[i].positions[j]);
}
}
// Setting invalid timestamps.
// Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints.
res.trajectory.joint_trajectory.points[i].time_from_start = ros::Duration(0.0);
}
ROS_INFO("Bottom took %f sec to create", (ros::WallTime::now() - create_time).toSec());
ROS_INFO("Serviced planning request in %f wall-seconds, trajectory duration is %f", (ros::WallTime::now() - start_time).toSec(), res.trajectory.joint_trajectory.points[goal_index].time_from_start.toSec());
res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
res.planning_time = ros::Duration((ros::WallTime::now() - start_time).toSec());
return true;
}
void Ephemeris<Frame>::ComputeApsides(
not_null<MassiveBody const*> const body,
typename DiscreteTrajectory<Frame>::Iterator const begin,
typename DiscreteTrajectory<Frame>::Iterator const end,
DiscreteTrajectory<Frame>& apoapsides,
DiscreteTrajectory<Frame>& periapsides) {
not_null<ContinuousTrajectory<Frame> const*> const body_trajectory =
trajectory(body);
typename ContinuousTrajectory<Frame>::Hint hint;
std::experimental::optional<Instant> previous_time;
std::experimental::optional<DegreesOfFreedom<Frame>>
previous_degrees_of_freedom;
std::experimental::optional<Square<Length>> previous_squared_distance;
std::experimental::optional<Variation<Square<Length>>>
previous_squared_distance_derivative;
for (auto it = begin; it != end; ++it) {
Instant const time = it.time();
DegreesOfFreedom<Frame> const degrees_of_freedom = it.degrees_of_freedom();
DegreesOfFreedom<Frame> const body_degrees_of_freedom =
body_trajectory->EvaluateDegreesOfFreedom(time, &hint);
RelativeDegreesOfFreedom<Frame> const relative =
degrees_of_freedom - body_degrees_of_freedom;
Square<Length> const squared_distance =
InnerProduct(relative.displacement(), relative.displacement());
// This is the derivative of |squared_distance|.
Variation<Square<Length>> const squared_distance_derivative =
2.0 * InnerProduct(relative.displacement(), relative.velocity());
if (previous_squared_distance_derivative &&
Sign(squared_distance_derivative) !=
Sign(*previous_squared_distance_derivative)) {
CHECK(previous_time &&
previous_degrees_of_freedom &&
previous_squared_distance);
// The derivative of |squared_distance| changed sign. Construct a Hermite
// approximation of |squared_distance| and find its extrema.
Hermite3<Instant, Square<Length>> const
squared_distance_approximation(
{*previous_time, time},
{*previous_squared_distance, squared_distance},
{*previous_squared_distance_derivative,
squared_distance_derivative});
std::set<Instant> const extrema =
squared_distance_approximation.FindExtrema();
// Now look at the extrema and check that exactly one is in the required
// time interval. This is normally the case, but it can fail due to
// ill-conditioning.
Instant apsis_time;
int valid_extrema = 0;
for (auto const& extremum : extrema) {
if (extremum >= *previous_time && extremum <= time) {
apsis_time = extremum;
++valid_extrema;
}
}
if (valid_extrema != 1) {
// Something went wrong when finding the extrema of
// |squared_distance_approximation|. Use a linear interpolation of
// |squared_distance_derivative| instead.
apsis_time = Barycentre<Instant, Variation<Square<Length>>>(
{time, *previous_time},
{*previous_squared_distance_derivative,
-squared_distance_derivative});
}
// Now that we know the time of the apsis, construct a Hermite
// approximation of the position of the body, and use it to derive its
// degrees of freedom. Note that an extremum of
// |squared_distance_approximation| is in general not an extremum for
// |position_approximation|: the distance computed using the latter is a
// 6th-degree polynomial. However, approximating this polynomial using a
// 3rd-degree polynomial would yield |squared_distance_approximation|, so
// we shouldn't be far from the truth.
Hermite3<Instant, Position<Frame>> position_approximation(
{*previous_time, time},
{previous_degrees_of_freedom->position(),
degrees_of_freedom.position()},
{previous_degrees_of_freedom->velocity(),
degrees_of_freedom.velocity()});
DegreesOfFreedom<Frame> const apsis_degrees_of_freedom(
position_approximation.Evaluate(apsis_time),
position_approximation.EvaluateDerivative(apsis_time));
if (Sign(squared_distance_derivative).Negative()) {
apoapsides.Append(apsis_time, apsis_degrees_of_freedom);
} else {
periapsides.Append(apsis_time, apsis_degrees_of_freedom);
}
}
previous_time = time;
previous_degrees_of_freedom = degrees_of_freedom;
previous_squared_distance = squared_distance;
previous_squared_distance_derivative = squared_distance_derivative;
}
}
bool KinematicIntegerBands::vert_repul_at(double tstep, bool trajdir, int k, const TrafficState& ownship,
const TrafficState& repac, int epsv) const {
// repac is not NULL at this point and k >= 0
if (k==0) {
return true;
}
std::pair<Vect3,Velocity> sovo = trajectory(ownship,0,trajdir);
Vect3 so = sovo.first;
Vect3 vo = sovo.second;
Vect3 si = repac.get_s();
Vect3 vi = repac.get_v();
bool rep = true;
if (k==1) {
rep = CriteriaCore::vertical_new_repulsive_criterion(so.Sub(si),vo,vi,linvel(ownship,tstep,trajdir,0),epsv);
}
if (rep) {
std::pair<Vect3,Velocity> sovot = trajectory(ownship,k*tstep,trajdir);
Vect3 sot = sovot.first;
Vect3 vot = sovot.second;
Vect3 sit = vi.ScalAdd(k*tstep,si);
Vect3 st = sot.Sub(sit);
Vect3 vop = linvel(ownship,tstep,trajdir,k-1);
Vect3 vok = linvel(ownship,tstep,trajdir,k);
return CriteriaCore::vertical_new_repulsive_criterion(st,vop,vi,vot,epsv) &&
CriteriaCore::vertical_new_repulsive_criterion(st,vot,vi,vok,epsv) &&
CriteriaCore::vertical_new_repulsive_criterion(st,vop,vi,vok,epsv);
}
return false;
}
bool KinematicIntegerBands::no_instantaneous_conflict(Detection3D* conflict_det, Detection3D* recovery_det,
double B, double T, double B2, double T2,
bool trajdir, const TrafficState& ownship, const std::vector<TrafficState>& traffic,
const TrafficState& repac,
int epsh, int epsv) {
bool usehcrit = repac.isValid() && epsh != 0;
bool usevcrit = repac.isValid() && epsv != 0;
std::pair<Vect3,Velocity> nsovo = trajectory(ownship,0,trajdir);
Vect3 so = ownship.get_s();
Vect3 vo = ownship.get_v();
Vect3 si = repac.get_s();
Vect3 vi = repac.get_v();
Vect3 nvo = nsovo.second;
Vect3 s = so.Sub(si);
return
(!usehcrit || CriteriaCore::horizontal_new_repulsive_criterion(s,vo,vi,nvo,epsh)) &&
(!usevcrit || CriteriaCore::vertical_new_repulsive_criterion(s,vo,vi,nvo,epsv)) &&
no_conflict(conflict_det,recovery_det,B,T,B2,T2,trajdir,0,ownship,traffic);
}