Add PointAt constraint (#140)

SleipnirGroup · Apr 3, 2024 · c1a7646 · c1a7646
1 parent 849e534
commit c1a7646
Show file tree

Hide file tree

Showing 9 changed files with 136 additions and 3 deletions.
diff --git a/include/trajopt/constraint/PointAtConstraint.h b/include/trajopt/constraint/PointAtConstraint.h
@@ -0,0 +1,21 @@
+// Copyright (c) TrajoptLib contributors
+
+#pragma once
+
+#include "trajopt/set/IntervalSet1d.h"
+
+namespace trajopt {
+
+/**
+ * Specifies a point on the field at which the robot should point.
+ */
+struct PointAtConstraint {
+  /// field point x
+  double fieldPointX;
+  /// field point y
+  double fieldPointY;
+  /// the allowed robot heading tolerance, must be positive
+  double headingTolerance;
+};
+
+}  // namespace trajopt
diff --git a/include/trajopt/constraint/holonomic/HolonomicConstraint.h b/include/trajopt/constraint/holonomic/HolonomicConstraint.h
@@ -4,12 +4,14 @@
 
 #include "trajopt/constraint/AngularVelocityConstraint.h"
 #include "trajopt/constraint/Constraint.h"
+#include "trajopt/constraint/PointAtConstraint.h"
 #include "trajopt/constraint/holonomic/HolonomicVelocityConstraint.h"
 #include "trajopt/util/AppendVariant.h"
 
 namespace trajopt {
 
 using HolonomicConstraint = decltype(_append_variant(
-    Constraint{}, AngularVelocityConstraint{}, HolonomicVelocityConstraint{}));
+    Constraint{}, AngularVelocityConstraint{}, HolonomicVelocityConstraint{},
+    PointAtConstraint{}));
 
 }  // namespace trajopt
