Merge branch 'constraints' into main

baylessj · Nov 3, 2020 · 7147c8d · 7147c8d
2 parents 4622e82 + 0f894e8
commit 7147c8d
Show file tree

Hide file tree

Showing 6 changed files with 41 additions and 13 deletions.
diff --git a/include/constraints.hpp b/include/constraints.hpp
@@ -24,12 +24,16 @@ struct Constraints {
    * @param imin_accel The minimum allowable acceleration for the robot in
    *                   meters per second per second.
    */
-  Constraints(double imax_vel = std::numeric_limits<double>::max(),
+  Constraints(double imax_vel,
               double imax_accel = std::numeric_limits<double>::max(),
               double imax_jerk = std::numeric_limits<double>::max(),
               double imin_accel = std::nan(""))
     : max_vel(imax_vel), max_accel(imax_accel), max_jerk(imax_jerk) {
-    min_accel = std::isnan(imin_accel) ? -imax_accel : imin_accel;
+      if (imax_accel == std::numeric_limits<double>::max()) {
+        min_accel = std::numeric_limits<double>::lowest();
+      } else {
+        min_accel = std::isnan(imin_accel) ? -imax_accel : imin_accel;
+      }
   }
 
   /**

diff --git a/include/physicalmodel/passthroughmodel.hpp b/include/physicalmodel/passthroughmodel.hpp
@@ -17,14 +17,18 @@ class PassthroughModel : public PhysicalModel {
    */
   PassthroughModel() = default;
 
-  Constraints constraints(UNUSED const Pose pose,
-                          UNUSED double curvature,
-                          UNUSED double vel) override {
-    return Constraints();
+  Constraints constraints(const Pose pose,
+                          double curvature,
+                          double vel) override {
+    UNUSED(pose);
+    UNUSED(curvature);
+    return Constraints(vel);
   };
 
-  std::vector<double> linear_to_wheel_vels(UNUSED double lin_vel,
-                                           UNUSED double curvature) override {
+  std::vector<double> linear_to_wheel_vels(double lin_vel,
+                                           double curvature) override {
+    UNUSED(lin_vel);
+    UNUSED(curvature);
     return std::vector<double>{};
   }
 

diff --git a/include/physicalmodel/physicalmodel.hpp b/include/physicalmodel/physicalmodel.hpp
@@ -11,7 +11,7 @@
 #include "geometry/pose.hpp"
 
 // This silences a warning in GCC/Clang about not using passed parameters
-#define UNUSED __attribute__((unused))
+#define UNUSED(var) ((void)var)
 
 namespace squiggles {
 class PhysicalModel {

diff --git a/include/physicalmodel/tankmodel.hpp b/include/physicalmodel/tankmodel.hpp
@@ -25,7 +25,7 @@ class TankModel : public PhysicalModel {
   TankModel(double itrack_width, Constraints ilinear_constraints);
 
   Constraints
-  constraints(UNUSED const Pose pose, double curvature, double vel) override;
+  constraints(const Pose pose, double curvature, double vel) override;
 
   std::vector<double> linear_to_wheel_vels(double lin_vel,
                                            double curvature) override;

diff --git a/src/tankmodel.cpp b/src/tankmodel.cpp
@@ -14,15 +14,17 @@ TankModel::TankModel(double itrack_width, Constraints ilinear_constraints)
   : track_width(itrack_width), linear_constraints(ilinear_constraints) {}
 
 Constraints
-TankModel::constraints(UNUSED const Pose pose, double curvature, double vel) {
+TankModel::constraints(const Pose pose, double curvature, double vel) {
+  UNUSED(pose);
   auto max_vel = vel_constraint(pose, curvature, vel);
   auto [min_accel, max_accel] = accel_constraint(pose, curvature, vel);
   return Constraints(max_vel, max_accel, 0.0, min_accel);
 }
 
-double TankModel::vel_constraint(UNUSED const Pose pose,
+double TankModel::vel_constraint(const Pose pose,
                                  double curvature,
                                  double vel) {
+  UNUSED(pose);
   auto wheels = linear_to_wheel_vels(vel, curvature);
   auto left = wheels[0];
   auto right = wheels[1];
@@ -37,9 +39,10 @@ double TankModel::vel_constraint(UNUSED const Pose pose,
   return ((left + right) / 2.0);
 }
 
-std::tuple<double, double> TankModel::accel_constraint(UNUSED const Pose pose,
+std::tuple<double, double> TankModel::accel_constraint(const Pose pose,
                                                        double curvature,
                                                        double vel) const {
+  UNUSED(pose);
   // auto [left, right] = linear_to_wheel_vels(vel, curvature);
   // auto max_wheel_speed = std::max(left, right);
   // auto min_wheel_speed = std::min(left, right);

diff --git a/tst/model-constraints-test.cpp b/tst/model-constraints-test.cpp
@@ -10,6 +10,23 @@
 
 using namespace squiggles;
 
+TEST(model_constraints_test, min_constraints) {
+  auto constraints = Constraints(1.0);
+  auto model = std::make_shared<TankModel>(0.4, constraints);
+  auto spline = SplineGenerator(constraints, model, 0.1);
+  auto path = spline.generate({
+    ControlVector(Pose(0, 0, 0), 0.0, 0.0),
+    ControlVector(Pose(0, 2, 0), 0.0, 0.0),
+  });
+
+  ASSERT_NEAR(path.back().vector.pose.x, 0, TEST_EPSILON);
+  ASSERT_NEAR(path.back().vector.pose.y, 2, TEST_EPSILON);
+  ASSERT_NEAR(path.back().vector.pose.yaw, 0, TEST_EPSILON);
+
+  ASSERT_NEAR(path.front().vector.vel, 0.0, TEST_EPSILON);
+  ASSERT_LE(path.back().vector.vel, 0.1);
+}
+
 TEST(model_constraints_test, sharp_turn) {
   auto constraints = Constraints(2.0, 2.0, 1.0);
   auto model = std::make_shared<TankModel>(0.4, constraints);