diff --git a/include/trajopt/constraint/PointAtConstraint.h b/include/trajopt/constraint/PointAtConstraint.h new file mode 100644 index 00000000..457a6344 --- /dev/null +++ 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 index 6ecd7bee..358c632b 100644 --- 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 index 54b57b18..ef123fab 100644 --- 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 index 809897e1..9400b7e4 100644 --- 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(holonomicConstraint)) { ApplyConstraint(opti, x, y, theta, std::get(holonomicConstraint)); + } else if (std::holds_alternative(holonomicConstraint)) { + auto pointAtConstraint = std::get(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(holonomicConstraint)) { ApplyConstraint(opti, x, y, theta, std::get(holonomicConstraint)); diff --git a/src/optimization/OptiSys.h b/src/optimization/OptiSys.h index e06e6fac..40080d20 100644 --- 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 diff --git a/src/optimization/SleipnirOpti.cpp b/src/optimization/SleipnirOpti.cpp index 03dcddaf..5b85f6c1 100644 --- 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 index 428bc30b..a35939e9 100644 --- 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 index b7f1851f..7d065fe2 100644 --- 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 index 16627dd1..c75beb82 100644 --- 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);