changed how trajectory is computed

k-sheridan · May 26, 2017 · fd4638b · fd4638b
1 parent 3fc3ae8
commit fd4638b
Show file tree

Hide file tree

Showing 4 changed files with 7 additions and 7 deletions.
diff --git a/include/pauvsi_trajectory/Config.h b/include/pauvsi_trajectory/Config.h
@@ -27,7 +27,7 @@
 #define DIST2Dt_MAX 3
 #define D2DT_TIME_OPTIM_ITER 6
 
-#define DT_OPTIM_ACCURATE false
+#define DT_OPTIM_ACCURATE true
 #define ACCURATE_TIME_OPTIM_ITERS 10
 #define ACCURATE_DT_LOW -2
 #define ACCURATE_DT_HIGH 2

diff --git a/include/pauvsi_trajectory/Physics.h b/include/pauvsi_trajectory/Physics.h
@@ -16,7 +16,7 @@
 // these are the max and min individual motor forces.
 // you can tune the aggression of the quad using the max in Newtons
 #define MOTOR_FORCE_MIN 1
-#define MOTOR_FORCE_MAX 20
+#define MOTOR_FORCE_MAX 15
 
 //mass of quad kg
 #define MASS 5

diff --git a/include/pauvsi_trajectory/Simulation.h b/include/pauvsi_trajectory/Simulation.h
@@ -9,7 +9,7 @@
 #define PAUVSI_TRAJECTORY_INCLUDE_PAUVSI_TRAJECTORY_SIMULATION_H_
 
 #define MOTOR_FORCE SIGMA 0.2
-#define POSITION_SIGMA 0.02
+#define POSITION_SIGMA 0.03
 #define OMEGA_SIGMA 0.0005
 #define VEL_SIGMA 0.01
 #define QUAT_SIGMA 0.001

diff --git a/include/pauvsi_trajectory/TrajectoryGenerator.cpp b/include/pauvsi_trajectory/TrajectoryGenerator.cpp
@@ -42,10 +42,6 @@ TrajectorySegment TrajectoryGenerator::computeHighOrderMinimumTimeTrajectory(Dyn
 
 	ROS_DEBUG_STREAM("size 1: " << seg.x.size());
 
-	seg = this->computeGeometricallyFeasibleTrajectory(constraints);
-
-	ROS_DEBUG_STREAM("size 2: " << seg.x.size());
-
 	seg = this->minimizeTimeFAST(constraints, phys);
 
 	ROS_DEBUG_STREAM("size 3: " << seg.x.size());
@@ -55,6 +51,10 @@ TrajectorySegment TrajectoryGenerator::computeHighOrderMinimumTimeTrajectory(Dyn
 		seg = this->minimizeTimeACCURATE(constraints, phys);
 	}
 
+	ROS_DEBUG_STREAM("size 2: " << seg.x.size());
+
+	seg = this->computeGeometricallyFeasibleTrajectory(constraints);
+
 	ROS_DEBUG_STREAM("size 4: " << seg.x.size());
 
 	return seg;