diff --git a/src/lib.rs b/src/lib.rs
@@ -101,6 +101,13 @@ mod ffi {
         fn wpt_x(self: Pin<&mut SwervePathBuilderImpl>, idx: usize, x: f64);
         fn wpt_y(self: Pin<&mut SwervePathBuilderImpl>, idx: usize, y: f64);
         fn wpt_heading(self: Pin<&mut SwervePathBuilderImpl>, idx: usize, heading: f64);
+        fn wpt_point_at(
+            self: Pin<&mut SwervePathBuilderImpl>,
+            idx: usize,
+            field_point_x: f64,
+            field_point_y: f64,
+            heading_tolerance: f64,
+        );
 
         fn sgmt_linear_velocity_direction(
             self: Pin<&mut SwervePathBuilderImpl>,
@@ -135,6 +142,14 @@ mod ffi {
             to_idx: usize,
             heading: f64,
         );
+        fn sgmt_point_at(
+            self: Pin<&mut SwervePathBuilderImpl>,
+            from_idx: usize,
+            to_idx: usize,
+            field_point_x: f64,
+            field_point_y: f64,
+            heading_tolerance: f64,
+        );
 
         fn sgmt_circle_obstacle(
             self: Pin<&mut SwervePathBuilderImpl>,
@@ -265,6 +280,22 @@ impl SwervePathBuilder {
         crate::ffi::SwervePathBuilderImpl::wpt_heading(self.path.pin_mut(), idx, heading);
     }
 
+    pub fn wpt_point_at(
+        &mut self,
+        idx: usize,
+        field_point_x: f64,
+        field_point_y: f64,
+        heading_tolerance: f64,
+    ) {
+        crate::ffi::SwervePathBuilderImpl::wpt_point_at(
+            self.path.pin_mut(),
+            idx,
+            field_point_x,
+            field_point_y,
+            heading_tolerance,
+        )
+    }
+
     pub fn sgmt_linear_velocity_direction(&mut self, from_idx: usize, to_idx: usize, angle: f64) {
         crate::ffi::SwervePathBuilderImpl::sgmt_linear_velocity_direction(
             self.path.pin_mut(),
@@ -330,6 +361,24 @@ impl SwervePathBuilder {
         );
     }
 
+    pub fn sgmt_point_at(
+        &mut self,
+        from_idx: usize,
+        to_idx: usize,
+        field_point_x: f64,
+        field_point_y: f64,
+        heading_tolerance: f64,
+    ) {
+        crate::ffi::SwervePathBuilderImpl::sgmt_point_at(
+            self.path.pin_mut(),
+            from_idx,
+            to_idx,
+            field_point_x,
+            field_point_y,
+            heading_tolerance,
+        )
+    }
+
     pub fn sgmt_circle_obstacle(
         &mut self,
         from_idx: usize,

diff --git a/src/optimization/HolonomicTrajoptUtil.inc b/src/optimization/HolonomicTrajoptUtil.inc
@@ -9,6 +9,7 @@
 #include "trajopt/constraint/Constraint.h"
 #include "trajopt/constraint/HeadingConstraint.h"
 #include "trajopt/constraint/LinePointConstraint.h"
+#include "trajopt/constraint/PointAtConstraint.h"
 #include "trajopt/constraint/PointLineConstraint.h"
 #include "trajopt/constraint/PointPointConstraint.h"
 #include "trajopt/constraint/TranslationConstraint.h"
@@ -47,6 +48,30 @@ void ApplyHolonomicConstraint(Opti& opti, const Expr& x, const Expr& y,
   } else if (std::holds_alternative<LinePointConstraint>(holonomicConstraint)) {
     ApplyConstraint(opti, x, y, theta,
                     std::get<LinePointConstraint>(holonomicConstraint));
+  } else if (std::holds_alternative<PointAtConstraint>(holonomicConstraint)) {
+    auto pointAtConstraint = std::get<PointAtConstraint>(holonomicConstraint);
+    double fieldPointX = pointAtConstraint.fieldPointX;
+    double fieldPointY = pointAtConstraint.fieldPointY;
+    double headingTolerance = pointAtConstraint.headingTolerance;
+    /**
+     * dx,dy = desired heading
+     * ux,uy = unit vector of desired heading
+     * hx,hy = heading
+     * dot = dot product of ux,uy and hx,hy
+     *
+     * constrain dot to cos(1.0), which is colinear
+     * and cos(thetaTolerance)
+     */
+    auto dx = fieldPointX - x;
+    auto dy = fieldPointY - y;
+    auto ux = dx / hypot(dx, dy);  // NOLINT
+    auto uy = dy / hypot(dx, dy);  // NOLINT
+    auto hx = cos(theta);          // NOLINT
+    auto hy = sin(theta);          // NOLINT
+    auto dot = hx * ux + hy * uy;
+
+    ApplyIntervalSet1dConstraint(
+        opti, dot, IntervalSet1d(std::cos(headingTolerance), 1.0));
   } else if (std::holds_alternative<PointLineConstraint>(holonomicConstraint)) {
     ApplyConstraint(opti, x, y, theta,
                     std::get<PointLineConstraint>(holonomicConstraint));

diff --git a/src/optimization/OptiSys.h b/src/optimization/OptiSys.h
@@ -20,8 +20,10 @@ concept ExprSys = requires(Expr expr, const Expr constExpr, double num) {
   expr = sin(constExpr);  // NOLINT
   expr = cos(constExpr);  // NOLINT
 
-  expr = fmin(constExpr, constExpr);  // NOLINT
-  expr = fmax(constExpr, constExpr);  // NOLINT
+  expr = fmin(constExpr, constExpr);   // NOLINT
+  expr = fmax(constExpr, constExpr);   // NOLINT
+  expr = abs(constExpr);               // NOLINT
+  expr = hypot(constExpr, constExpr);  // NOLINT
 };
 
 template <typename Expr, typename Opti>

diff --git a/src/optimization/SleipnirOpti.cpp b/src/optimization/SleipnirOpti.cpp
@@ -63,6 +63,14 @@ SleipnirExpr fmin(const SleipnirExpr& a, const SleipnirExpr& b) {
                       b.expr);
 }
 
+SleipnirExpr abs(const SleipnirExpr& a) {
+  return SleipnirExpr(sleipnir::abs(a.expr));
+}
+
+SleipnirExpr hypot(const SleipnirExpr& a, const SleipnirExpr& b) {
+  return SleipnirExpr(sleipnir::hypot(a.expr, b.expr));
+}
+
 sleipnir::EqualityConstraints operator==(const SleipnirExpr& a,
                                          const SleipnirExpr& b) {
   return a.expr == b.expr;

diff --git a/src/optimization/SleipnirOpti.h b/src/optimization/SleipnirOpti.h
@@ -39,6 +39,8 @@ struct SleipnirExpr {
 
   friend SleipnirExpr fmin(const SleipnirExpr& a, const SleipnirExpr& b);
   friend SleipnirExpr fmax(const SleipnirExpr& a, const SleipnirExpr& b);
+  friend SleipnirExpr abs(const SleipnirExpr& a);
+  friend SleipnirExpr hypot(const SleipnirExpr& a, const SleipnirExpr& b);
 
   friend sleipnir::EqualityConstraints operator==(const SleipnirExpr& a,
                                                   const SleipnirExpr& b);

diff --git a/src/trajoptlibrust.cpp b/src/trajoptlibrust.cpp
@@ -155,6 +155,15 @@ void SwervePathBuilderImpl::wpt_heading(size_t idx, double heading) {
       idx, trajopt::HeadingConstraint{trajopt::IntervalSet1d(heading)});
 }
 
+void SwervePathBuilderImpl::wpt_point_at(size_t idx, double field_point_x,
+                                         double field_point_y,
+                                         double heading_tolerance) {
+  path.WptConstraint(
+      idx, trajopt::PointAtConstraint{.fieldPointX = field_point_x,
+                                      .fieldPointY = field_point_y,
+                                      .headingTolerance = heading_tolerance});
+}
+
 void SwervePathBuilderImpl::sgmt_linear_velocity_direction(size_t from_idx,
                                                            size_t to_idx,
                                                            double angle) {
@@ -203,6 +212,17 @@ void SwervePathBuilderImpl::sgmt_heading(size_t from_idx, size_t to_idx,
   path.SgmtConstraint(from_idx, to_idx, trajopt::HeadingConstraint{heading});
 }
 
+void SwervePathBuilderImpl::sgmt_point_at(size_t from_idx, size_t to_idx,
+                                          double field_point_x,
+                                          double field_point_y,
+                                          double heading_tolerance) {
+  path.SgmtConstraint(
+      from_idx, to_idx,
+      trajopt::PointAtConstraint{.fieldPointX = field_point_x,
+                                 .fieldPointY = field_point_y,
+                                 .headingTolerance = heading_tolerance});
+}
+
 void SwervePathBuilderImpl::sgmt_circle_obstacle(size_t from_idx, size_t to_idx,
                                                  double x, double y,
                                                  double radius) {

diff --git a/src/trajoptlibrust.h b/src/trajoptlibrust.h
@@ -37,6 +37,8 @@ class SwervePathBuilderImpl {
   void wpt_x(size_t idx, double x);
   void wpt_y(size_t idx, double y);
   void wpt_heading(size_t idx, double heading);
+  void wpt_point_at(size_t idx, double field_point_x, double field_point_y,
+                    double heading_tolerance);
 
   void sgmt_linear_velocity_direction(size_t from_idx, size_t to_idx,
                                       double angle);
@@ -49,6 +51,8 @@ class SwervePathBuilderImpl {
   void sgmt_x(size_t from_idx, size_t to_idx, double x);
   void sgmt_y(size_t from_idx, size_t to_idx, double y);
   void sgmt_heading(size_t from_idx, size_t to_idx, double heading);
+  void sgmt_point_at(size_t from_idx, size_t to_idx, double field_point_x,
+                     double field_point_y, double heading_tolerance);
 
   void sgmt_circle_obstacle(size_t from_idx, size_t to_idx, double x, double y,
                             double radius);