cleaning up for pr

src/modules/interface/planner.h

@@ -118,7 +118,7 @@ int plan_go_to_from(struct planner *p, const struct traj_eval *curr_eval, bool r
 int plan_start_trajectory(struct planner *p, struct piecewise_traj* trajectory, bool reversed, bool relative, struct vec start_from, float start_yaw);
 
 // start compressed trajectory. start_from param is ignored if relative == false.
-int plan_start_compressed_trajectory(struct planner *p, struct piecewise_traj_compressed* trajectory, bool relative, struct vec start_from, float start_yaw);
+int plan_start_compressed_trajectory(struct planner *p, struct piecewise_traj_compressed* trajectory, bool relative, struct vec start_from);
 
 // Query if the trjectory is finished
 bool plan_is_finished(struct planner *p, float t);

src/modules/interface/pptraj_compressed.h

@@ -46,7 +46,6 @@ struct piecewise_traj_compressed
 	float duration;
 	float timescale;
 	struct vec shift;
-	float shift_yaw;
 	const void* data;
 
 	// mutable part of the data structure. We plan to mess around with this part

src/modules/src/crtp_commander_high_level.c

@@ -652,7 +652,7 @@ int start_trajectory(const struct data_start_trajectory* data)
             &trajectories_memory[trajDesc->trajectoryIdentifier.mem.offset]
           );
           compressed_trajectory.t_begin = t;
-          result = plan_start_compressed_trajectory(&planner, &compressed_trajectory, data->relative, pos, yaw);
+          result = plan_start_compressed_trajectory(&planner, &compressed_trajectory, data->relative, pos);
           xSemaphoreGive(lockTraj);
         }
       }

src/modules/src/planner.c

@@ -241,7 +241,7 @@ int plan_start_trajectory(struct planner *p, struct piecewise_traj* trajectory,
 	return 0;
 }
 
-int plan_start_compressed_trajectory( struct planner *p, struct piecewise_traj_compressed* trajectory, bool relative, struct vec start_from, float start_yaw)
+int plan_start_compressed_trajectory( struct planner *p, struct piecewise_traj_compressed* trajectory, bool relative, struct vec start_from)
 {
 	p->reversed = 0;
 	p->state = TRAJECTORY_STATE_FLYING;
@@ -250,20 +250,15 @@ int plan_start_compressed_trajectory( struct planner *p, struct piecewise_traj_c
 
 	if (relative) {
 		trajectory->shift = vzero();
-		trajectory->shift_yaw = 0;
-		struct traj_eval traj_init = piecewise_compressed_eval(trajectory, trajectory->t_begin);
+		struct traj_eval traj_init = piecewise_compressed_eval(
+			trajectory, trajectory->t_begin
+		);
 
 		// translate trajectory to current position
 		struct vec shift_pos = vsub(start_from, traj_init.pos);
 		trajectory->shift = shift_pos;
-
-		// compute the shortest possible rotation towards trajectory start yaw to current yaw
-		float traj_yaw = normalize_radians(traj_init.yaw);
-		start_yaw = normalize_radians(start_yaw);
-		trajectory->shift_yaw = shortest_signed_angle_radians(start_yaw, traj_yaw);
 	} else {
 		trajectory->shift = vzero();
-		trajectory->shift_yaw = 0;
 	}
 
 	return 0;

src/modules/src/pptraj.c

@@ -349,13 +349,6 @@ void traj_eval_transform(struct traj_eval *ev, struct vec shift, float rotation)
 struct traj_eval piecewise_eval(
   struct piecewise_traj const *traj, float t)
 {
-	//TODO: rotate trajectory before translating it by traj->shift
-	/* This must be done after the polynomial evaluation since we cannot rotate
-	the polynomial itself before evaluation. As such, we will also need to shift
-	after evaluation, instead of baking the shift into the polynomial. At that
-	point do we still need to include yaw in the shift?
-	*/
-
 	int cursor = 0;
 	t = t - traj->t_begin;
 	while (cursor < traj->n_pieces) {
@@ -397,7 +390,7 @@ struct traj_eval piecewise_eval_reversed(
 		struct poly4d const *piece = &(traj->pieces[cursor]);
 		if (t <= piece->duration * traj->timescale) {
 			poly4d_tmp = *piece;
-			poly4d_shift(&poly4d_tmp, traj->shift.x, traj->shift.y, traj->shift.z, traj->shift_yaw);
+			poly4d_shift(&poly4d_tmp, traj->shift.x, traj->shift.y, traj->shift.z, 0);
 			poly4d_stretchtime(&poly4d_tmp, traj->timescale);
 			for (int i = 0; i < 4; ++i) {
 				polyreflect(poly4d_tmp.p[i]);
@@ -412,7 +405,6 @@ struct traj_eval piecewise_eval_reversed(
 	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.yaw = normalize_radians(ev.yaw + traj->shift_yaw);
 	ev.vel = vzero();
 	ev.acc = vzero();
 	ev.jerk = vzero();
@@ -430,7 +422,6 @@ void piecewise_plan_5th_order(struct piecewise_traj *pp, float duration,
 	p->duration = duration;
 	pp->timescale = 1.0;
 	pp->shift = vzero();
-	pp->shift_yaw = 0;
 	pp->n_pieces = 1;
 	poly5(p->p[0], duration, p0.x, v0.x, a0.x, p1.x, v1.x, a1.x);
 	poly5(p->p[1], duration, p0.y, v0.y, a0.y, p1.y, v1.y, a1.y);
@@ -447,7 +438,6 @@ void piecewise_plan_7th_order_no_jerk(struct piecewise_traj *pp, float duration,
 	p->duration = duration;
 	pp->timescale = 1.0;
 	pp->shift = vzero();
-	pp->shift_yaw = 0;
 	pp->n_pieces = 1;
 	poly7_nojerk(p->p[0], duration, p0.x, v0.x, a0.x, p1.x, v1.x, a1.x);
 	poly7_nojerk(p->p[1], duration, p0.y, v0.y, a0.y, p1.y, v1.y, a1.y);