Merge pull request #308 from USC-ACTLab/hlcmdEvalShiftBug

HighLevelCmd: Add missing shift for traj. eval
bitcraze · Apr 20, 2018 · e88c926 · e88c926
2 parents 71ebff0 + 585d85c
commit e88c926
Showing 1 changed file with 2 additions and 0 deletions.
diff --git a/src/modules/src/pptraj.c b/src/modules/src/pptraj.c
@@ -304,6 +304,7 @@ struct traj_eval piecewise_eval(
 	// if we get here, the trajectory has ended
 	struct poly4d const *end_piece = &(traj->pieces[traj->n_pieces - 1]);
 	struct traj_eval ev = poly4d_eval(end_piece, end_piece->duration);
+	ev.pos = vadd(ev.pos, traj->shift);
 	ev.vel = vzero();
 	ev.acc = vzero();
 	ev.omega = vzero();
@@ -333,6 +334,7 @@ struct traj_eval piecewise_eval_reversed(
 	// if we get here, the trajectory has ended
 	struct poly4d const *end_piece = &(traj->pieces[0]);
 	struct traj_eval ev = poly4d_eval(end_piece, 0.0f);
+	ev.pos = vadd(ev.pos, traj->shift);
 	ev.vel = vzero();
 	ev.acc = vzero();
 	ev.omega = vzero();