├── .clang-format ├── .gitignore ├── .gitmodules ├── .travis.yml ├── LICENSE ├── Pathfinder-Java ├── build.gradle └── src │ ├── main │ └── java │ │ └── grpl │ │ └── pathfinder │ │ ├── Vec2.java │ │ ├── coupled │ │ ├── CoupledCausalTrajGen.java │ │ ├── CoupledChassis.java │ │ ├── CoupledConfigurationState.java │ │ ├── CoupledKinematicState.java │ │ ├── CoupledState.java │ │ ├── CoupledWheelState.java │ │ └── package-info.java │ │ ├── package-info.java │ │ ├── path │ │ ├── AbstractCurve2d.java │ │ ├── AbstractSpline2d.java │ │ ├── Arc2d.java │ │ ├── ArcParameterizer.java │ │ ├── Curve2d.java │ │ ├── HermiteCubic.java │ │ ├── HermiteFactory.java │ │ ├── HermiteQuintic.java │ │ ├── Spline2d.java │ │ └── package-info.java │ │ ├── profile │ │ ├── AbstractNativeProfile.java │ │ ├── AbstractProfile.java │ │ ├── Profile.java │ │ ├── TrapezoidalProfile.java │ │ └── package-info.java │ │ ├── transmission │ │ ├── AbstractDcTransmission.java │ │ ├── DcMotor.java │ │ ├── DcTransmission.java │ │ ├── JavaDcTransmissionAdapter.java │ │ └── package-info.java │ │ └── util │ │ ├── INativeResource.java │ │ ├── NativeResource.java │ │ └── PathfinderJNI.java │ ├── native │ ├── cpp │ │ ├── coupled │ │ │ ├── coupled_causaltrajgen.cpp │ │ │ ├── coupled_chassis.cpp │ │ │ └── jnicoupled.cpp │ │ ├── jnihandle.cpp │ │ ├── jniutil.cpp │ │ ├── path │ │ │ ├── abstract_curve.cpp │ │ │ ├── abstract_spline.cpp │ │ │ ├── arc.cpp │ │ │ ├── arc_parameterizer.cpp │ │ │ └── hermite.cpp │ │ ├── profile │ │ │ ├── abstract_profile.cpp │ │ │ ├── jniprofile.cpp │ │ │ ├── profile_adapter.cpp │ │ │ └── trapezoidal_profile.cpp │ │ └── transmission │ │ │ ├── abstract_dc_transmission.cpp │ │ │ ├── dc_motor.cpp │ │ │ └── dc_transmission_adapter.cpp │ └── include │ │ ├── coupled │ │ └── jnicoupled.h │ │ ├── jnieigen.h │ │ ├── jnihandle.h │ │ ├── jniutil.h │ │ └── profile │ │ └── jniprofile.h │ └── test │ └── java │ └── grpl │ └── pathfinder │ ├── coupled │ └── CoupledTest.java │ ├── path │ ├── ArcParameterizerTest.java │ ├── HermiteCubicTest.java │ └── HermiteQuinticTest.java │ ├── profile │ └── TrapezoidalProfileTest.java │ └── util │ └── NativeResourceTest.java ├── Pathfinder ├── build.gradle ├── plot.gradle └── src │ ├── bench │ ├── bench_main.cpp │ ├── coupled │ │ └── bench_cdt_fullpipe.cpp │ ├── path │ │ └── bench_arc_param.cpp │ └── profile │ │ └── bench_trapezoidal.cpp │ ├── include │ └── grpl │ │ ├── pf.h │ │ └── pf │ │ ├── constants.h │ │ ├── coupled │ │ ├── causal_trajectory_generator.h │ │ ├── chassis.h │ │ └── state.h │ │ ├── path │ │ ├── arc.h │ │ ├── arc_parameterizer.h │ │ ├── augmented_arc.h │ │ ├── curve.h │ │ ├── hermite.h │ │ └── spline.h │ │ ├── pathfinder.h │ │ ├── profile │ │ ├── profile.h │ │ └── trapezoidal.h │ │ └── transmission │ │ └── dc.h │ ├── test │ ├── coupled │ │ └── cdt.cpp │ ├── path │ │ ├── test_arc.cpp │ │ ├── test_arc_param.cpp │ │ └── test_hermite.cpp │ ├── profile │ │ └── test_trapezoidal.cpp │ ├── test_main.cpp │ └── test_util.h │ └── testplot │ ├── arc.plt │ ├── arc_param.plt │ ├── cdt_model.plt │ ├── hermite.plt │ ├── hermite_base.gp │ ├── settings.gp │ └── trapezoidal.plt ├── README.md ├── appveyor.yml ├── azure-init.yml ├── azure-pipelines.yml ├── azure-steps.yml ├── build.gradle ├── gradle └── wrapper │ ├── gradle-wrapper.jar │ └── gradle-wrapper.properties ├── gradlew ├── gradlew.bat ├── libs ├── bench-athena.patch └── build.gradle ├── settings.gradle └── vendordeps.gradle /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AlignOperands: 'true' 4 | ColumnLimit: '110' 5 | NamespaceIndentation: Inner 6 | TabWidth: '2' 7 | UseTab: Never 8 | AllowShortFunctionsOnASingleLine: Inline 9 | AlignConsecutiveAssignments: 'true' 10 | AlignConsecutiveDeclarations: 'true' 11 | 12 | ... 13 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # OS Files 2 | .DS_Store 3 | .DS_Store? 4 | ._* 5 | .Spotlight-V100 6 | .AppleDouble 7 | .LSOverride 8 | .Trashes 9 | ehthumbs.db 10 | Thumbs.db 11 | 12 | # Backup files 13 | *~ 14 | 15 | # IDE 16 | *.sln 17 | *.vcxproj 18 | *.vcxproj.* 19 | .vs/ 20 | .vscode/ 21 | .idea/ 22 | *.iml 23 | *.ipr 24 | *.iws 25 | out/ 26 | 27 | # Gradle 28 | .gradle/ 29 | build/ 30 | 31 | # Buildship 32 | .project 33 | .settings/ 34 | bin/ 35 | 36 | .classpath 37 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "libs/googletest"] 2 | path = libs/googletest 3 | url = https://github.com/google/googletest 4 | [submodule "libs/eigen"] 5 | path = libs/eigen 6 | url = https://github.com/eigenteam/eigen-git-mirror.git 7 | [submodule "libs/googlebench"] 8 | path = libs/googlebench 9 | url = https://github.com/google/benchmark 10 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | 3 | jdk: 4 | - oraclejdk8 5 | 6 | os: linux 7 | dist: trusty 8 | 9 | matrix: 10 | include: 11 | - env: CI_COMPILER=CLANG 12 | sudo: required # Clang requires sudo for ptrace with address sanitizer 13 | addons: 14 | apt: 15 | packages: 16 | - clang-3.8 17 | - g++-5 18 | - g++-5-multilib 19 | - gcc-5-multilib 20 | - gcc-multilib 21 | - gnuplot 22 | sources: 23 | - ubuntu-toolchain-r-test 24 | - llvm-toolchain-precise-3.8 25 | 26 | - env: CI_COMPILER=GCC 27 | addons: 28 | apt: 29 | packages: 30 | - g++-5 31 | - g++-5-multilib 32 | - gcc-5-multilib 33 | - gcc-multilib 34 | - gnuplot 35 | sources: 36 | - ubuntu-toolchain-r-test 37 | 38 | install: 39 | - sudo update-alternatives --remove-all gcc || true 40 | - sudo update-alternatives --remove-all g++ || true 41 | - sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-5 10 || true 42 | - sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-5 10 || true 43 | 44 | script: 45 | - ./gradlew build --stacktrace --console=plain -PwithBench 46 | 47 | notifications: 48 | email: false -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2018 Grapple Robotics. All rights reserved. 2 | 3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | 5 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | 3. Neither the name of Grapple nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /Pathfinder-Java/build.gradle: -------------------------------------------------------------------------------- 1 | plugins { 2 | id 'cpp' 3 | id 'java' 4 | id 'edu.wpi.first.GradleJni' version '0.3.0' 5 | } 6 | 7 | sourceCompatibility = '1.8' 8 | targetCompatibility = '1.8' 9 | 10 | repositories { 11 | mavenCentral() 12 | } 13 | 14 | model { 15 | components { 16 | pathfinderjni(JniNativeLibrarySpec) { 17 | enableCheckTask true 18 | javaCompileTasks << compileJava 19 | 20 | sources { 21 | cpp { 22 | source { 23 | srcDir 'src/native/cpp' 24 | include '**/*.cpp' 25 | } 26 | exportedHeaders { 27 | srcDirs 'src/native/include' 28 | } 29 | lib project: ':Pathfinder', library: 'pathfinder', linkage: 'static' 30 | lib project: ':libs', library: 'eigen', linkage: 'api' 31 | } 32 | } 33 | } 34 | } 35 | } 36 | 37 | dependencies { 38 | testImplementation( 39 | 'org.junit.jupiter:junit-jupiter-api:5.1.0' 40 | ) 41 | testRuntimeOnly( 42 | 'org.junit.jupiter:junit-jupiter-engine:5.1.0' 43 | ) 44 | } 45 | 46 | doxygen { 47 | executables { 48 | doxygen version : '1.8.13' 49 | } 50 | 51 | generate_html true 52 | javadoc_autobrief true 53 | source project.file('src/main/java') 54 | } 55 | 56 | task sourcesJar(type: Jar) { 57 | dependsOn classes 58 | classifier = 'sources' 59 | } 60 | 61 | task javadocJar(type: Jar) { 62 | dependsOn javadoc 63 | from javadoc.destinationDir 64 | } 65 | 66 | task zipDoxygen(type: Zip) { 67 | from doxygen 68 | baseName = "Pathfinder" 69 | classifier = "doxygen" 70 | } 71 | 72 | project.afterEvaluate { 73 | publishing { 74 | publications { 75 | pathfinderJava(MavenPublication) { 76 | artifactId 'Pathfinder-Java' 77 | 78 | artifact jar 79 | artifact sourcesJar { 80 | classifier 'sources' 81 | } 82 | artifact javadocJar { 83 | classifier 'javadoc' 84 | } 85 | artifact zipDoxygen { 86 | classifier 'doxygen' 87 | } 88 | } 89 | pathfinderJni(MavenPublication) { 90 | artifactId 'Pathfinder-JNI' 91 | 92 | binaryArtifacts(it, "pathfinderjni", true) 93 | } 94 | } 95 | } 96 | } 97 | 98 | test { 99 | useJUnitPlatform() 100 | // TODO: make this not-shit 101 | def libPath = new File(project.buildDir, "libs/pathfinderjni/shared/${edu.wpi.first.toolchain.NativePlatforms.desktop}/debug").absolutePath 102 | systemProperties.put("java.library.path", libPath) 103 | testLogging { 104 | events "passed", "skipped", "failed" 105 | } 106 | } 107 | 108 | test.dependsOn assemble -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/Vec2.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder; 2 | 3 | public class Vec2 { 4 | 5 | private final double x, y; 6 | 7 | private Vec2(double x, double y) { 8 | this.x = x; 9 | this.y = y; 10 | } 11 | 12 | public double magnitude() { 13 | return Math.sqrt(x*x + y*y); 14 | } 15 | 16 | public double angle() { 17 | return Math.atan2(y, x); 18 | } 19 | 20 | public double x() { 21 | return x; 22 | } 23 | 24 | public double y() { 25 | return y; 26 | } 27 | 28 | public double[] xy() { 29 | return new double[] { x, y }; 30 | } 31 | 32 | public Vec2 unit() { 33 | double mag = magnitude(); 34 | return Vec2.cartesian(x / mag, y / mag); 35 | } 36 | 37 | public Vec2 plus(Vec2 other) { 38 | return new Vec2(x() + other.x(), y() + other.y()); 39 | } 40 | 41 | public Vec2 minus(Vec2 other) { 42 | return new Vec2(x() - other.x(), y() - other.y()); 43 | } 44 | 45 | public Vec2 times(double scalar) { 46 | return new Vec2(x() * scalar, y() * scalar); 47 | } 48 | 49 | public static Vec2 cartesian(double x, double y) { 50 | return new Vec2(x, y); 51 | } 52 | 53 | public static Vec2 cartesian(double[] xy) { 54 | return cartesian(xy[0], xy[1]); 55 | } 56 | 57 | public static Vec2 polar(double mag, double theta) { 58 | return new Vec2(mag * Math.cos(theta), mag * Math.sin(theta)); 59 | } 60 | 61 | public static Vec2 zero() { 62 | return new Vec2(0, 0); 63 | } 64 | 65 | @Override 66 | public boolean equals(Object o) { 67 | if (!(o instanceof Vec2)) 68 | return false; 69 | Vec2 vec2 = (Vec2) o; 70 | return Math.abs(vec2.x() - x()) < 1e-6 && 71 | Math.abs(vec2.y() - y()) < 1e-6; 72 | } 73 | } 74 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/coupled/CoupledCausalTrajGen.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.coupled; 2 | 3 | import grpl.pathfinder.path.Curve2d; 4 | import grpl.pathfinder.profile.Profile; 5 | import grpl.pathfinder.util.INativeResource; 6 | import grpl.pathfinder.util.NativeResource; 7 | 8 | import java.util.List; 9 | 10 | /** 11 | * Causal trajectory generator, given the chassis, path and motion profile. 12 | *

13 | * This class provides the necessary plumbing in order to generate trajectories from a given path, 14 | * applying motion and timing information based on the limits and constraints of a drivetrain. The 15 | * generator applies a motion profile to a path, while fitting within given constraints. 16 | *

17 | *

18 | * This generator is causal and will generate a single state (output) with knowledge only of the current 19 | * state of the system. This generator can be used on-the-fly, and is primarily advantageous in speed and 20 | * flexibility. The cost is accuracy towards the end of the path, as a sudden deceleration may be required 21 | * with insufficient time steps, as the end velocity may not be zero. 22 | *

23 | *

24 | * It is recommended to use this generator if on-the-fly generation is required, or memory and 25 | * computational resources are sparse. This generator also enables the ability to provide feedback 26 | * information on chassis kinematics to the next generation step, making it possible to embed a feedback 27 | * loop into the generation phase. 28 | *

29 | */ 30 | public class CoupledCausalTrajGen extends NativeResource { 31 | 32 | private CoupledChassis chassis; 33 | private long nativeCurveBuffer = 0L; 34 | 35 | private Profile profile; 36 | private NativeResource nativeProfile; 37 | 38 | public static final int LEFT = 0; 39 | public static final int RIGHT = 1; 40 | 41 | /** 42 | * Create a new Causal Trajectory Generator for a given chassis. 43 | * 44 | * @param chassis The coupled chassis, used to provide limits for the trajectory the trajectory 45 | * kinematics. 46 | */ 47 | public CoupledCausalTrajGen(CoupledChassis chassis) { 48 | super(allocate()); 49 | this.chassis = chassis; 50 | } 51 | 52 | /** 53 | * Configure the curves and profile to use for the Causal Trajectory Generator. 54 | *

55 | * This must be called before any calls to {@link #generate(CoupledState, double)} 56 | *

57 | * 58 | * @param curves The collection of curves to use for the trajectory (the path) 59 | * @param profile The profile to use for the trajectory. {@link grpl.pathfinder.profile.TrapezoidalProfile} 60 | * is recommended. 61 | */ 62 | public void configure(List curves, Profile profile) { 63 | this.profile = profile; 64 | 65 | if (profile instanceof NativeResource) { 66 | this.nativeProfile = (NativeResource) profile; 67 | } else { 68 | throw new UnsupportedOperationException("Non-native profiles are not yet supported. See issue #11"); 69 | } 70 | 71 | if (this.nativeCurveBuffer != 0L) 72 | releaseBuffer(this.nativeCurveBuffer); 73 | this.nativeCurveBuffer = acquireBuffer(); 74 | 75 | pack(this.nativeCurveBuffer, curves); 76 | } 77 | 78 | /** 79 | * Generate the next state of the trajectory given the current state. 80 | *

81 | * Requires a call to {@link #configure(List, Profile)} to setup the system. 82 | *

83 | * 84 | * @param lastState The current ("last") state of the trajectory. If this is the first call to 85 | * generate, this may be considered the "initial conditions". 86 | * @param time The time of the next point (lastState.time + dt), in seconds. 87 | */ 88 | public CoupledState generate(CoupledState lastState, double time) { 89 | return new CoupledState(generateNative(nativeHandle(), chassis.nativeHandle(), nativeCurveBuffer, nativeProfile.nativeHandle(), lastState.toArray(), time)); 90 | } 91 | 92 | @Override 93 | public void close() { 94 | if (nativeCurveBuffer != 0L) 95 | releaseBuffer(nativeCurveBuffer); 96 | 97 | free(nativeHandle()); 98 | zeroHandle(); 99 | } 100 | 101 | private static void pack(long buffer, List curves) { 102 | for (Curve2d curve : curves) { 103 | if (curve instanceof INativeResource) { 104 | enqueueNative(buffer, ((INativeResource) curve).nativeHandle()); 105 | } else { 106 | enqueueAdapter(buffer, curve); 107 | } 108 | } 109 | } 110 | 111 | /* JNI */ 112 | private static native long allocate(); 113 | 114 | private static native void free(long handle); 115 | 116 | private static native double[] generateNative(long h, long chassisHandle, long buf, long prof, double[] arr, double time); 117 | 118 | // Curve buffer 119 | private static native long acquireBuffer(); 120 | 121 | private static native void enqueueNative(long bufferHandle, long curveHandle); 122 | 123 | private static native void enqueueAdapter(long bufferHandle, Curve2d curve); 124 | 125 | private static native void releaseBuffer(long bufferHandle); 126 | } 127 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/coupled/CoupledChassis.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.coupled; 2 | 3 | import grpl.pathfinder.transmission.DcTransmission; 4 | import grpl.pathfinder.transmission.JavaDcTransmissionAdapter; 5 | import grpl.pathfinder.util.NativeResource; 6 | 7 | /** 8 | * Mathematical model representation of a coupled (tank / differential) drivetrain. 9 | *

10 | * Chassis contains members for the transmissions (motors), as well as other configurations 11 | * regarding the chassis (track radius, wheel radius, mass, etc). 12 | *

13 | *

14 | * The chassis mirrors the physical "layout" of the drivetrain. 15 | *

16 | *

17 | * {@link CoupledCausalTrajGen} 18 | */ 19 | public class CoupledChassis extends NativeResource { 20 | 21 | private DcTransmission transLeft, transRight; 22 | private NativeResource nativeLeft, nativeRight; 23 | private double mass, trackRadius, wheelRadius; 24 | 25 | /** 26 | * Construct a coupled chassis 27 | * 28 | * @param left The left side transmission. 29 | * @param right The right side transmission. 30 | * @param wheelRadius The wheel radius, in metres. Should be emperically measured 31 | * for best performance. Note that both the left and right 32 | * transmission wheels must be the same radius. 33 | * @param trackRadius The track radius, a.k.a half the chassis width, in metres. 34 | * Measured as half the distance between the centres of the left 35 | * and right transmissions at their points of contact with the ground. 36 | * @param mass The mass of the chassis, in kilograms. 37 | */ 38 | public CoupledChassis(DcTransmission left, DcTransmission right, double wheelRadius, double trackRadius, double mass) { 39 | super(allocateStub()); 40 | this.transLeft = left; 41 | this.transRight = right; 42 | this.nativeLeft = left instanceof NativeResource ? (NativeResource) left : new JavaDcTransmissionAdapter(left); 43 | this.nativeRight = right instanceof NativeResource ? (NativeResource) right : new JavaDcTransmissionAdapter(right); 44 | this.mass = mass; 45 | this.trackRadius = trackRadius; 46 | this.wheelRadius = wheelRadius; 47 | construct(nativeHandle(), nativeLeft.nativeHandle(), nativeRight.nativeHandle(), wheelRadius, trackRadius, mass); 48 | } 49 | 50 | /** 51 | * @return The mass of the chassis, in kilograms 52 | */ 53 | public double getMass() { 54 | return mass; 55 | } 56 | 57 | /** 58 | * @return The track radius (half the chassis width) in metres. Measured 59 | * between the left and right transmissions. 60 | */ 61 | public double getTrackRadius() { 62 | return trackRadius; 63 | } 64 | 65 | /** 66 | * @return The wheel radius, in metres. 67 | */ 68 | public double getWheelRadius() { 69 | return wheelRadius; 70 | } 71 | 72 | /** 73 | * @return The left-side transmission of the chassis. 74 | */ 75 | public DcTransmission getLeft() { 76 | return transLeft; 77 | } 78 | 79 | /** 80 | * @return The right-side transmission of the chassis. 81 | */ 82 | public DcTransmission getRight() { 83 | return transRight; 84 | } 85 | 86 | /** 87 | * Calculate the absolute linear (translational) velocity limit of the chassis in metres 88 | * per second (ms^-1). 89 | *

90 | * This calculation relates purely to the free-speed of the motors, meaning for a fully 91 | * constrained calculation, {@link #accelerationLimit(CoupledConfigurationState, double, double)} 92 | * should be called and used to constrain the velocity if necessary. 93 | *

94 | * 95 | * @param configuration The configuration of the chassis 96 | * @param curvature The instantaneous curvature, in metres^-1, expected of the chassis, 97 | * such as the curvature of a path being followed. 98 | * @return The absolute linear (translational) velocity limit in metres per second (ms^-2). 99 | */ 100 | public double linearVelocityLimit(CoupledConfigurationState configuration, double curvature) { 101 | return linvelLimit(nativeHandle(), configuration.toArray(), curvature); 102 | } 103 | 104 | /** 105 | * Calculate the minimum and maximum linear (translational) acceleration limits of the chassis, 106 | * in metres per second per second (ms^-2). 107 | *

108 | * This calculation uses torque limits of the transmissions, meaning speed limits 109 | * are not directly taken into account. For a fully constrained representation, 110 | *

111 | * {@link #linearVelocityLimit(CoupledConfigurationState, double)} must also be called to constrain 112 | * if necessary. 113 | * 114 | * @param configuration The configuration of the chassis 115 | * @param curvature The instantaneous curvature, in metres^-1, expected of the chassis, 116 | * such as the curvature of a path being followed. 117 | * @param velocity The current linear velocity of the chassis, in metres per second (ms^-1). 118 | * @return A pair, ordered [min, max], of the linear acceleration limits, in metres per second 119 | * per second (ms^-2). 120 | */ 121 | public double[] accelerationLimit(CoupledConfigurationState configuration, double curvature, double velocity) { 122 | return accLimit(nativeHandle(), configuration.toArray(), curvature, velocity); 123 | } 124 | 125 | /** 126 | * Split a centre state of this chassis into the left and right transmission state components. 127 | * 128 | * @return A pair, ordered [left, right], of {@link CoupledWheelState}. 129 | */ 130 | public CoupledWheelState[] split(CoupledState centre) { 131 | double[] left = new double[9]; 132 | double[] right = new double[9]; 133 | 134 | splitNative(nativeHandle(), centre.toArray(), left, right); 135 | 136 | return new CoupledWheelState[]{ 137 | new CoupledWheelState(left), new CoupledWheelState(right) 138 | }; 139 | } 140 | 141 | @Override 142 | public void close() { 143 | free(nativeHandle()); 144 | zeroHandle(); 145 | 146 | if (nativeLeft instanceof JavaDcTransmissionAdapter) 147 | nativeLeft.close(); 148 | if (nativeRight instanceof JavaDcTransmissionAdapter) 149 | nativeRight.close(); 150 | } 151 | 152 | /* JNI */ 153 | private static native long allocateStub(); 154 | 155 | private static native void construct(long h, long left, long right, double wheelR, double trackR, double mass); 156 | 157 | private static native void free(long h); 158 | 159 | private static native double linvelLimit(long h, double[] config, double curvature); 160 | 161 | private static native double[] accLimit(long h, double[] config, double curvature, double vel); 162 | 163 | private static native void splitNative(long h, double[] centre, double[] outLeft, double[] outRight); 164 | } 165 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/coupled/CoupledConfigurationState.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.coupled; 2 | 3 | import grpl.pathfinder.Vec2; 4 | 5 | /** 6 | * Drivetrain configuration state, describing the configuration of the chassis. 7 | *

8 | * Formally, the 'configuration' of a body is all variables required to fully define the position 9 | * of the body and its manipulators. For a coupled drivetrain, this is simply its position in the 10 | * the work space, as well as its heading. In the case of the coupled drivetrain, this may be referred 11 | * to as the "transform" of the chassis. 12 | *

13 | */ 14 | public class CoupledConfigurationState { 15 | 16 | /** 17 | * The position of the centre of the drivetrain, in metres. 18 | */ 19 | public Vec2 position; 20 | 21 | /** 22 | * The heading of the drivetrain, in radians. 23 | */ 24 | public double heading; 25 | 26 | public CoupledConfigurationState() { 27 | this.position = Vec2.zero(); 28 | this.heading = 0; 29 | } 30 | 31 | public CoupledConfigurationState(Vec2 position, double heading) { 32 | this.position = position; 33 | this.heading = heading; 34 | } 35 | 36 | public CoupledConfigurationState(double x, double y, double heading) { 37 | this(Vec2.cartesian(x, y), heading); 38 | } 39 | 40 | public CoupledConfigurationState(double[] arr) { 41 | this(arr[0], arr[1], arr[2]); 42 | } 43 | 44 | public double[] toArray() { 45 | return new double[]{position.x(), position.y(), heading}; 46 | } 47 | 48 | @Override 49 | public boolean equals(Object o) { 50 | if (!(o instanceof CoupledConfigurationState)) 51 | return false; 52 | CoupledConfigurationState k2 = (CoupledConfigurationState) o; 53 | return k2.position.equals(position) && 54 | Math.abs(k2.heading - heading) < 1e-6; 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/coupled/CoupledKinematicState.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.coupled; 2 | 3 | /** 4 | * Drivetrain kinematic state, describing the movement and motion of the chassis. 5 | */ 6 | public class CoupledKinematicState { 7 | 8 | /** 9 | * The distance covered by the drivetrain, in metres. 10 | */ 11 | public double distance; 12 | 13 | /** 14 | * The linear velocity of the drivetrain, in metres per second (ms^-1). 15 | */ 16 | public double velocity; 17 | 18 | /** 19 | * The linear acceleration of the drivetrain, in metres per second 20 | * per second (ms^-2). 21 | */ 22 | public double acceleration; 23 | 24 | public CoupledKinematicState() { 25 | this.distance = 0; 26 | this.velocity = 0; 27 | this.acceleration = 0; 28 | } 29 | 30 | public CoupledKinematicState(double dist, double vel, double acc) { 31 | this.distance = dist; 32 | this.velocity = vel; 33 | this.acceleration = acc; 34 | } 35 | 36 | public CoupledKinematicState(double[] arr) { 37 | this(arr[0], arr[1], arr[2]); 38 | } 39 | 40 | public double[] toArray() { 41 | return new double[]{distance, velocity, acceleration}; 42 | } 43 | 44 | @Override 45 | public boolean equals(Object o) { 46 | if (!(o instanceof CoupledKinematicState)) 47 | return false; 48 | CoupledKinematicState k2 = (CoupledKinematicState) o; 49 | return Math.abs(k2.distance - distance) < 1e-6 && 50 | Math.abs(k2.velocity - velocity) < 1e-6 && 51 | Math.abs(k2.acceleration - acceleration) < 1e-6; 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/coupled/CoupledState.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.coupled; 2 | 3 | /** 4 | * The state of a coupled drivetrain at any point in time, as a single state within a 5 | * trajectory. 6 | */ 7 | public class CoupledState { 8 | 9 | /** 10 | * The time point of this state, in seconds. 11 | */ 12 | public double time; 13 | 14 | /** 15 | * The instantaneous curvature of the state, in m^-1 16 | */ 17 | public double curvature; 18 | 19 | /** 20 | * The instantaneous change in curvature of the state, in m^-2 (dk/ds) 21 | */ 22 | public double dcurvature; 23 | 24 | /** 25 | * The configuration of the chassis at the time of the state. 26 | */ 27 | public CoupledConfigurationState config; 28 | 29 | /** 30 | * The kinematics of the chassis at the time of the state. 31 | */ 32 | public CoupledKinematicState kinematics; 33 | 34 | public boolean finished; 35 | 36 | public CoupledState() { 37 | this.time = 0; 38 | this.curvature = 0; 39 | this.dcurvature = 0; 40 | this.config = new CoupledConfigurationState(); 41 | this.kinematics = new CoupledKinematicState(); 42 | this.finished = false; 43 | } 44 | 45 | public CoupledState(double time, double curvature, double dcurvature, 46 | CoupledConfigurationState config, CoupledKinematicState kin, boolean finished) { 47 | this.time = time; 48 | this.curvature = curvature; 49 | this.dcurvature = dcurvature; 50 | this.config = config; 51 | this.kinematics = kin; 52 | this.finished = finished; 53 | } 54 | 55 | public CoupledState(double[] arr) { 56 | this.time = arr[0]; 57 | this.curvature = arr[1]; 58 | this.dcurvature = arr[2]; 59 | 60 | double[] con = new double[]{arr[3], arr[4], arr[5]}; 61 | double[] kin = new double[]{arr[6], arr[7], arr[8]}; 62 | 63 | this.finished = arr[9] > 0.5; 64 | 65 | this.kinematics = new CoupledKinematicState(kin); 66 | this.config = new CoupledConfigurationState(con); 67 | } 68 | 69 | public double[] toArray() { 70 | double[] kinArr = kinematics.toArray(); 71 | double[] conArr = config.toArray(); 72 | 73 | double[] arr = new double[4 + kinArr.length + conArr.length]; 74 | arr[0] = time; 75 | arr[1] = curvature; 76 | arr[2] = dcurvature; 77 | for (int i = 0; i < conArr.length; i++) 78 | arr[3 + i] = conArr[i]; 79 | for (int i = 0; i < kinArr.length; i++) 80 | arr[6 + i] = kinArr[i]; 81 | arr[9] = finished ? 1.0 : 0.0; 82 | return arr; 83 | } 84 | 85 | } 86 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/coupled/CoupledWheelState.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.coupled; 2 | 3 | import grpl.pathfinder.Vec2; 4 | 5 | /** 6 | * The state of a wheel (side) of the coupled drivetrain at any point in time, primarily 7 | * for use with encoders / other following regimes. 8 | */ 9 | public class CoupledWheelState { 10 | 11 | /** 12 | * The time point of this state, in seconds. 13 | */ 14 | public double time; 15 | 16 | /** 17 | * The position of the wheel at the time of the state. 18 | */ 19 | public Vec2 position; 20 | 21 | /** 22 | * The kinematics of the wheel at the time of the state. Note this is linear, not rotational. 23 | */ 24 | public CoupledKinematicState kinematics; 25 | 26 | /** 27 | * The voltage applied to the transmission connected to this wheel, in Volts 28 | */ 29 | public double voltage; 30 | 31 | /** 32 | * The current drawn by the transmission connected to this wheel, in Amperes. 33 | */ 34 | public double current; 35 | 36 | public boolean finished; 37 | 38 | public CoupledWheelState(double time, Vec2 pos, CoupledKinematicState kinem, 39 | double voltage, double current, boolean finished) { 40 | this.time = time; 41 | this.position = pos; 42 | this.kinematics = kinem; 43 | this.voltage = voltage; 44 | this.current = current; 45 | this.finished = finished; 46 | } 47 | 48 | public CoupledWheelState(double[] arr) { 49 | this.time = arr[0]; 50 | 51 | double[] posArr = new double[]{arr[1], arr[2]}; 52 | double[] kinArr = new double[]{arr[3], arr[4], arr[5]}; 53 | 54 | this.position = Vec2.cartesian(posArr); 55 | this.kinematics = new CoupledKinematicState(kinArr); 56 | 57 | this.voltage = arr[6]; 58 | this.current = arr[7]; 59 | this.finished = arr[8] > 0.5; 60 | } 61 | 62 | public double[] toArray() { 63 | double[] posArr = position.xy(); 64 | double[] kinArr = kinematics.toArray(); 65 | 66 | double[] arr = new double[4 + kinArr.length + posArr.length]; 67 | arr[0] = time; 68 | for (int i = 0; i < posArr.length; i++) 69 | arr[1 + i] = posArr[i]; 70 | for (int i = 0; i < kinArr.length; i++) 71 | arr[3 + i] = kinArr[i]; 72 | 73 | arr[6] = voltage; 74 | arr[7] = current; 75 | arr[8] = finished ? 1.0 : 0.0; 76 | return arr; 77 | } 78 | 79 | } 80 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/coupled/package-info.java: -------------------------------------------------------------------------------- 1 | /** 2 | * Coupled / tank / differential drivetrain models. 3 | * 4 | * The grpl.pathfinder.coupled package contains all models and other structures for use 5 | * with coupled-style drivetrains. 6 | * 7 | * A coupled-style drivetrain defines a drivetrain with individually controlled, parallel 8 | * left and right sides, also known as a tank drive or differential drive. May also refer 9 | * to skid-steer systems. 10 | */ 11 | package grpl.pathfinder.coupled; -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/package-info.java: -------------------------------------------------------------------------------- 1 | /** 2 | * The Grapple Robotics Pathfinder library. 3 | */ 4 | package grpl.pathfinder; -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/path/AbstractCurve2d.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.path; 2 | 3 | import grpl.pathfinder.Vec2; 4 | import grpl.pathfinder.util.NativeResource; 5 | 6 | public class AbstractCurve2d extends NativeResource implements Curve2d { 7 | public AbstractCurve2d(long handle) { 8 | super(handle); 9 | } 10 | 11 | @Override 12 | public Vec2 position(double s) { 13 | return Vec2.cartesian(position(nativeHandle(), s)); 14 | } 15 | 16 | @Override 17 | public Vec2 derivative(double s) { 18 | return Vec2.cartesian(derivative(nativeHandle(), s)); 19 | } 20 | 21 | @Override 22 | public Vec2 rotation(double s) { 23 | return Vec2.cartesian(rotation(nativeHandle(), s)); 24 | } 25 | 26 | @Override 27 | public double curvature(double s) { 28 | return curvature(nativeHandle(), s); 29 | } 30 | 31 | @Override 32 | public double dcurvature(double s) { 33 | return dcurvature(nativeHandle(), s); 34 | } 35 | 36 | @Override 37 | public double length() { 38 | return length(nativeHandle()); 39 | } 40 | 41 | @Override 42 | public void close() { 43 | free(nativeHandle()); 44 | zeroHandle(); 45 | } 46 | 47 | /* JNI */ 48 | private static native double[] position(long h, double t); 49 | private static native double[] derivative(long h, double t); 50 | private static native double[] rotation(long h, double t); 51 | private static native double curvature(long h, double t); 52 | private static native double dcurvature(long h, double t); 53 | private static native double length(long h); 54 | 55 | private static native void free(long h); 56 | } 57 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/path/AbstractSpline2d.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.path; 2 | 3 | import grpl.pathfinder.Vec2; 4 | import grpl.pathfinder.util.NativeResource; 5 | 6 | public class AbstractSpline2d extends NativeResource implements Spline2d { 7 | 8 | AbstractSpline2d(long handle) { 9 | super(handle); 10 | } 11 | 12 | @Override 13 | public Vec2 position(double t) { 14 | return Vec2.cartesian(position(nativeHandle(), t)); 15 | } 16 | 17 | @Override 18 | public Vec2 derivative(double t) { 19 | return Vec2.cartesian(derivative(nativeHandle(), t)); 20 | } 21 | 22 | @Override 23 | public Vec2 rotation(double t) { 24 | return Vec2.cartesian(rotation(nativeHandle(), t)); 25 | } 26 | 27 | @Override 28 | public double curvature(double t) { 29 | return curvature(nativeHandle(), t); 30 | } 31 | 32 | @Override 33 | public void close() { 34 | free(nativeHandle()); 35 | zeroHandle(); 36 | } 37 | 38 | /* JNI */ 39 | private static native double[] position(long h, double t); 40 | private static native double[] derivative(long h, double t); 41 | private static native double[] rotation(long h, double t); 42 | private static native double curvature(long h, double t); 43 | 44 | private static native void free(long h); 45 | } 46 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/path/Arc2d.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.path; 2 | 3 | import grpl.pathfinder.Vec2; 4 | 5 | /** 6 | * A 2-Dimensional Circular Arc 7 | * 8 | * Implementation of a 2-Dimensional circular arc, parameterized to arc length 's'. 9 | */ 10 | public class Arc2d extends AbstractCurve2d { 11 | 12 | /** 13 | * Create a circular arc from a set of 3 points (start, any, and end). 14 | * 15 | * @param start The start point of the curve, in x,y metres. 16 | * @param mid Any point along the curve sitting between start and end, 17 | * in x,y metres. 18 | * @param end The end point of the curve, in x,y metres. 19 | */ 20 | Arc2d(Vec2 start, Vec2 mid, Vec2 end) { 21 | super(allocate(start.xy(), mid.xy(), end.xy())); 22 | } 23 | 24 | private Arc2d(long handle) { 25 | super(handle); 26 | } 27 | 28 | protected static Arc2d fromNativeHandle(long handle) { 29 | return new Arc2d(handle); 30 | } 31 | 32 | private static native long allocate(double[] start, double[] mid, double[] end); 33 | 34 | } 35 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/path/ArcParameterizer.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.path; 2 | 3 | import grpl.pathfinder.util.INativeResource; 4 | import grpl.pathfinder.util.NativeResource; 5 | 6 | import java.util.List; 7 | import java.util.Vector; 8 | 9 | /** 10 | * Arc Parameterizer, for reparameterizing splines to curves. 11 | *

12 | * The Arc Parameterizer takes in splines, parameterized to spline parameter 't', and produces 13 | * arcs parameterized by arc length 's'. The parameterizer uses augmented arcs, which are of a 14 | * non-constant curvature in order to approximate the spline while still remaining continuous in 15 | * curvature at the knot points. 16 | *

17 | */ 18 | public class ArcParameterizer extends NativeResource { 19 | 20 | public ArcParameterizer() { 21 | super(allocate()); 22 | } 23 | 24 | /** 25 | * Parameterize a container of splines into augmented 2D arcs. 26 | *

27 | * Reparameterizes a set of splines from being with respect to spline parameter 't', to a set of curves 28 | * that are with respect to arc length 's'. 29 | * 30 | * @param splines A list of splines to parameterize 31 | * @return The parameterized curves generated from the splines 32 | */ 33 | public List parameterize(List splines) { 34 | long buffer = pack(splines); 35 | List list = new Vector(); 36 | try { 37 | long[] arcs = parameterize(nativeHandle(), buffer); 38 | 39 | for (long arc : arcs) 40 | list.add(Arc2d.fromNativeHandle(arc)); 41 | } 42 | finally { 43 | releaseBuffer(buffer); 44 | buffer = 0L; 45 | } 46 | return list; 47 | } 48 | 49 | /** 50 | * Configure the parameters for the parameterizer, which are used as the criteria for deciding 51 | * when to produce a new arc. 52 | * 53 | * @param maxArcLength The maximum arc length. Any arcs larger than this will be recursively 54 | * split. Unit is metres. 55 | * @param maxDeltaCurvature The maximum change in curvature between the start and end of the 56 | * arc. Any arcs with a large change in curvature will be recursively 57 | * split. Unit is m^-1. 58 | */ 59 | public void configure(double maxArcLength, double maxDeltaCurvature) { 60 | configure(nativeHandle(), maxArcLength, maxDeltaCurvature); 61 | } 62 | 63 | @Override 64 | public void close() { 65 | free(nativeHandle()); 66 | zeroHandle(); 67 | } 68 | 69 | private static long pack(List splines) { 70 | long buffer = acquireBuffer(); 71 | for (Spline2d spline : splines) { 72 | // If the spline is already native, it's much cheaper to pass it directly than to construct 73 | // an adapter in the JNI that calls back to java for each sample. 74 | if (spline instanceof INativeResource) { 75 | enqueueNative(buffer, ((INativeResource) spline).nativeHandle()); 76 | } else { 77 | enqueueAdapter(buffer, spline); 78 | } 79 | } 80 | return buffer; 81 | } 82 | 83 | /* JNI */ 84 | private static native long allocate(); 85 | 86 | private static native void free(long h); 87 | 88 | private static native void configure(long handle, double maxArcLength, double maxDeltaCurvature); 89 | 90 | private static native long[] parameterize(long handle, long buffer); 91 | 92 | private static native long acquireBuffer(); 93 | 94 | private static native void enqueueNative(long bufferHandle, long splineHandle); 95 | 96 | private static native void enqueueAdapter(long bufferHandle, Spline2d spline); 97 | 98 | private static native void releaseBuffer(long bufferHandle); 99 | 100 | } 101 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/path/Curve2d.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.path; 2 | 3 | import grpl.pathfinder.Vec2; 4 | 5 | /** 6 | * A position curve parameterized to arc length 's'. 7 | *

8 | * A curve parameterized to its arc length, allowing extremely quick 9 | * calculation of the length of the curve, primarily useful when using 10 | * curves to generate a trajectory, as the parameter is a real-world unit 11 | * relating directly to the state (distance travelled). 12 | */ 13 | public interface Curve2d { 14 | 15 | /** 16 | * Calculate the position of a point on the curve, at any arc length 's'. 17 | * 18 | * @param s The distance along the arc. 19 | * @return The position of the curve at arc length 's', in m. 20 | */ 21 | Vec2 position(double s); 22 | 23 | /** 24 | * Calculate the derivative of a point on the curve, at any arc length 's'. 25 | * 26 | * @param s The distance along the arc. 27 | * @return The derivative of the curve at arc length 's', unitless. 28 | */ 29 | Vec2 derivative(double s); 30 | 31 | /** 32 | * Calculate the rotation of a point on the curve, at any arc length 's'. 33 | * This is the unit vector of {@link #derivative(double)}. 34 | * 35 | * @param s The distance along the arc. 36 | * @return The rotation of the curve at arc length 's', unitless. 37 | */ 38 | Vec2 rotation(double s); 39 | 40 | /** 41 | * Calculate the curvature of the curve at any arc length 's'. 42 | * 43 | * @param s The distance along the arc 44 | * @return The curvature of the arc at arc length 's', in m^-1. 45 | */ 46 | double curvature(double s); 47 | 48 | /** 49 | * Calculate the derivative of curvature of the curve at any arc length 's'. 50 | * 51 | * @param s The distance along the arc 52 | * @return The derviative of curvature of the arc at arc length 's' (dk/ds), 53 | * in m^-2. 54 | */ 55 | double dcurvature(double s); 56 | 57 | /** 58 | * Calculate the total length of the arc. 59 | *

60 | * The arc length is what defines the range of parameter 's' used throughout this class. 61 | * 62 | * @return The total length of the curve, in metres. 63 | */ 64 | double length(); 65 | 66 | } 67 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/path/HermiteCubic.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.path; 2 | 3 | import grpl.pathfinder.Vec2; 4 | 5 | /** 6 | * Implementation of a cubic (order 3) hermite spline. 7 | * 8 | * A Cubic Hermite Spline is a spline of order 3, as defined here 9 | * https://en.wikipedia.org/wiki/Cubic_Hermite_spline. 10 | * 11 | * The cubic spline is defined in regards to its waypoints, which are defined in terms of position and 12 | * tangent. 13 | */ 14 | public class HermiteCubic extends AbstractSpline2d { 15 | 16 | /** 17 | * Construct a cubic hermite spline, given a start and end point. 18 | * 19 | * @param start The waypoint of the start of the spline 20 | * @param end The waypoint of the end of the spline. 21 | */ 22 | HermiteCubic(Waypoint start, Waypoint end) { 23 | super(allocate(start.toNative(), end.toNative())); 24 | } 25 | 26 | /** 27 | * Waypoint for a cubic hermite spline. 28 | */ 29 | public static class Waypoint { 30 | 31 | /** 32 | * The 2D position of the waypoint, in metres. Ordered x, y. 33 | */ 34 | public Vec2 position; 35 | 36 | /** 37 | * The 2D tangent to the waypoint, in metres. Ordered x, y. 38 | */ 39 | public Vec2 tangent; 40 | 41 | public Waypoint(Vec2 position, Vec2 tangent) { 42 | this.position = position; 43 | this.tangent = tangent; 44 | } 45 | 46 | protected double[] toNative() { 47 | return new double[] { 48 | position.x(), position.y(), 49 | tangent.x(), tangent.y() 50 | }; 51 | } 52 | } 53 | 54 | private static native long allocate(double[] start, double[] end); 55 | } 56 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/path/HermiteFactory.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.path; 2 | 3 | import java.util.List; 4 | import java.util.Vector; 5 | 6 | public class HermiteFactory { 7 | 8 | public static List generateCubic(List wps) { 9 | if (wps.size() == 0) 10 | return null; 11 | 12 | int num_curves = wps.size() - 1; 13 | List list = new Vector(num_curves); 14 | 15 | for (int i = 0; i < num_curves; i++) { 16 | HermiteCubic.Waypoint start = wps.get(i), end = wps.get(i+1); 17 | list.add(new HermiteCubic(start, end)); 18 | } 19 | return list; 20 | } 21 | 22 | public static List generateQuintic(List wps) { 23 | if (wps.size() == 0) 24 | return null; 25 | 26 | int num_curves = wps.size() - 1; 27 | List list = new Vector(num_curves); 28 | 29 | for (int i = 0; i < num_curves; i++) { 30 | HermiteQuintic.Waypoint start = wps.get(i), end = wps.get(i+1); 31 | list.add(new HermiteQuintic(start, end)); 32 | } 33 | return list; 34 | } 35 | 36 | } 37 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/path/HermiteQuintic.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.path; 2 | 3 | import grpl.pathfinder.Vec2; 4 | 5 | /** 6 | * Implementation of a quintic (order 5) hermite spline. 7 | * 8 | * The quintic spline is defined in regards to its waypoints, which are defined in terms of position, 9 | * tangent and the derivative of the tangent. 10 | */ 11 | public class HermiteQuintic extends AbstractSpline2d { 12 | 13 | /** 14 | * Construct a quintic hermite spline, given a start and end point. 15 | * 16 | * @param start The waypoint of the start of the spline 17 | * @param end The waypoint of the end of the spline. 18 | */ 19 | HermiteQuintic(Waypoint start, Waypoint end) { 20 | super(allocate(start.toNative(), end.toNative())); 21 | } 22 | 23 | /** 24 | * Waypoint for a quintic hermite spline. 25 | */ 26 | public static class Waypoint { 27 | 28 | /** 29 | * The 2D position of the waypoint, in metres. Ordered x, y. 30 | */ 31 | public Vec2 position; 32 | 33 | /** 34 | * The 2D tangent to the waypoint, in metres. Ordered x, y. 35 | */ 36 | public Vec2 tangent; 37 | 38 | /** 39 | * 2D derivative of the tangent to the waypoint, in metres. Ordered x, y. 40 | */ 41 | public Vec2 dtangent; 42 | 43 | public Waypoint(Vec2 position, Vec2 tangent, Vec2 dtangent) { 44 | this.position = position; 45 | this.tangent = tangent; 46 | this.dtangent = dtangent; 47 | } 48 | 49 | protected double[] toNative() { 50 | return new double[] { 51 | position.x(), position.y(), 52 | tangent.x(), tangent.y(), 53 | dtangent.x(), dtangent.y() 54 | }; 55 | } 56 | } 57 | 58 | private static native long allocate(double[] start, double[] end); 59 | 60 | } 61 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/path/Spline2d.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.path; 2 | 3 | import grpl.pathfinder.Vec2; 4 | 5 | /** 6 | * A position curve parameterized to spline parameter 't'. 7 | *

8 | * A spline is simply a position curve parameterized to spline parameter 't' 9 | * (note that this is distinct to time), which lays in the range of 0 to 1, 10 | * representing the start and end of the spline respectively. 11 | */ 12 | public interface Spline2d { 13 | 14 | /** 15 | * Calculate the position of a point on the spline, at any spline parameter 16 | * value 't' 17 | * 18 | * @param t The spline parameter, where 0 is the start and 1 is the end of the 19 | * spline. 20 | * @return The position at spline parameter 't', in m. 21 | */ 22 | Vec2 position(double t); 23 | 24 | /** 25 | * Calculate the derivative of a point on the spline, at any spline parameter 26 | * value 't' (the derivative of {@link #position(double)}) 27 | * 28 | * @param t The spline parameter, where 0 is the start and 1 is the end of the 29 | * spline. 30 | * @return The derivative at spline parameter 't', in m/t 31 | */ 32 | Vec2 derivative(double t); 33 | 34 | /** 35 | * Calculate the rotation of a point on the spline, at any spline parameter 36 | * value 't'. This is the unit vector of {@link #derivative(double)} 37 | * 38 | * @param t The spline parameter, where 0 is the start and 1 is the end of the 39 | * spline. 40 | * @return The rotation (unit derivative) at spline parameter 't', unitless. 41 | */ 42 | Vec2 rotation(double t); 43 | 44 | /** 45 | * Calculate the curvature of the spline at any spline parameter value 't'. 46 | * 47 | * @param t The spline parameter, where 0 is the start and 1 is the end of the 48 | * spline. 49 | * @return The curvature at spline parameter 't', in m^-1. 50 | */ 51 | double curvature(double t); 52 | 53 | } 54 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/path/package-info.java: -------------------------------------------------------------------------------- 1 | /** 2 | * Path, spline and curve members. 3 | * 4 | * The grpl.pathfinder.path package contains all classes to deal with pathing, be 5 | * it splines, curves or the utilities that accompany them. The classes in this 6 | * namespace deal only with position paths, and do not include parameterizations 7 | * to time (i.e. trajectories). 8 | */ 9 | package grpl.pathfinder.path; -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/profile/AbstractNativeProfile.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.profile; 2 | 3 | import grpl.pathfinder.util.NativeResource; 4 | 5 | public class AbstractNativeProfile extends NativeResource implements Profile { 6 | private int _limitedTerm; 7 | 8 | AbstractNativeProfile(long handle, int limitedTerm) { 9 | super(handle); 10 | this._limitedTerm = limitedTerm; 11 | } 12 | 13 | @Override 14 | public State createState() { 15 | return new State(this.getLimitedTerm()); 16 | } 17 | 18 | @Override 19 | public void setGoal(double goal) { 20 | setGoal(nativeHandle(), goal); 21 | } 22 | 23 | @Override 24 | public double getGoal() { 25 | return getGoal(nativeHandle()); 26 | } 27 | 28 | @Override 29 | public void setTimeslice(double timeslice) { 30 | setTimeslice(nativeHandle(), timeslice); 31 | } 32 | 33 | @Override 34 | public double getTimeslice() { 35 | return getTimeslice(nativeHandle()); 36 | } 37 | 38 | @Override 39 | public void applyLimit(int derivative, double min, double max) { 40 | applyLimit(nativeHandle(), derivative, min, max); 41 | } 42 | 43 | @Override 44 | public State calculate(State last, double time) { 45 | State seg = createState(); 46 | seg.time = time; 47 | seg.kinematics = calculateNative(nativeHandle(), last.kinematics, last.time, time); 48 | return seg; 49 | } 50 | 51 | @Override 52 | public int getLimitedTerm() { 53 | return _limitedTerm; 54 | } 55 | 56 | @Override 57 | public void close() { 58 | free(nativeHandle()); 59 | zeroHandle(); 60 | } 61 | 62 | /* JNI */ 63 | private static native double[] calculateNative(long h, double[] last, double lastTime, double time); 64 | 65 | private static native void setGoal(long h, double g); 66 | private static native double getGoal(long h); 67 | private static native void setTimeslice(long h, double ts); 68 | private static native double getTimeslice(long h); 69 | private static native void applyLimit(long h, int d, double min, double max); 70 | 71 | private static native void free(long h); 72 | 73 | } 74 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/profile/AbstractProfile.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.profile; 2 | 3 | public abstract class AbstractProfile implements Profile { 4 | } 5 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/profile/Profile.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.profile; 2 | 3 | /** 4 | * Interface class for all motion profile types. 5 | *

6 | * A motion profile describes some function that forms the shape of the position-time curve 7 | * and its derivatives, allowing velocity, acceleration and jerk to be shaped in ways that may 8 | * be desirable, such as optimizing for speed, safety, or smooth operation. 9 | *

10 | * Profiles are predictive in Pathfinder, meaning each calculation does not require a full history 11 | * of the profile. This allows it to adjust to changing system conditions and limits during a 12 | * profile, increasing flexibility and efficiency. 13 | *

14 | * Since the system is predictive, it may result in a small oscillation or sudden deceleration 15 | * if a sufficient timestep is not used. For this reason, a timeslice mechanism is included in the 16 | * profile. 17 | */ 18 | public interface Profile { 19 | 20 | public static final int POSITION = 0; 21 | public static final int VELOCITY = 1; 22 | public static final int ACCELERATION = 2; 23 | 24 | /** 25 | * A single state (sample point) of a motion profile. This contains the kinematics of 26 | * the profile sampled at a given time. 27 | */ 28 | public static class State { 29 | 30 | /** 31 | * The time point of this state, in seconds 32 | */ 33 | public double time = 0; 34 | 35 | /** 36 | * The kinematic state of the system at the time of the state. 37 | * 38 | * {@link #POSITION} 39 | * {@link #VELOCITY} 40 | * {@link #ACCELERATION} 41 | */ 42 | public double[] kinematics; 43 | 44 | protected State(int limited_term) { 45 | kinematics = new double[limited_term + 1]; 46 | } 47 | } 48 | 49 | /** 50 | * Create a basic 0 state for {@link State} 51 | * @return A basic, 0 state. 52 | */ 53 | State createState(); 54 | 55 | /** 56 | * Set the goal (setpoint) of the profile. 57 | * 58 | * @param goal The goal (setpoint) of the profile, in metres. 59 | */ 60 | void setGoal(double goal); 61 | 62 | /** 63 | * Get the goal (setpoint) of the profile. 64 | * 65 | * @return The goal (setpoint) of the profile, in metres. 66 | */ 67 | double getGoal(); 68 | 69 | /** 70 | * Set the timeslice period. 71 | *

72 | * The timeslice is used in {@link #calculate(State, double)}, where a single 73 | * call will be translated into N smaller calls, where N is ceil(T_calc / T_slice), where T_calc is the 74 | * period at which {@link #calculate(State, double)} is called, and T_slice is the timeslice period. 75 | *

76 | * Timeslice is used to counteract slow calls to {@link #calculate(State, double)} in order to still 77 | * produce smooth curves. 78 | * 79 | * @param timeslice The timeslice period T_slice, in seconds. 80 | */ 81 | void setTimeslice(double timeslice); 82 | 83 | /** 84 | * Get the timeslice period. 85 | * 86 | * @return The timeslice period, T_slice, in seconds. 87 | */ 88 | double getTimeslice(); 89 | 90 | /** 91 | * Apply a constrained limit to the profile. This will limit the maximum and minimum value of 92 | * this term during the profile (e.g. maximum velocity / acceleration). 93 | *

94 | * These limits may be changed between calls to {@link #calculate(State, double)}, but if the maximum 95 | * or minimum is lower in magnitude, it may cause sudden decelerations. 96 | * 97 | * @param derivative The term to apply the limit to. 98 | * @param min The minimum value of the term, in the units of the term 99 | * @param max The maximum value of the term, in the units of the term 100 | */ 101 | void applyLimit(int derivative, double min, double max); 102 | 103 | /** 104 | * Calculate a single state of the motion profile, in a predictive manner. 105 | *

106 | * This method function uses the last (current) state of the system in order to generate the following 107 | * state at the time given. It does this in a predictive manner, meaning it predicts when it must 108 | * begin slowing down in order to not overshoot the setpoint. This means the profile calculation does 109 | * not require a full history of the profile, allowing it to adjust to changing system conditions and 110 | * limits. 111 | */ 112 | State calculate(State last, double time); 113 | 114 | /** 115 | * Get the index of the limited term (the highest order, non-infinite term). 116 | */ 117 | int getLimitedTerm(); 118 | 119 | } 120 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/profile/TrapezoidalProfile.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.profile; 2 | 3 | /** 4 | * Implementation of a trapezoidal (limited acceleration) motion profile. 5 | *

6 | * A trapezoidal motion profile is a motion profile limited by acceleration (therefore, infinite jerk). 7 | * The profile can be described by three distinct sections: ramp-up, hold and ramp-down. 8 | *

9 | * During ramp-up, the system is accelerating towards its max velocity. 10 | *

11 | * During hold, the system is not accelerating, and holds its max velocity. Depending on the setpoint, 12 | * the system may not have time to reach hold before it must ramp-down, resulting in a trianglular 13 | * velocity profile. 14 | *

15 | * During ramp-down, the system is decelerating towards 0. 16 | * 17 | * See {@link Profile} 18 | */ 19 | public class TrapezoidalProfile extends AbstractNativeProfile { 20 | 21 | public TrapezoidalProfile() { 22 | super(allocate(), ACCELERATION); 23 | } 24 | 25 | private static native long allocate(); 26 | 27 | } 28 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/profile/package-info.java: -------------------------------------------------------------------------------- 1 | /** 2 | * Motion profiles 3 | * 4 | * The grpl.pathfinder.profile package contains all motion profile implementations. 5 | * Motion profiles are used to describe the shape of the position-time curve to the nth 6 | * degree. 7 | */ 8 | package grpl.pathfinder.profile; -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/transmission/AbstractDcTransmission.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.transmission; 2 | 3 | import grpl.pathfinder.util.NativeResource; 4 | 5 | public class AbstractDcTransmission extends NativeResource implements DcTransmission { 6 | 7 | public AbstractDcTransmission(long handle) { 8 | super(handle); 9 | } 10 | 11 | @Override 12 | public double getFreeSpeed(double voltage) { 13 | return freeSpeed(nativeHandle(), voltage); 14 | } 15 | 16 | @Override 17 | public double getCurrent(double voltage, double speed) { 18 | return current(nativeHandle(), voltage, speed); 19 | } 20 | 21 | @Override 22 | public double getTorque(double current) { 23 | return torque(nativeHandle(), current); 24 | } 25 | 26 | @Override 27 | public double getFreeVoltage(double speed) { 28 | return freeVoltage(nativeHandle(), speed); 29 | } 30 | 31 | @Override 32 | public double getCurrentVoltage(double current) { 33 | return currentVoltage(nativeHandle(), current); 34 | } 35 | 36 | @Override 37 | public double getTorqueCurrent(double torque) { 38 | return torqueCurrent(nativeHandle(), torque); 39 | } 40 | 41 | @Override 42 | public double getNominalVoltage() { 43 | return nominalVoltage(nativeHandle()); 44 | } 45 | 46 | @Override 47 | public void close() { 48 | free(nativeHandle()); 49 | zeroHandle(); 50 | } 51 | 52 | /* JNI */ 53 | private static native double freeSpeed(long h, double voltage); 54 | private static native double current(long h, double voltage, double speed); 55 | private static native double torque(long h, double current); 56 | 57 | private static native double freeVoltage(long h, double speed); 58 | private static native double currentVoltage(long h, double current); 59 | private static native double torqueCurrent(long h, double torque); 60 | 61 | private static native double nominalVoltage(long h); 62 | 63 | private static native void free(long h); 64 | } 65 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/transmission/DcMotor.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.transmission; 2 | 3 | /** 4 | * Mathematical Model of a DC Brushed Motor 5 | *

6 | * Basic DC Motor Model, dervied from the ideal resistive motor model with Back EMF 7 | * (+ ---[ R ]---( V_w )--- -), where R = V / I_stall (as V_w = 0 at stall), and 8 | * V_w = kv*w. 9 | */ 10 | public class DcMotor extends AbstractDcTransmission { 11 | 12 | /** 13 | * Construct a DC Brushed Motor Model. 14 | * 15 | * @param voltageNom The nominal operating voltage of the motor, in Volts 16 | * @param freeSpeed The maximum free (no load) speed of the motor at v_nom, in rad/s 17 | * @param freeCurrent The current drawn when the motor is running at free_speed 18 | * with no load 19 | * @param stallCurrent The current drawn when v_nom is applied with a locked rotor (stalled) 20 | * @param stallTorque The torque applied by the motor at v_nom with a locked rotor (stalled) 21 | */ 22 | public DcMotor(double voltageNom, double freeSpeed, double freeCurrent, double stallCurrent, double stallTorque) { 23 | super(allocate(voltageNom, freeSpeed, freeCurrent, stallCurrent, stallTorque)); 24 | } 25 | 26 | private static native long allocate(double voltageNom, double freeSpeed, double freeCurrent, double stallCurrent, double stallTorque); 27 | 28 | } 29 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/transmission/DcTransmission.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.transmission; 2 | 3 | /** 4 | * Interface class of a DC electric transmission, usually {@link DcMotor} 5 | */ 6 | public interface DcTransmission { 7 | 8 | /** 9 | * Calculate the free (no load) speed of the transmission at an applied voltage. 10 | * 11 | * @param voltage The voltage applied to the transmission, in Volts 12 | * @return The free speed of the transmission, in rad/s 13 | */ 14 | double getFreeSpeed(double voltage); 15 | 16 | /** 17 | * Calculate the current draw of the transmission at an applied voltage and speed. 18 | * 19 | * @param voltage The voltage applied to the transmission, in Volts 20 | * @param speed The current speed of the transmission, in rad/s 21 | * @return The current drawn by the transmission, in Amps 22 | */ 23 | double getCurrent(double voltage, double speed); 24 | 25 | /** 26 | * Calculate the torque applied by the transmission at a given current draw. 27 | * 28 | * @param current The current drawn by the transmission, calculated in 29 | * {@link #getCurrent(double, double)}, in Amps 30 | * @return The torque applied by the transmission, in Nm 31 | */ 32 | double getTorque(double current); 33 | 34 | /** 35 | * Calculate the component voltage applied to the transmission in order to obtain 36 | * a free speed. 37 | *

38 | * To obtain the full applied voltage, sum {@link #getFreeVoltage(double)} and 39 | * {@link #getCurrentVoltage(double)}. 40 | * 41 | * @param speed The speed of the transmission, in rad/s 42 | * @return The free voltage component of the transmission, in Volts 43 | */ 44 | double getFreeVoltage(double speed); 45 | 46 | /** 47 | * Calculate the component voltage applied to the transmission in order to draw 48 | * a current. 49 | *

50 | * Current is usually provided by {@link #getTorqueCurrent(double)} 51 | *

52 | * To obtain the full applied voltage, sum {@link #getFreeVoltage(double)} and 53 | * {@link #getCurrentVoltage(double)}. 54 | * 55 | * @param current The current drawn by the transmission, in Amps 56 | * @return The current voltage component of the transmission, in Volts 57 | */ 58 | double getCurrentVoltage(double current); 59 | 60 | /** 61 | * Calculate the current draw of the transmission given a torque. 62 | * 63 | * @param torque The torque applied by the transmission, in Nm. 64 | * @return The current draw required to apply the torque, in Amps. 65 | */ 66 | double getTorqueCurrent(double torque); 67 | 68 | /** 69 | * Get the nominal, operating voltage of the transmission. 70 | * 71 | * @return The nominal, operating voltage of the transmission, in Volts 72 | */ 73 | double getNominalVoltage(); 74 | 75 | } 76 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/transmission/JavaDcTransmissionAdapter.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.transmission; 2 | 3 | import grpl.pathfinder.util.NativeResource; 4 | 5 | public class JavaDcTransmissionAdapter extends AbstractDcTransmission { 6 | 7 | public JavaDcTransmissionAdapter(DcTransmission transmission) { 8 | super(allocate(transmission)); 9 | } 10 | 11 | private static native long allocate(DcTransmission javaTransmission); 12 | 13 | } 14 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/transmission/package-info.java: -------------------------------------------------------------------------------- 1 | /** 2 | * Motor transmissions. 3 | * 4 | * The grpl.pathfinder.transmission package contains implementations of transmissions, which convert 5 | * control signals to motion (e.g. motors). 6 | */ 7 | package grpl.pathfinder.transmission; -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/util/INativeResource.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.util; 2 | 3 | public interface INativeResource extends AutoCloseable { 4 | long nativeHandle(); 5 | boolean closed(); 6 | } 7 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/util/NativeResource.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.util; 2 | 3 | public abstract class NativeResource extends PathfinderJNI implements INativeResource { 4 | private long _handle = 0; 5 | 6 | public NativeResource(long handle) { 7 | this._handle = handle; 8 | } 9 | 10 | @Override 11 | public long nativeHandle() { 12 | return _handle; 13 | } 14 | 15 | protected void zeroHandle() { 16 | _handle = 0; 17 | } 18 | 19 | @Override 20 | public void close() { } 21 | 22 | @Override 23 | public boolean closed() { 24 | return _handle == 0; 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/main/java/grpl/pathfinder/util/PathfinderJNI.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.util; 2 | 3 | public class PathfinderJNI { 4 | 5 | static boolean _loaded = false; 6 | 7 | static { 8 | loadPathfinderJNI(); 9 | } 10 | 11 | public static boolean loadPathfinderJNI() { 12 | if (!_loaded) { 13 | System.loadLibrary("pathfinderjni"); 14 | _loaded = true; 15 | return true; 16 | } 17 | return false; 18 | } 19 | 20 | } 21 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/coupled/coupled_causaltrajgen.cpp: -------------------------------------------------------------------------------- 1 | #include "grpl_pathfinder_coupled_CoupledCausalTrajGen.h" 2 | #include "jnieigen.h" 3 | #include "jnihandle.h" 4 | #include "jniutil.h" 5 | #include "coupled/jnicoupled.h" 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | using namespace grpl::pf; 15 | 16 | using curve_container_t = std::vector>>; 17 | 18 | static jdoubleArray state_to_java(JNIEnv *env, coupled::state &st) { 19 | double tmp_state_arr[10]; 20 | jdoubleArray nArr = env->NewDoubleArray(10); 21 | tmp_state_arr[0] = st.time; 22 | tmp_state_arr[1] = st.curvature; 23 | tmp_state_arr[2] = st.dcurvature; 24 | for (int i = 0; i < 3; i++) { 25 | tmp_state_arr[3 + i] = st.config[i]; 26 | tmp_state_arr[6 + i] = st.kinematics[i]; 27 | } 28 | tmp_state_arr[9] = st.finished ? 1.0 : 0.0; 29 | env->SetDoubleArrayRegion(nArr, 0, 10, tmp_state_arr); 30 | return nArr; 31 | } 32 | 33 | JNIEXPORT jlong JNICALL Java_grpl_pathfinder_coupled_CoupledCausalTrajGen_allocate(JNIEnv *env, jclass claz) { 34 | return jni_as_handle(new coupled::causal_trajectory_generator()); 35 | } 36 | 37 | JNIEXPORT void JNICALL Java_grpl_pathfinder_coupled_CoupledCausalTrajGen_free(JNIEnv *env, jclass claz, 38 | jlong handle) { 39 | delete jni_handle(env, handle); 40 | } 41 | 42 | JNIEXPORT jdoubleArray JNICALL Java_grpl_pathfinder_coupled_CoupledCausalTrajGen_generateNative( 43 | JNIEnv *env, jclass claz, jlong handle, jlong chassisHandle, jlong buff, jlong profile_handle, 44 | jdoubleArray stateArr, jdouble time) { 45 | curve_container_t *curves = jni_handle(env, buff); 46 | 47 | profile::profile *prof = jni_handle(env, profile_handle); 48 | coupled::chassis *ch = jni_handle(env, chassisHandle); 49 | 50 | coupled::state s = jni_coupled_java_to_state(env, stateArr); 51 | coupled::state n = jni_handle(env, handle) 52 | ->generate(*ch, curves->begin(), curves->end(), *prof, s, time); 53 | 54 | return state_to_java(env, n); 55 | } 56 | 57 | JNIEXPORT jlong JNICALL Java_grpl_pathfinder_coupled_CoupledCausalTrajGen_acquireBuffer(JNIEnv *env, 58 | jclass claz) { 59 | return jni_as_handle(new curve_container_t()); 60 | } 61 | 62 | JNIEXPORT void JNICALL Java_grpl_pathfinder_coupled_CoupledCausalTrajGen_enqueueNative(JNIEnv *env, 63 | jclass claz, 64 | jlong buff, 65 | jlong curveHandle) { 66 | curve_container_t *curves = jni_handle(env, buff); 67 | 68 | path::curve<2> *cur = jni_handle>(env, curveHandle); 69 | curves->push_back(*cur); 70 | } 71 | 72 | JNIEXPORT void JNICALL Java_grpl_pathfinder_coupled_CoupledCausalTrajGen_enqueueAdapter(JNIEnv *env, 73 | jclass claz, 74 | jlong buff, 75 | jobject curve) { 76 | // TODO 77 | } 78 | 79 | JNIEXPORT void JNICALL Java_grpl_pathfinder_coupled_CoupledCausalTrajGen_releaseBuffer(JNIEnv *env, 80 | jclass claz, 81 | jlong buff) { 82 | delete jni_handle(env, buff); 83 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/coupled/coupled_chassis.cpp: -------------------------------------------------------------------------------- 1 | #include "grpl_pathfinder_coupled_CoupledChassis.h" 2 | #include "jnieigen.h" 3 | #include "jnihandle.h" 4 | #include "jniutil.h" 5 | #include "coupled/jnicoupled.h" 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | using namespace grpl::pf::coupled; 13 | using namespace grpl::pf::transmission; 14 | 15 | // We use std::allocator and std::allocator_traits since we need to be able to reserve 16 | // the memory (allocate) without constructing the chassis until more information is available 17 | // (i.e. the native handles to left and right transmissions) 18 | using allocator_t = std::allocator; 19 | using traits_t = std::allocator_traits; 20 | 21 | allocator_t _ch_alloc; 22 | 23 | static void wheelstate_to_java(JNIEnv *env, wheel_state &ws, jdoubleArray arr) { 24 | double tmp_state_arr[10]; 25 | tmp_state_arr[0] = ws.time; 26 | tmp_state_arr[1] = ws.position[0]; 27 | tmp_state_arr[2] = ws.position[1]; 28 | for (int i = 0; i < 3; i++) { 29 | tmp_state_arr[3 + i] = ws.kinematics[i]; 30 | } 31 | tmp_state_arr[6] = ws.voltage; 32 | tmp_state_arr[7] = ws.current; 33 | tmp_state_arr[8] = ws.finished ? 1.0 : 0.0; 34 | env->SetDoubleArrayRegion(arr, 0, 9, tmp_state_arr); 35 | } 36 | 37 | JNIEXPORT jlong JNICALL Java_grpl_pathfinder_coupled_CoupledChassis_allocateStub(JNIEnv *env, jclass claz) { 38 | return jni_as_handle(traits_t::allocate(_ch_alloc, 1)); 39 | } 40 | 41 | JNIEXPORT void JNICALL Java_grpl_pathfinder_coupled_CoupledChassis_construct(JNIEnv *env, jclass claz, 42 | jlong handle, jlong leftHandle, 43 | jlong rightHandle, 44 | jdouble wheelR, jdouble trackR, 45 | jdouble mass) { 46 | dc_transmission *trans_left = jni_handle(env, leftHandle); 47 | dc_transmission *trans_right = jni_handle(env, rightHandle); 48 | traits_t::construct(_ch_alloc, jni_handle(env, handle), *trans_left, *trans_right, wheelR, trackR, 49 | mass); 50 | } 51 | 52 | JNIEXPORT void JNICALL Java_grpl_pathfinder_coupled_CoupledChassis_free(JNIEnv *env, jclass claz, 53 | jlong handle) { 54 | chassis *ch = jni_handle(env, handle); 55 | traits_t::destroy(_ch_alloc, ch); 56 | traits_t::deallocate(_ch_alloc, ch, 1); 57 | } 58 | 59 | JNIEXPORT jdouble JNICALL Java_grpl_pathfinder_coupled_CoupledChassis_linvelLimit(JNIEnv *env, jclass claz, 60 | jlong handle, 61 | jdoubleArray config, 62 | jdouble curv) { 63 | configuration_state conf = eigen_adapt_jdoubleArray(env, config); 64 | return jni_handle(env, handle)->linear_vel_limit(conf, curv); 65 | } 66 | 67 | JNIEXPORT jdoubleArray JNICALL Java_grpl_pathfinder_coupled_CoupledChassis_accLimit( 68 | JNIEnv *env, jclass claz, jlong handle, jdoubleArray config, jdouble curv, jdouble vel) { 69 | configuration_state conf = eigen_adapt_jdoubleArray(env, config); 70 | std::pair ret = jni_handle(env, handle)->acceleration_limits(conf, curv, vel); 71 | 72 | double data[2]{ret.first, ret.second}; 73 | 74 | jdoubleArray arr = env->NewDoubleArray(2); 75 | env->SetDoubleArrayRegion(arr, 0, 2, data); 76 | return arr; 77 | } 78 | 79 | JNIEXPORT void JNICALL Java_grpl_pathfinder_coupled_CoupledChassis_splitNative(JNIEnv *env, jclass claz, 80 | jlong handle, 81 | jdoubleArray centreArr, 82 | jdoubleArray leftArr, 83 | jdoubleArray rightArr) { 84 | std::pair split = 85 | jni_handle(env, handle)->split(jni_coupled_java_to_state(env, centreArr)); 86 | 87 | wheelstate_to_java(env, split.first, leftArr); 88 | wheelstate_to_java(env, split.second, rightArr); 89 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/coupled/jnicoupled.cpp: -------------------------------------------------------------------------------- 1 | #include "coupled/jnicoupled.h" 2 | 3 | grpl::pf::coupled::state jni_coupled_java_to_state(JNIEnv *env, jdoubleArray arr) { 4 | double * els = env->GetDoubleArrayElements(arr, nullptr); 5 | grpl::pf::coupled::state s{els[0], 6 | els[1], 7 | els[2], 8 | eigen_adapt_array(env, els + 3), 9 | eigen_adapt_array(env, els + 6), 10 | els[9] > 0.5}; 11 | env->ReleaseDoubleArrayElements(arr, els, 0); 12 | return s; 13 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/jnihandle.cpp: -------------------------------------------------------------------------------- 1 | #include "jnihandle.h" 2 | #include "jniutil.h" 3 | 4 | jfieldID jni_get_handle_field(JNIEnv *env, jobject obj) { 5 | return jni_get_field_id(env, obj, "_handle", "J"); 6 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/jniutil.cpp: -------------------------------------------------------------------------------- 1 | #include "jniutil.h" 2 | 3 | jfieldID jni_get_field_id(JNIEnv *env, jobject obj, const char *name, const char *sig) { 4 | return env->GetFieldID(env->GetObjectClass(obj), name, sig); 5 | } 6 | 7 | jmethodID jni_get_method_id(JNIEnv *env, jobject obj, const char *name, const char *sig) { 8 | return env->GetMethodID(env->GetObjectClass(obj), name, sig); 9 | } 10 | 11 | jmethodID jni_get_method_id(JNIEnv *env, const char *className, const char *name, const char *sig) { 12 | return env->GetMethodID(env->FindClass(className), name, sig); 13 | } 14 | 15 | jdouble jni_get_double_field(JNIEnv *env, jobject obj, const char *name) { 16 | return env->GetDoubleField(obj, jni_get_field_id(env, obj, name, "D")); 17 | } 18 | 19 | void jni_set_double_field(JNIEnv *env, jobject obj, const char *name, jdouble dbl) { 20 | env->SetDoubleField(obj, jni_get_field_id(env, obj, name, "D"), dbl); 21 | } 22 | 23 | jobject jni_construct(JNIEnv *env, const char *cls_name, const char *sig, ...) { 24 | jclass clazz = env->FindClass(cls_name); 25 | jmethodID constructor = env->GetMethodID(clazz, "", sig); 26 | 27 | va_list args; 28 | va_start(args, sig); 29 | jobject result = env->NewObjectV(clazz, constructor, args); 30 | va_end(args); 31 | // jobject result = env->NewObject(clazz, constructor); 32 | return result; 33 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/path/abstract_curve.cpp: -------------------------------------------------------------------------------- 1 | #include "grpl_pathfinder_path_AbstractCurve2d.h" 2 | #include "jnieigen.h" 3 | #include "jnihandle.h" 4 | 5 | #include 6 | 7 | using namespace grpl::pf::path; 8 | 9 | using curve_t = curve<2>; 10 | 11 | JNIEXPORT jdoubleArray JNICALL Java_grpl_pathfinder_path_AbstractCurve2d_position(JNIEnv *env, jclass claz, 12 | jlong handle, jdouble s) { 13 | curve_t::vector_t vec = jni_handle(env, handle)->position(s); 14 | return eigen_create_jdoubleArray(env, vec); 15 | } 16 | 17 | JNIEXPORT jdoubleArray JNICALL Java_grpl_pathfinder_path_AbstractCurve2d_derivative(JNIEnv *env, jclass claz, 18 | jlong handle, jdouble s) { 19 | curve_t::vector_t vec = jni_handle(env, handle)->derivative(s); 20 | return eigen_create_jdoubleArray(env, vec); 21 | } 22 | 23 | JNIEXPORT jdoubleArray JNICALL Java_grpl_pathfinder_path_AbstractCurve2d_rotation(JNIEnv *env, jclass claz, 24 | jlong handle, jdouble s) { 25 | curve_t::vector_t vec = jni_handle(env, handle)->rotation(s); 26 | return eigen_create_jdoubleArray(env, vec); 27 | } 28 | 29 | JNIEXPORT jdouble JNICALL Java_grpl_pathfinder_path_AbstractCurve2d_curvature(JNIEnv *env, jclass claz, 30 | jlong handle, jdouble s) { 31 | return jni_handle(env, handle)->curvature(s); 32 | } 33 | 34 | JNIEXPORT jdouble JNICALL Java_grpl_pathfinder_path_AbstractCurve2d_dcurvature(JNIEnv *env, jclass claz, 35 | jlong handle, 36 | jdouble s) { 37 | return jni_handle(env, handle)->dcurvature(s); 38 | } 39 | 40 | JNIEXPORT jdouble JNICALL Java_grpl_pathfinder_path_AbstractCurve2d_length(JNIEnv *env, jclass claz, 41 | jlong handle) { 42 | return jni_handle(env, handle)->length(); 43 | } 44 | 45 | JNIEXPORT void JNICALL Java_grpl_pathfinder_path_AbstractCurve2d_free(JNIEnv *env, jclass claz, 46 | jlong handle) { 47 | delete jni_handle(env, handle); 48 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/path/abstract_spline.cpp: -------------------------------------------------------------------------------- 1 | #include "grpl_pathfinder_path_AbstractSpline2d.h" 2 | #include "jnieigen.h" 3 | #include "jnihandle.h" 4 | 5 | #include 6 | 7 | using namespace grpl::pf::path; 8 | 9 | using spline_t = spline<2>; 10 | 11 | JNIEXPORT jdoubleArray JNICALL Java_grpl_pathfinder_path_AbstractSpline2d_position(JNIEnv *env, jclass claz, 12 | jlong handle, jdouble t) { 13 | spline_t::vector_t vec = jni_handle(env, handle)->position(t); 14 | return eigen_create_jdoubleArray(env, vec); 15 | } 16 | 17 | JNIEXPORT jdoubleArray JNICALL Java_grpl_pathfinder_path_AbstractSpline2d_derivative(JNIEnv *env, jclass claz, 18 | jlong handle, jdouble t) { 19 | spline_t::vector_t vec = jni_handle(env, handle)->derivative(t); 20 | return eigen_create_jdoubleArray(env, vec); 21 | } 22 | 23 | JNIEXPORT jdoubleArray JNICALL Java_grpl_pathfinder_path_AbstractSpline2d_rotation(JNIEnv *env, jclass claz, 24 | jlong handle, jdouble t) { 25 | spline_t::vector_t vec = jni_handle(env, handle)->rotation(t); 26 | return eigen_create_jdoubleArray(env, vec); 27 | } 28 | 29 | JNIEXPORT jdouble JNICALL Java_grpl_pathfinder_path_AbstractSpline2d_curvature(JNIEnv *env, jclass claz, 30 | jlong handle, jdouble t) { 31 | return jni_handle(env, handle)->curvature(t); 32 | } 33 | 34 | JNIEXPORT void JNICALL Java_grpl_pathfinder_path_AbstractSpline2d_free(JNIEnv *env, jclass claz, 35 | jlong handle) { 36 | delete jni_handle(env, handle); 37 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/path/arc.cpp: -------------------------------------------------------------------------------- 1 | #include "grpl_pathfinder_path_Arc2d.h" 2 | #include "jnieigen.h" 3 | #include "jnihandle.h" 4 | 5 | #include 6 | 7 | using namespace grpl::pf::path; 8 | 9 | JNIEXPORT jlong JNICALL Java_grpl_pathfinder_path_Arc2d_allocate(JNIEnv *env, jclass claz, 10 | jdoubleArray startArr, jdoubleArray midArr, 11 | jdoubleArray endArr) { 12 | using arc_t = arc2d; 13 | 14 | arc_t::vector_t start = eigen_adapt_jdoubleArray(env, startArr); 15 | arc_t::vector_t mid = eigen_adapt_jdoubleArray(env, midArr); 16 | arc_t::vector_t end = eigen_adapt_jdoubleArray(env, endArr); 17 | 18 | return jni_as_handle(new arc_t(start, mid, end)); 19 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/path/arc_parameterizer.cpp: -------------------------------------------------------------------------------- 1 | #include "grpl_pathfinder_path_ArcParameterizer.h" 2 | #include "jnieigen.h" 3 | #include "jnihandle.h" 4 | #include "jniutil.h" 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | using namespace grpl::pf::path; 12 | 13 | using spline_container_t = std::vector>>; 14 | using curve_container_t = std::vector; 15 | 16 | class java_spline_wrapper : public spline<2> { 17 | public: 18 | using vector_t = typename spline::vector_t; 19 | 20 | java_spline_wrapper(JNIEnv *env, jobject obj) : _env(env) { 21 | _obj = env->NewGlobalRef(obj); 22 | _posid = jni_get_method_id(env, obj, "position", "(D)Lgrpl/pathfinder/path/Vec2;"); 23 | _velid = jni_get_method_id(env, obj, "derivative", "(D)Lgrpl/pathfinder/path/Vec2;"); 24 | _rotid = jni_get_method_id(env, obj, "rotation", "(D)Lgrpl/pathfinder/path/Vec2;"); 25 | _curvid = jni_get_method_id(env, obj, "curvature", "(D)D"); 26 | _vecxy = jni_get_method_id(env, "grpl/pathfinder/path/Vec2", "xy", "()[D"); 27 | } 28 | 29 | virtual ~java_spline_wrapper() { _env->DeleteGlobalRef(_obj); } 30 | 31 | vector_t position(double s) override { 32 | jobject vec2 = _env->CallObjectMethod(_obj, _posid, s); 33 | jdoubleArray arr = reinterpret_cast(_env->CallObjectMethod(vec2, _vecxy)); 34 | return eigen_adapt_jdoubleArray(_env, arr); 35 | } 36 | 37 | vector_t derivative(double s) override { 38 | jobject vec2 = _env->CallObjectMethod(_obj, _velid, s); 39 | jdoubleArray arr = reinterpret_cast(_env->CallObjectMethod(vec2, _vecxy)); 40 | return eigen_adapt_jdoubleArray(_env, arr); 41 | } 42 | 43 | vector_t rotation(double s) override { 44 | jobject vec2 = _env->CallObjectMethod(_obj, _rotid, s); 45 | jdoubleArray arr = reinterpret_cast(_env->CallObjectMethod(vec2, _vecxy)); 46 | return eigen_adapt_jdoubleArray(_env, arr); 47 | } 48 | 49 | double curvature(double s) override { return _env->CallDoubleMethod(_obj, _curvid, s); } 50 | 51 | private: 52 | JNIEnv * _env; 53 | jobject _obj; 54 | jmethodID _posid, _velid, _rotid, _curvid, _vecxy; 55 | }; 56 | 57 | JNIEXPORT jlong JNICALL Java_grpl_pathfinder_path_ArcParameterizer_allocate(JNIEnv *env, jclass claz) { 58 | return jni_as_handle(new arc_parameterizer()); 59 | } 60 | 61 | JNIEXPORT void JNICALL Java_grpl_pathfinder_path_ArcParameterizer_free(JNIEnv *env, jclass claz, 62 | jlong handle) { 63 | delete jni_handle(env, handle); 64 | } 65 | 66 | JNIEXPORT void JNICALL Java_grpl_pathfinder_path_ArcParameterizer_configure(JNIEnv *env, jclass claz, 67 | jlong handle, 68 | jdouble maxArcLength, 69 | jdouble maxDeltaCurv) { 70 | jni_handle(env, handle)->configure(maxArcLength, maxDeltaCurv); 71 | } 72 | 73 | JNIEXPORT jlongArray JNICALL Java_grpl_pathfinder_path_ArcParameterizer_parameterize(JNIEnv *env, jclass claz, 74 | jlong handle, 75 | jlong bufferHandle) { 76 | spline_container_t *splines = jni_handle(env, bufferHandle); 77 | curve_container_t curves; 78 | jni_handle(env, handle) 79 | ->parameterize(splines->begin(), splines->end(), std::back_inserter(curves), curves.max_size()); 80 | 81 | return jni_copy_to_handles(env, curves); 82 | } 83 | 84 | JNIEXPORT jlong JNICALL Java_grpl_pathfinder_path_ArcParameterizer_acquireBuffer(JNIEnv *env, jclass claz) { 85 | return jni_as_handle(new spline_container_t()); 86 | } 87 | 88 | JNIEXPORT void JNICALL Java_grpl_pathfinder_path_ArcParameterizer_enqueueNative(JNIEnv *env, jclass claz, 89 | jlong bufferHandle, 90 | jlong splineHandle) { 91 | spline_container_t *splines = jni_handle(env, bufferHandle); 92 | 93 | spline<2> *spl = jni_handle>(env, splineHandle); 94 | splines->push_back(*spl); 95 | } 96 | 97 | JNIEXPORT void JNICALL Java_grpl_pathfinder_path_ArcParameterizer_enqueueAdapter(JNIEnv *env, jclass claz, 98 | jlong bufferHandle, 99 | jobject javaSpline) { 100 | spline_container_t *splines = jni_handle(env, bufferHandle); 101 | spline<2> * spl = new java_spline_wrapper(env, javaSpline); 102 | splines->push_back(*spl); 103 | } 104 | 105 | JNIEXPORT void JNICALL Java_grpl_pathfinder_path_ArcParameterizer_releaseBuffer(JNIEnv *env, jclass claz, 106 | jlong bufferHandle) { 107 | delete jni_handle(env, bufferHandle); 108 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/path/hermite.cpp: -------------------------------------------------------------------------------- 1 | #include "grpl_pathfinder_path_HermiteCubic.h" 2 | #include "grpl_pathfinder_path_HermiteQuintic.h" 3 | #include "jnieigen.h" 4 | #include "jnihandle.h" 5 | 6 | #include 7 | 8 | using namespace grpl::pf::path; 9 | 10 | static typename hermite_cubic::waypoint cubic_waypoint(JNIEnv *env, jdoubleArray wp) { 11 | double *wparr = env->GetDoubleArrayElements(wp, nullptr); 12 | 13 | typename hermite_quintic::vector_t pos{wparr[0], wparr[1]}, tang{wparr[2], wparr[3]}; 14 | env->ReleaseDoubleArrayElements(wp, wparr, 0); 15 | return typename hermite_cubic::waypoint{pos, tang}; 16 | } 17 | 18 | static typename hermite_quintic::waypoint quintic_waypoint(JNIEnv *env, jdoubleArray wp) { 19 | double *wparr = env->GetDoubleArrayElements(wp, nullptr); 20 | 21 | typename hermite_quintic::vector_t pos{wparr[0], wparr[1]}, tang{wparr[2], wparr[3]}, 22 | dtang{wparr[4], wparr[5]}; 23 | env->ReleaseDoubleArrayElements(wp, wparr, 0); 24 | return typename hermite_quintic::waypoint{pos, tang, dtang}; 25 | } 26 | 27 | JNIEXPORT jlong JNICALL Java_grpl_pathfinder_path_HermiteCubic_allocate(JNIEnv *env, jclass clz, 28 | jdoubleArray start, 29 | jdoubleArray end) { 30 | using hermite_t = hermite_cubic; 31 | 32 | hermite_t::waypoint wpstart = cubic_waypoint(env, start); 33 | hermite_t::waypoint wpend = cubic_waypoint(env, end); 34 | 35 | return jni_as_handle(new hermite_t(wpstart, wpend)); 36 | } 37 | 38 | JNIEXPORT jlong JNICALL Java_grpl_pathfinder_path_HermiteQuintic_allocate(JNIEnv *env, jclass clz, 39 | jdoubleArray start, 40 | jdoubleArray end) { 41 | using hermite_t = hermite_quintic; 42 | 43 | hermite_t::waypoint wpstart = quintic_waypoint(env, start); 44 | hermite_t::waypoint wpend = quintic_waypoint(env, end); 45 | 46 | return jni_as_handle(new hermite_t(wpstart, wpend)); 47 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/profile/abstract_profile.cpp: -------------------------------------------------------------------------------- 1 | #include "grpl_pathfinder_profile_AbstractNativeProfile.h" 2 | #include "jnihandle.h" 3 | #include "profile/jniprofile.h" 4 | 5 | #include 6 | 7 | using namespace grpl::pf::profile; 8 | 9 | JNIEXPORT jdoubleArray JNICALL Java_grpl_pathfinder_profile_AbstractNativeProfile_calculateNative( 10 | JNIEnv *env, jclass claz, jlong handle, jdoubleArray last, jdouble lastTime, jdouble time) { 11 | state st = jni_array_to_native_state(env, lastTime, last); 12 | 13 | st = jni_handle(env, handle)->calculate(st, time); 14 | return jni_state_to_array(env, st); 15 | } 16 | 17 | JNIEXPORT void JNICALL Java_grpl_pathfinder_profile_AbstractNativeProfile_setGoal(JNIEnv *env, jclass claz, 18 | jlong handle, jdouble goal) { 19 | jni_handle(env, handle)->set_goal(goal); 20 | } 21 | 22 | JNIEXPORT jdouble JNICALL Java_grpl_pathfinder_profile_AbstractNativeProfile_getGoal(JNIEnv *env, jclass claz, 23 | jlong handle) { 24 | return static_cast(jni_handle(env, handle)->get_goal()); 25 | } 26 | 27 | JNIEXPORT void JNICALL Java_grpl_pathfinder_profile_AbstractNativeProfile_setTimeslice(JNIEnv *env, jclass claz, 28 | jlong handle, jdouble ts) { 29 | jni_handle(env, handle)->set_timeslice(ts); 30 | } 31 | 32 | JNIEXPORT jdouble JNICALL Java_grpl_pathfinder_profile_AbstractNativeProfile_getTimeslice(JNIEnv *env, jclass claz, 33 | jlong handle) { 34 | return static_cast(jni_handle(env, handle)->get_timeslice()); 35 | } 36 | 37 | JNIEXPORT void JNICALL Java_grpl_pathfinder_profile_AbstractNativeProfile_applyLimit(JNIEnv *env, jclass claz, 38 | jlong handle, jint deriv, 39 | jdouble min, jdouble max) { 40 | jni_handle(env, handle)->apply_limit(deriv, min, max); 41 | } 42 | 43 | JNIEXPORT void JNICALL Java_grpl_pathfinder_profile_AbstractNativeProfile_free(JNIEnv *env, jclass claz, 44 | jlong handle) { 45 | delete jni_handle(env, handle); 46 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/profile/jniprofile.cpp: -------------------------------------------------------------------------------- 1 | #include "profile/jniprofile.h" 2 | 3 | grpl::pf::profile::state jni_array_to_native_state(JNIEnv *env, jdouble t, jdoubleArray kinematics) { 4 | return grpl::pf::profile::state{t, eigen_adapt_jdoubleArray(env, kinematics)}; 5 | } 6 | 7 | grpl::pf::profile::state jni_java_to_native_state(JNIEnv *env, jobject obj) { 8 | jdouble t = jni_get_double_field(env, obj, "time"); 9 | 10 | jdoubleArray kin_array = jni_get_field(env, obj, "kinematics", "[D"); 11 | 12 | return jni_array_to_native_state(env, t, kin_array); 13 | } 14 | 15 | jobject jni_state_to_java(JNIEnv *env, grpl::pf::profile::state st) { 16 | jdoubleArray kin_arr = eigen_create_jdoubleArray(env, st.kinematics); 17 | 18 | jobject j_st = 19 | jni_construct(env, "grpl/pathfinder/profile/Profile$State", "(I)V", st.kinematics.size()); 20 | jni_set_double_field(env, j_st, "time", st.time); 21 | jni_set_field(env, j_st, "kinematics", "[D", kin_arr); 22 | return j_st; 23 | } 24 | 25 | jdoubleArray jni_state_to_array(JNIEnv *env, grpl::pf::profile::state st) { 26 | return eigen_create_jdoubleArray(env, st.kinematics); 27 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/profile/profile_adapter.cpp: -------------------------------------------------------------------------------- 1 | // See github issue #11 2 | 3 | // #include "grpl_pathfinder_profile_JavaProfileAdapter.h" 4 | // #include "jnihandle.h" 5 | // #include "profile/jniprofile.h" 6 | 7 | // #include 8 | 9 | // using namespace grpl::pf::profile; 10 | 11 | // class java_profile_adapter : public profile { 12 | // public: 13 | // java_profile_adapter(JNIEnv *env, jobject obj) : _env(env) { 14 | // _obj = env->NewGlobalRef(obj); 15 | // _limterm = env->CallIntMethod(obj, jni_get_method_id(env, obj, "getLimitedTerm", "()I")); 16 | 17 | // _calc = jni_get_method_id(env, obj, "calcFromNative", "([DDD])[D"); 18 | // } 19 | 20 | // const size_t limited_term() const override { return _limterm; } 21 | 22 | // segment calculate(segment &last, double time) override { 23 | // jdoubleArray lastJava = jni_segment_to_array(_env, last); 24 | // jdouble lastTime = last.time; 25 | 26 | // jdoubleArray ret = static_cast(_env->CallObjectMethod(_obj, _calc, lastJava, lastTime, time)); 27 | // return jni_array_to_native_segment(_env, time, ret); 28 | // } 29 | 30 | // private: 31 | // JNIEnv *_env; 32 | // jobject _obj; 33 | // size_t _limterm; 34 | 35 | // jmethodID _calc; 36 | // }; 37 | 38 | // JNIEXPORT jlong JNICALL Java_grpl_pathfinder_profile_JavaProfileAdapter_allocate(JNIEnv *env, jclass claz, 39 | // jobject jprofile) { 40 | // return jni_as_handle(new java_profile_adapter(env, jprofile)); 41 | // } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/profile/trapezoidal_profile.cpp: -------------------------------------------------------------------------------- 1 | #include "grpl_pathfinder_profile_TrapezoidalProfile.h" 2 | #include "jnihandle.h" 3 | #include "profile/jniprofile.h" 4 | 5 | #include 6 | 7 | using namespace grpl::pf::profile; 8 | 9 | JNIEXPORT jlong JNICALL Java_grpl_pathfinder_profile_TrapezoidalProfile_allocate(JNIEnv *env, jclass obj) { 10 | return jni_as_handle(new trapezoidal()); 11 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/transmission/abstract_dc_transmission.cpp: -------------------------------------------------------------------------------- 1 | #include "grpl_pathfinder_transmission_AbstractDcTransmission.h" 2 | #include "jnihandle.h" 3 | 4 | #include 5 | 6 | using namespace grpl::pf::transmission; 7 | 8 | JNIEXPORT jdouble JNICALL Java_grpl_pathfinder_transmission_AbstractDcTransmission_freeSpeed( 9 | JNIEnv *env, jclass claz, jlong handle, jdouble voltage) { 10 | return jni_handle(env, handle)->get_free_speed(voltage); 11 | } 12 | 13 | JNIEXPORT jdouble JNICALL Java_grpl_pathfinder_transmission_AbstractDcTransmission_current( 14 | JNIEnv *env, jclass claz, jlong handle, jdouble voltage, jdouble speed) { 15 | return jni_handle(env, handle)->get_current(voltage, speed); 16 | } 17 | 18 | JNIEXPORT jdouble JNICALL Java_grpl_pathfinder_transmission_AbstractDcTransmission_torque(JNIEnv *env, 19 | jclass claz, 20 | jlong handle, 21 | jdouble current) { 22 | return jni_handle(env, handle)->get_torque(current); 23 | } 24 | 25 | JNIEXPORT jdouble JNICALL Java_grpl_pathfinder_transmission_AbstractDcTransmission_freeVoltage( 26 | JNIEnv *env, jclass claz, jlong handle, jdouble speed) { 27 | return jni_handle(env, handle)->get_free_voltage(speed); 28 | } 29 | 30 | JNIEXPORT jdouble JNICALL Java_grpl_pathfinder_transmission_AbstractDcTransmission_currentVoltage( 31 | JNIEnv *env, jclass claz, jlong handle, jdouble current) { 32 | return jni_handle(env, handle)->get_current_voltage(current); 33 | } 34 | 35 | JNIEXPORT jdouble JNICALL Java_grpl_pathfinder_transmission_AbstractDcTransmission_torqueCurrent( 36 | JNIEnv *env, jclass claz, jlong handle, jdouble torque) { 37 | return jni_handle(env, handle)->get_torque_current(torque); 38 | } 39 | 40 | JNIEXPORT jdouble JNICALL Java_grpl_pathfinder_transmission_AbstractDcTransmission_nominalVoltage( 41 | JNIEnv *env, jclass claz, jlong handle) { 42 | return jni_handle(env, handle)->nominal_voltage(); 43 | } 44 | 45 | JNIEXPORT void JNICALL Java_grpl_pathfinder_transmission_AbstractDcTransmission_free(JNIEnv *env, jclass claz, 46 | jlong handle) { 47 | delete jni_handle(env, handle); 48 | } 49 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/transmission/dc_motor.cpp: -------------------------------------------------------------------------------- 1 | #include "grpl_pathfinder_transmission_DcMotor.h" 2 | #include "jnihandle.h" 3 | 4 | #include 5 | 6 | using namespace grpl::pf::transmission; 7 | 8 | JNIEXPORT jlong JNICALL Java_grpl_pathfinder_transmission_DcMotor_allocate( 9 | JNIEnv *env, jclass claz, jdouble v_nom, jdouble free_speed, jdouble free_current, 10 | jdouble stall_current, jdouble stall_torque) { 11 | return jni_as_handle(new dc_motor(v_nom, free_speed, free_current, stall_current, stall_torque)); 12 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/cpp/transmission/dc_transmission_adapter.cpp: -------------------------------------------------------------------------------- 1 | #include "grpl_pathfinder_transmission_JavaDcTransmissionAdapter.h" 2 | #include "jnihandle.h" 3 | #include "jniutil.h" 4 | 5 | #include 6 | 7 | using namespace grpl::pf::transmission; 8 | 9 | class java_dc_transmission_adapter : public dc_transmission { 10 | public: 11 | java_dc_transmission_adapter(JNIEnv *env, jobject obj) : _env(env) { 12 | _obj = env->NewGlobalRef(obj); 13 | _freespeed = jni_get_method_id(env, obj, "getFreeSpeed", "(D)D"); 14 | _current = jni_get_method_id(env, obj, "getCurrent", "(DD)D"); 15 | _torque = jni_get_method_id(env, obj, "getTorque", "(D)D"); 16 | _freevolt = jni_get_method_id(env, obj, "getFreeVoltage", "(D)D"); 17 | _currvolt = jni_get_method_id(env, obj, "getCurrentVoltage", "(D)D"); 18 | _torcurr = jni_get_method_id(env, obj, "getTorqueCurrent", "(D)D"); 19 | _nomvolt = jni_get_method_id(env, obj, "getNominalVoltage", "()D"); 20 | } 21 | 22 | virtual ~java_dc_transmission_adapter() { _env->DeleteGlobalRef(_obj); } 23 | 24 | double get_free_speed(double voltage) const override { 25 | return _env->CallDoubleMethod(_obj, _freespeed, voltage); 26 | } 27 | 28 | double get_current(double voltage, double speed) const override { 29 | return _env->CallDoubleMethod(_obj, _current, voltage, speed); 30 | } 31 | 32 | double get_torque(double current) const override { return _env->CallDoubleMethod(_obj, _torque, current); } 33 | 34 | double get_free_voltage(double speed) const override { 35 | return _env->CallDoubleMethod(_obj, _freevolt, speed); 36 | } 37 | 38 | double get_current_voltage(double current) const override { 39 | return _env->CallDoubleMethod(_obj, _currvolt, current); 40 | } 41 | 42 | double get_torque_current(double torque) const override { 43 | return _env->CallDoubleMethod(_obj, _torcurr, torque); 44 | } 45 | 46 | double nominal_voltage() const override { return _env->CallDoubleMethod(_obj, _nomvolt); } 47 | 48 | private: 49 | JNIEnv * _env; 50 | jobject _obj; 51 | jmethodID _freespeed, _current, _torque; 52 | jmethodID _freevolt, _currvolt, _torcurr; 53 | jmethodID _nomvolt; 54 | }; 55 | 56 | JNIEXPORT jlong JNICALL Java_grpl_pathfinder_transmission_JavaDcTransmissionAdapter_allocate( 57 | JNIEnv *env, jclass claz, jobject javaobj) { 58 | return jni_as_handle(new java_dc_transmission_adapter(env, javaobj)); 59 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/include/coupled/jnicoupled.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "grpl/pf/coupled/state.h" 5 | #include "jnieigen.h" 6 | #include "jniutil.h" 7 | 8 | grpl::pf::coupled::state jni_coupled_java_to_state(JNIEnv *env, jdoubleArray arr); -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/include/jnieigen.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | template 7 | inline matrix_t eigen_adapt_jdoubleArray(JNIEnv *env, jdoubleArray arr) { 8 | double * kin = env->GetDoubleArrayElements(arr, nullptr); 9 | matrix_t mat = Eigen::Map(kin); 10 | env->ReleaseDoubleArrayElements(arr, kin, 0); 11 | return mat; 12 | } 13 | 14 | template 15 | inline matrix_t eigen_adapt_array(JNIEnv *env, double *arr) { 16 | matrix_t mat = Eigen::Map(arr); 17 | return mat; 18 | } 19 | 20 | template 21 | inline jdoubleArray eigen_create_jdoubleArray(JNIEnv *env, matrix_t mat) { 22 | jdoubleArray arr = env->NewDoubleArray(mat.size()); 23 | env->SetDoubleArrayRegion(arr, 0, mat.size(), mat.data()); 24 | return arr; 25 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/include/jnihandle.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | jfieldID jni_get_handle_field(JNIEnv *env, jobject obj); 6 | 7 | template 8 | T *jni_handle(JNIEnv *env, jlong handle) { 9 | T *t = reinterpret_cast(handle); 10 | if (t == nullptr) { 11 | const char *exbuff = "This object is closed!"; 12 | env->ThrowNew(env->FindClass("grpl/pathfinder/util/ClosedException"), exbuff); 13 | } 14 | return t; 15 | } 16 | 17 | template 18 | T *jni_get_handle(JNIEnv *env, jobject obj) { 19 | jlong handle = env->GetLongField(obj, jni_get_handle_field(env, obj)); 20 | return jni_handle(env, handle); 21 | } 22 | 23 | template 24 | void jni_set_handle(JNIEnv *env, jobject obj, T *t) { 25 | jlong handle = reinterpret_cast(t); 26 | env->SetLongField(obj, jni_get_handle_field(env, obj), handle); 27 | } 28 | 29 | template 30 | jlong jni_as_handle(T *t) { 31 | return reinterpret_cast(t); 32 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/include/jniutil.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | jfieldID jni_get_field_id(JNIEnv *env, jobject obj, const char *name, const char *sig); 7 | jmethodID jni_get_method_id(JNIEnv *env, jobject obj, const char *name, const char *sig); 8 | jmethodID jni_get_method_id(JNIEnv *env, const char *className, const char *name, const char *sig); 9 | 10 | jdouble jni_get_double_field(JNIEnv *env, jobject obj, const char *name); 11 | 12 | void jni_set_double_field(JNIEnv *env, jobject obj, const char *name, jdouble dbl); 13 | 14 | template 15 | type jni_get_field(JNIEnv *env, jobject obj, const char *name, const char *sig) { 16 | return static_cast(env->GetObjectField(obj, jni_get_field_id(env, obj, name, sig))); 17 | } 18 | 19 | template 20 | void jni_set_field(JNIEnv *env, jobject obj, const char *name, const char *sig, type data) { 21 | env->SetObjectField(obj, jni_get_field_id(env, obj, name, sig), static_cast(data)); 22 | } 23 | 24 | jobject jni_construct(JNIEnv *env, const char *cls_name, const char *sig, ...); 25 | 26 | template 27 | jlongArray jni_copy_to_handles(JNIEnv *env, std::vector &vec) { 28 | jlong *addr = new jlong[vec.size()]; 29 | size_t i = 0; 30 | for (typename std::vector::iterator it = vec.begin(); it != vec.end(); ++it) { 31 | // Create a copy of T somewhere on the heap, since it's now Javas to deal with. 32 | addr[i++] = reinterpret_cast(new T(*it)); 33 | } 34 | 35 | jlongArray arr = env->NewLongArray(vec.size()); 36 | env->SetLongArrayRegion(arr, 0, vec.size(), addr); 37 | delete[] addr; 38 | return arr; 39 | } -------------------------------------------------------------------------------- /Pathfinder-Java/src/native/include/profile/jniprofile.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "jnieigen.h" 5 | #include "jniutil.h" 6 | #include "grpl/pf/profile/profile.h" 7 | 8 | grpl::pf::profile::state jni_array_to_native_state(JNIEnv *env, jdouble t, jdoubleArray kinematics); 9 | 10 | grpl::pf::profile::state jni_java_to_native_state(JNIEnv *env, jobject obj); 11 | 12 | jobject jni_state_to_java(JNIEnv *env, grpl::pf::profile::state st); 13 | 14 | jdoubleArray jni_state_to_array(JNIEnv *env, grpl::pf::profile::state st); -------------------------------------------------------------------------------- /Pathfinder-Java/src/test/java/grpl/pathfinder/coupled/CoupledTest.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.coupled; 2 | 3 | import grpl.pathfinder.Vec2; 4 | import grpl.pathfinder.path.ArcParameterizer; 5 | import grpl.pathfinder.path.Curve2d; 6 | import grpl.pathfinder.path.HermiteFactory; 7 | import grpl.pathfinder.path.HermiteQuintic; 8 | import grpl.pathfinder.profile.TrapezoidalProfile; 9 | import grpl.pathfinder.transmission.DcMotor; 10 | import org.junit.jupiter.api.AfterEach; 11 | import org.junit.jupiter.api.BeforeEach; 12 | import org.junit.jupiter.api.Test; 13 | 14 | import java.io.IOException; 15 | import java.io.Writer; 16 | import java.util.ArrayList; 17 | import java.util.List; 18 | 19 | public class CoupledTest { 20 | 21 | DcMotor motor; 22 | CoupledChassis chassis; 23 | CoupledCausalTrajGen gen; 24 | 25 | @BeforeEach 26 | public void setup() { 27 | // Dual CIM 28 | motor = new DcMotor(12.0, 5330 * 2.0 * Math.PI / 60.0 / 12.75, 2 * 2.7, 2 * 131.0, 2 * 2.41 * 12.75); 29 | chassis = new CoupledChassis(motor, motor, 0.0762, 0.5, 25.0); 30 | gen = new CoupledCausalTrajGen(chassis); 31 | } 32 | 33 | @AfterEach 34 | public void tearDown() throws Exception { 35 | motor.close(); 36 | chassis.close(); 37 | gen.close(); 38 | } 39 | 40 | private void csv(Writer writer, Object[] obj) throws IOException { 41 | for (int i = 0; i < obj.length; i++) { 42 | writer.write(obj[i].toString()); 43 | if (i != obj.length - 1) 44 | writer.write(","); 45 | } 46 | writer.write("\n"); 47 | } 48 | 49 | private void echo(Writer writer, CoupledState state) throws IOException { 50 | Object obj[] = new Object[]{ 51 | state.time, state.config.position.x(), state.config.position.y(), 52 | state.config.heading, state.kinematics.distance, state.kinematics.velocity, state.kinematics.acceleration, 53 | state.curvature, 0, 0, 0, 54 | }; 55 | csv(writer, obj); 56 | } 57 | 58 | private void echo(Writer writer, CoupledWheelState state, int idx) throws IOException { 59 | Object obj[] = new Object[]{ 60 | state.time, state.position.x(), state.position.y(), 61 | 0, state.kinematics.distance, state.kinematics.velocity, state.kinematics.acceleration, 62 | 0, idx, state.voltage, state.current 63 | }; 64 | csv(writer, obj); 65 | } 66 | 67 | @Test 68 | public void testCdtHermite() throws IOException { 69 | TrapezoidalProfile profile = new TrapezoidalProfile(); 70 | 71 | List waypoints = new ArrayList<>(); 72 | waypoints.add(new HermiteQuintic.Waypoint(Vec2.cartesian(0, 0), Vec2.cartesian(5, 0), Vec2.cartesian(0, 0))); 73 | waypoints.add(new HermiteQuintic.Waypoint(Vec2.cartesian(4, 4), Vec2.cartesian(0, 5), Vec2.cartesian(0, 0))); 74 | 75 | List hermites = HermiteFactory.generateQuintic(waypoints); 76 | ArcParameterizer param = new ArcParameterizer(); 77 | param.configure(0.01, 0.01); 78 | List curves = param.parameterize(hermites); 79 | 80 | gen.configure(curves, profile); 81 | 82 | CoupledState state = new CoupledState(); 83 | 84 | // FileWriter out = new FileWriter("cdt.csv"); 85 | // csv(out, new String[] { 86 | // "t", "x", "y", "heading", "distance", "derivative", 87 | // "acceleration", "curvature", "path", "voltage", "current" 88 | // }); 89 | 90 | for (double t = 0; !state.finished && t < 5; t += 0.01) { 91 | state = gen.generate(state, t); 92 | // echo(out, state); 93 | 94 | CoupledWheelState[] wheels = chassis.split(state); 95 | // echo(out, wheels[CoupledCausalTrajGen.LEFT], 1); 96 | // echo(out, wheels[CoupledCausalTrajGen.RIGHT], 2); 97 | } 98 | 99 | // out.close(); 100 | param.close(); 101 | profile.close(); 102 | 103 | } 104 | } 105 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/test/java/grpl/pathfinder/path/ArcParameterizerTest.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.path; 2 | 3 | import grpl.pathfinder.Vec2; 4 | import org.junit.jupiter.api.BeforeEach; 5 | import org.junit.jupiter.api.Test; 6 | 7 | import java.io.IOException; 8 | import java.util.ArrayList; 9 | import java.util.List; 10 | 11 | public class ArcParameterizerTest { 12 | 13 | ArcParameterizer arcParam; 14 | List nativeSplines; 15 | 16 | @BeforeEach 17 | void createParam() { 18 | arcParam = new ArcParameterizer(); 19 | arcParam.configure(0.1, 0.1); 20 | 21 | List wps = new ArrayList<>(); 22 | wps.add(new HermiteCubic.Waypoint(Vec2.cartesian(2, 2), Vec2.cartesian(5, 0))); 23 | wps.add(new HermiteCubic.Waypoint(Vec2.cartesian(5, 5), Vec2.cartesian(3, 2))); 24 | wps.add(new HermiteCubic.Waypoint(Vec2.cartesian(10, 5), Vec2.cartesian(0, 5))); 25 | nativeSplines = HermiteFactory.generateCubic(wps); 26 | } 27 | 28 | @Test 29 | void testParameterization() throws IOException { 30 | List arcs = arcParam.parameterize(nativeSplines); 31 | // BufferedWriter writer = new BufferedWriter(new FileWriter("javaparam.csv")); 32 | // writer.write("x,y\n"); 33 | // arcs.forEach((arc) -> { 34 | // for (double s = 0; s < arc.length(); s+=0.01) { 35 | // Vec2 pos = arc.position(s); 36 | // try { 37 | // writer.write(pos.x() + "," + pos.y() + "\n"); 38 | // } catch (IOException e) { 39 | // e.printStackTrace(); 40 | // } 41 | // } 42 | // }); 43 | // writer.close(); 44 | } 45 | 46 | } 47 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/test/java/grpl/pathfinder/path/HermiteCubicTest.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.path; 2 | 3 | import grpl.pathfinder.Vec2; 4 | import org.junit.jupiter.api.BeforeEach; 5 | import org.junit.jupiter.api.Test; 6 | 7 | import static org.junit.jupiter.api.Assertions.*; 8 | 9 | public class HermiteCubicTest { 10 | 11 | HermiteCubic.Waypoint start, end; 12 | HermiteCubic spline; 13 | 14 | @BeforeEach 15 | void createSpline() { 16 | start = new HermiteCubic.Waypoint(Vec2.cartesian(0, 0), Vec2.polar(5, 0)); 17 | end = new HermiteCubic.Waypoint(Vec2.cartesian(5, 4), Vec2.polar(5, -Math.PI / 2.0)); 18 | spline = new HermiteCubic(start, end); 19 | } 20 | 21 | @Test 22 | void testStartPoint() { 23 | assertEquals(start.position, spline.position(0)); 24 | assertEquals(start.tangent, spline.derivative(0)); 25 | } 26 | 27 | @Test 28 | void testEndPoint() { 29 | assertEquals(end.position, spline.position(1)); 30 | assertEquals(end.tangent, spline.derivative(1)); 31 | } 32 | 33 | } 34 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/test/java/grpl/pathfinder/path/HermiteQuinticTest.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.path; 2 | 3 | import grpl.pathfinder.Vec2; 4 | import org.junit.jupiter.api.BeforeEach; 5 | import org.junit.jupiter.api.Test; 6 | 7 | import static org.junit.jupiter.api.Assertions.assertEquals; 8 | 9 | public class HermiteQuinticTest { 10 | 11 | HermiteQuintic.Waypoint start, end; 12 | HermiteQuintic spline; 13 | 14 | @BeforeEach 15 | void createSpline() { 16 | start = new HermiteQuintic.Waypoint(Vec2.cartesian(0, 0), Vec2.polar(5, 0), Vec2.polar(3,3 )); 17 | end = new HermiteQuintic.Waypoint(Vec2.cartesian(5, 4), Vec2.polar(5, -Math.PI / 2.0), Vec2.polar(-2, 3)); 18 | spline = new HermiteQuintic(start, end); 19 | } 20 | 21 | @Test 22 | void testStartPoint() { 23 | assertEquals(start.position, spline.position(0)); 24 | assertEquals(start.tangent, spline.derivative(0)); 25 | } 26 | 27 | @Test 28 | void testEndPoint() { 29 | assertEquals(end.position, spline.position(1)); 30 | assertEquals(end.tangent, spline.derivative(1)); 31 | } 32 | 33 | } 34 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/test/java/grpl/pathfinder/profile/TrapezoidalProfileTest.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.profile; 2 | 3 | import grpl.pathfinder.util.NativeResource; 4 | import grpl.pathfinder.util.NativeResourceTest; 5 | import org.junit.jupiter.api.BeforeEach; 6 | import org.junit.jupiter.api.Test; 7 | 8 | import static org.junit.jupiter.api.Assertions.*; 9 | 10 | public class TrapezoidalProfileTest extends NativeResourceTest { 11 | 12 | TrapezoidalProfile profile; 13 | 14 | @BeforeEach 15 | void createProfile() { 16 | profile = new TrapezoidalProfile(); 17 | } 18 | 19 | @Override 20 | protected NativeResource nativeResource() { 21 | return profile; 22 | } 23 | 24 | @Test 25 | void testGoal() { 26 | profile.setGoal(12.0); 27 | assertEquals(12.0, profile.getGoal(), 1e-12); 28 | } 29 | 30 | @Test 31 | void testTimeslice() { 32 | profile.setTimeslice(0.123); 33 | assertEquals(0.123, profile.getTimeslice(), 1e-12); 34 | } 35 | 36 | @Test 37 | void testProfile() { 38 | profile.applyLimit(Profile.VELOCITY, -3, 3); 39 | profile.applyLimit(Profile.ACCELERATION, -3, 4); 40 | profile.setGoal(5.0); 41 | profile.setTimeslice(0); 42 | 43 | Profile.State state = profile.createState(); 44 | for (double t = 0; t < 7; t+=0.01) { 45 | state = profile.calculate(state, t); 46 | assertTrue(Math.abs(state.kinematics[Profile.VELOCITY]) <= 3); 47 | assertTrue(Math.abs(state.kinematics[Profile.VELOCITY]) <= 4); 48 | } 49 | assertEquals(5.0, state.kinematics[Profile.POSITION], 0.001); 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /Pathfinder-Java/src/test/java/grpl/pathfinder/util/NativeResourceTest.java: -------------------------------------------------------------------------------- 1 | package grpl.pathfinder.util; 2 | 3 | import org.junit.jupiter.api.Disabled; 4 | import org.junit.jupiter.api.Test; 5 | 6 | import static org.junit.jupiter.api.Assertions.*; 7 | 8 | public abstract class NativeResourceTest { 9 | 10 | protected abstract NativeResource nativeResource(); 11 | 12 | @Test 13 | void testClose() throws Exception { 14 | assertFalse(nativeResource().closed()); 15 | nativeResource().close(); 16 | assertTrue(nativeResource().closed()); 17 | } 18 | 19 | } 20 | -------------------------------------------------------------------------------- /Pathfinder/build.gradle: -------------------------------------------------------------------------------- 1 | plugins { 2 | id 'cpp' 3 | id 'google-test-test-suite' 4 | } 5 | 6 | apply from: 'plot.gradle' 7 | 8 | model { 9 | binaries { 10 | all { 11 | if (!(toolChain instanceof Clang) && targetPlatform.name == edu.wpi.first.toolchain.NativePlatforms.desktop) 12 | println "WARNING: Not using Clang. Additional warnings and sanitation may be missing." 13 | } 14 | } 15 | components { 16 | pathfinder(NativeLibrarySpec) { 17 | sources.cpp { 18 | source { 19 | srcDir 'src/cpp' 20 | include '**/*.cpp' 21 | } 22 | exportedHeaders { 23 | srcDir 'src/include' 24 | include '**/*.h' 25 | } 26 | lib project: ':libs', library: "eigen", linkage: "api" 27 | } 28 | } 29 | } 30 | testSuites { 31 | pathfinderBench(GoogleTestTestSuiteSpec) { 32 | testing $.components.pathfinder 33 | 34 | binaries.all { 35 | if (toolChain instanceof GccCompatibleToolChain) { 36 | linker.args << '-pthread' 37 | } else { 38 | // MSVC 39 | linker.args << 'shlwapi.lib' 40 | } 41 | if (!project.hasProperty("withBench")) 42 | tasks.withType(RunTestExecutable) { RunTestExecutable task -> 43 | task.enabled = false 44 | } 45 | } 46 | 47 | sources.cpp { 48 | source { 49 | srcDir 'src/bench' 50 | include '**/*.cpp' 51 | } 52 | lib project: ':libs', library: 'googleBench', linkage: 'static' 53 | lib project: ':libs', library: 'eigen', linkage: 'api' 54 | } 55 | } 56 | 57 | pathfinderTest(GoogleTestTestSuiteSpec) { 58 | testing $.components.pathfinder 59 | 60 | binaries.all { 61 | cppCompiler.define 'EIGEN_RUNTIME_NO_MALLOC' 62 | if (toolChain instanceof GccCompatibleToolChain) { 63 | linker.args << '-pthread' 64 | 65 | if (toolChain instanceof Clang && targetPlatform.name == edu.wpi.first.toolchain.NativePlatforms.desktop) { 66 | cppCompiler.args << '-fsanitize=address' 67 | linker.args << '-fsanitize=address' 68 | } 69 | } else { 70 | // Windows breaks Eigen's static alignment. Seems to be a compiler bug. 71 | // https://forum.kde.org/viewtopic.php?f=74&t=97995&sid=cd769a78f5b8b159239b9f032e2b7432 72 | cppCompiler.define 'EIGEN_DONT_ALIGN_STATICALLY' 73 | } 74 | } 75 | 76 | sources.cpp { 77 | source { 78 | srcDir 'src/test' 79 | include '**/*.cpp' 80 | } 81 | exportedHeaders { 82 | srcDir 'src/test' 83 | include '**/*.h' 84 | } 85 | lib project: ':libs', library: "eigen", linkage: "api" 86 | } 87 | } 88 | } 89 | } 90 | 91 | doxygen { 92 | executables { 93 | doxygen version : '1.8.13' 94 | } 95 | 96 | generate_html true 97 | javadoc_autobrief true 98 | source project.file('src/include') 99 | } 100 | 101 | task zipHeaders(type: Zip) { 102 | from fileTree('src/include') 103 | 104 | baseName = "Pathfinder" 105 | classifier = "headers" 106 | } 107 | 108 | task zipDoxygen(type: Zip) { 109 | from doxygen 110 | baseName = "Pathfinder" 111 | classifier = "doxygen" 112 | } 113 | 114 | publishing { 115 | publications { 116 | pathfinder(MavenPublication) { 117 | artifactId 'Pathfinder' 118 | 119 | artifact zipHeaders { 120 | classifier 'headers' 121 | } 122 | 123 | artifact zipDoxygen { 124 | classifier 'doxygen' 125 | } 126 | } 127 | pathfinderTest(MavenPublication) { 128 | groupId += ".testing" 129 | artifactId 'Pathfinder-Test' 130 | 131 | binaryArtifacts(it, "pathfinderTest", false) 132 | } 133 | pathfinderBench(MavenPublication) { 134 | groupId += ".testing" 135 | artifactId 'Pathfinder-Bench' 136 | 137 | binaryArtifacts(it, "pathfinderBench", false) 138 | } 139 | } 140 | } 141 | 142 | task cleanTest() { 143 | doLast { 144 | delete { 145 | delete fileTree(dir: new File(project.buildDir, "test-results"), include: ["**/*.png", "**/*.csv"]) 146 | } 147 | } 148 | } 149 | 150 | check.finalizedBy plot -------------------------------------------------------------------------------- /Pathfinder/plot.gradle: -------------------------------------------------------------------------------- 1 | task plot() 2 | 3 | def plot_supported = true 4 | try { 5 | exec { 6 | commandLine 'gnuplot', '--version' 7 | } 8 | } catch (e) { 9 | println "GNUPlot Not Found!" 10 | plot_supported = false; 11 | } 12 | 13 | // if (plot_supported) { 14 | // def loadpath = file('src/testplot/') 15 | // fileTree(dir: loadpath, include: '**/*.plt').files.each { file -> 16 | // def t = tasks.create("plot${file.name.capitalize()}") { 17 | // doLast { 18 | // exec { 19 | // workingDir new File(project.buildDir, "test-results/pathfinderTest/any64/release") 20 | // commandLine 'gnuplot', '-e', "set loadpath \"${loadpath.absolutePath}\";load \"settings.gp\"", file.absolutePath 21 | // } 22 | // } 23 | // } 24 | // plot.dependsOn t 25 | // } 26 | // } -------------------------------------------------------------------------------- /Pathfinder/src/bench/bench_main.cpp: -------------------------------------------------------------------------------- 1 | #include "benchmark/benchmark.h" 2 | 3 | BENCHMARK_MAIN(); -------------------------------------------------------------------------------- /Pathfinder/src/bench/coupled/bench_cdt_fullpipe.cpp: -------------------------------------------------------------------------------- 1 | #include "grpl/pf/coupled/causal_trajectory_generator.h" 2 | #include "grpl/pf/path/hermite.h" 3 | #include "grpl/pf/path/arc_parameterizer.h" 4 | #include "grpl/pf/profile/trapezoidal.h" 5 | 6 | #include 7 | 8 | using namespace grpl::pf; 9 | 10 | #include 11 | 12 | static void BM_CDT_Full(benchmark::State &state) { 13 | using hermite_t = path::hermite_quintic; 14 | using profile_t = profile::trapezoidal; 15 | 16 | hermite_t::waypoint start{{2, 2}, {5, 0}, {0, 0}}, end{{5, 5}, {5, 5}, {0, 0}}; 17 | hermite_t hermite(start, end); 18 | 19 | double G = 12.75; 20 | transmission::dc_motor dualCIM{12.0, 5330 * 2.0 * constants::PI / 60.0 / G, 2 * 2.7, 2 * 131.0, 21 | 2 * 2.41 * G}; 22 | coupled::chassis chassis{dualCIM, dualCIM, 0.0762, 0.5, 25.0}; 23 | 24 | int num_iter = 0; 25 | int num_gens = 0; 26 | int num_curves = 0; 27 | double realtime = 0; 28 | 29 | double loop_time = 1.0 / static_cast(state.range(0)); 30 | 31 | for (auto _ : state) { 32 | state.PauseTiming(); 33 | // Setup 34 | // Param 35 | path::arc_parameterizer param; 36 | param.configure(0.01, 0.01); 37 | // Curves Buffer 38 | std::vector curves; 39 | curves.reserve(1024); 40 | // Profile 41 | profile_t profile; 42 | // Coupled 43 | coupled::causal_trajectory_generator gen; 44 | coupled::state c_state; 45 | state.ResumeTiming(); 46 | 47 | // Benchmark Start 48 | param.parameterize(hermite, std::back_inserter(curves), curves.max_size()); 49 | num_curves += curves.size(); 50 | 51 | double t; 52 | for (t = 0; !c_state.finished && t < 5.0; t += loop_time) { 53 | c_state = gen.generate(chassis, curves.begin(), curves.end(), profile, c_state, t); 54 | std::pair split = chassis.split(c_state); 55 | benchmark::DoNotOptimize(split); 56 | num_gens++; 57 | } 58 | realtime += t; 59 | num_iter++; 60 | } 61 | 62 | state.counters["NumStates"] = num_gens / num_iter; 63 | state.counters["NumCurves"] = num_curves / num_iter; 64 | state.counters["LoopDt"] = loop_time; 65 | state.counters["MPExecTime"] = realtime / num_iter; 66 | state.SetComplexityN(state.range(0)); 67 | } 68 | 69 | BENCHMARK(BM_CDT_Full)->Arg(10)->Arg(100)->Arg(1000)->Complexity()->Unit(benchmark::kMillisecond); -------------------------------------------------------------------------------- /Pathfinder/src/bench/path/bench_arc_param.cpp: -------------------------------------------------------------------------------- 1 | #include "grpl/pf/path/arc_parameterizer.h" 2 | #include "grpl/pf/path/hermite.h" 3 | 4 | #include 5 | 6 | using namespace grpl::pf; 7 | using namespace grpl::pf::path; 8 | 9 | #include 10 | 11 | static void BM_ArcParamHermite(benchmark::State &state) { 12 | using hermite_t = hermite_quintic; 13 | 14 | hermite_t::waypoint start{{2, 2}, {5, 0}, {0, 0}}, end{{5, 5}, {5, 5}, {0, 0}}; 15 | hermite_t hermite(start, end); 16 | 17 | int num_curves = 0, num_iter = 0; 18 | 19 | for (auto _ : state) { 20 | state.PauseTiming(); 21 | std::vector curves; 22 | arc_parameterizer param; 23 | double sensitivity = 1.0 / static_cast(state.range(0)); 24 | param.configure(sensitivity, sensitivity); 25 | curves.reserve(param.curve_count(hermite)); // Ensure the timing isn't affected by reallocs 26 | state.ResumeTiming(); 27 | 28 | param.parameterize(hermite, std::back_inserter(curves), curves.max_size()); 29 | num_curves += curves.size(); 30 | num_iter++; 31 | } 32 | 33 | state.counters["NumCurves"] = num_curves / num_iter; 34 | state.SetComplexityN(state.range(0)); 35 | } 36 | 37 | BENCHMARK(BM_ArcParamHermite)->Arg(1)->Arg(10)->Arg(100)->Arg(1000)->Complexity()->Unit(benchmark::kMillisecond); -------------------------------------------------------------------------------- /Pathfinder/src/bench/profile/bench_trapezoidal.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | using namespace grpl::pf::profile; 6 | 7 | static void BM_Profile_Trapezoidal(benchmark::State &state) { 8 | trapezoidal pr; 9 | pr.apply_limit(1, -3, 3); // Velocity Limit = -3 to 3m/s 10 | pr.apply_limit(2, -3, 4); // Acceleration limit = -3 to 4m/s 11 | pr.set_goal(5); // Goal = 5m 12 | pr.set_timeslice(0); // No Timeslice 13 | 14 | for (auto _ : state) { 15 | ::grpl::pf::profile::state st; 16 | double dt = 1.0 / static_cast(state.range(0)); 17 | for (double t = 0; t < 10; t += dt) { 18 | benchmark::DoNotOptimize(st = pr.calculate(st, t)); 19 | } 20 | } 21 | 22 | state.SetComplexityN(state.range(0)); 23 | } 24 | 25 | BENCHMARK(BM_Profile_Trapezoidal)->Arg(10)->Arg(100)->Arg(1000)->Arg(10000)->Complexity()->Unit(benchmark::kMillisecond); -------------------------------------------------------------------------------- /Pathfinder/src/include/grpl/pf.h: -------------------------------------------------------------------------------- 1 | #include "pf/pathfinder.h" -------------------------------------------------------------------------------- /Pathfinder/src/include/grpl/pf/constants.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define _USE_MATH_DEFINES 4 | #include 5 | 6 | #include 7 | 8 | namespace grpl { 9 | namespace pf { 10 | //! Vector index for position (metres) 11 | const size_t POSITION = 0; 12 | //! Vector index for velocity (metres per second, ms^-1) 13 | const size_t VELOCITY = 1; 14 | //! Vector index for acceleration (metres per second per second, ms^-2) 15 | const size_t ACCELERATION = 2; 16 | //! Vector index for jerk (ms^-3) 17 | const size_t JERK = 3; 18 | 19 | namespace constants { 20 | constexpr double PI = M_PI; 21 | //! Comparation threshold for floating point numbers 22 | constexpr double epsilon = 1e-10; 23 | //! Comparison threshold for inf 24 | constexpr double almost_inf = 1e10; 25 | 26 | //! Default acceptable error when comparing setpoints and process variables. 27 | constexpr double default_acceptable_error = 1e-5; 28 | 29 | const size_t profile_kinematics_order = 3; // Pos, Vel, Acc 30 | const size_t profile_limits_order = 4; // <>, Vel, Acc, Jerk 31 | } // namespace constants 32 | } // namespace pf 33 | } // namespace grpl -------------------------------------------------------------------------------- /Pathfinder/src/include/grpl/pf/coupled/causal_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "chassis.h" 4 | #include "grpl/pf/path/curve.h" 5 | #include "grpl/pf/profile/profile.h" 6 | #include "state.h" 7 | 8 | namespace grpl { 9 | namespace pf { 10 | namespace coupled { 11 | 12 | /** 13 | * Causal trajectory generator, given the chassis, path and motion profile. 14 | * 15 | * This class provides the necessary plumbing in order to generate trajectories from a given path, 16 | * applying motion and timing information based on the limits and constraints of a drivetrain. The 17 | * generator applies a motion profile to a path, while fitting within given constraints. 18 | * 19 | * This generator is causal and will generate a single state (output) with knowledge only of the current 20 | * state of the system. This generator can be used on-the-fly, and is primarily advantageous in speed and 21 | * flexibility. The cost is accuracy towards the end of the path, as a sudden deceleration may be required 22 | * with insufficient time steps, as the end velocity may not be zero. 23 | * 24 | * It is recommended to use this generator if on-the-fly generation is required, or memory and 25 | * computational resources are sparse. This generator also enables the ability to provide feedback 26 | * information on chassis kinematics to the next generation step, making it possible to embed a feedback 27 | * loop into the generation phase. 28 | */ 29 | class causal_trajectory_generator { 30 | public: 31 | using vector_t = Eigen::Vector2d; 32 | 33 | // TODO: Rename this class to something more fitting (drivetrain is a synonym for chassis). This 34 | // class can also be split apart to migrate most calculation to chassis, while keeping only the 35 | // parts required to follow a causal path within this class. 36 | 37 | /** 38 | * Generate the next state of the trajectory given the current state. 39 | * 40 | * The curves in this system define the path that will be followed. 41 | * 42 | * @param chassis The coupled chassis, used to provide limits for the trajectory the trajectory 43 | * kinematics. 44 | * @param curve_begin Iterator pointing to the start of the curve collection. See @ref std::iterator. 45 | * @param curve_end Iterator pointing to the end of the curve collection. See @ref std::iterator. 46 | * @param profile Reference to the profile to use. The profile must, at a minimum, provide 47 | * continous, bounded velocity and bounded acceleration. 48 | * @ref grpl::pf::profile::trapezoidal is recommended. 49 | * @param last The current ("last") state of the trajectory. If this is the first call to 50 | * generate, this may be considered the "initial conditions". 51 | * @param time The time of the next point (last.time + dt), in seconds. 52 | */ 53 | template 54 | state generate(chassis &chassis, const iterator_curve_t curve_begin, const iterator_curve_t curve_end, 55 | profile::profile &profile, state &last, double time) { 56 | path::curve<2> *curve; 57 | state output; 58 | double total_length, curve_distance; 59 | double distance = last.kinematics[0]; 60 | 61 | curve = find_curve(distance, curve_begin, curve_end, curve_distance, total_length); 62 | 63 | // TODO: The epsilon of the profile causes this to never advance, meaning the path 64 | // is never marked as 'finished' on some timesteps. 65 | if (curve == nullptr) { 66 | output = last; 67 | output.finished = true; 68 | return output; 69 | } 70 | 71 | profile.set_goal(total_length); 72 | 73 | vector_t centre = curve->position(curve_distance); 74 | vector_t centre_rot = curve->rotation(curve_distance); 75 | double curvature = curve->curvature(curve_distance); 76 | double dcurvature = curve->dcurvature(curve_distance); 77 | 78 | double heading = atan2(centre_rot.y(), centre_rot.x()); 79 | configuration_state config{centre.x(), centre.y(), heading}; 80 | 81 | output.time = time; 82 | output.config = config; 83 | 84 | // TODO: Allow multiple constraints (like current limits) 85 | // TODO: Enforce minimum acceleration constraints in profiles. 86 | // TODO: Does limiting jerk prevent oscillation 87 | double limit_vel = chassis.linear_vel_limit(config, curvature); 88 | std::pair limit_acc = 89 | chassis.acceleration_limits(config, curvature, last.kinematics[1]); 90 | 91 | profile.apply_limit(1, -limit_vel, limit_vel); 92 | profile.apply_limit(2, limit_acc.first, limit_acc.second); 93 | 94 | profile::state prof_state; 95 | prof_state.time = last.time; 96 | prof_state.kinematics = last.kinematics; 97 | 98 | prof_state = profile.calculate(prof_state, time); 99 | 100 | output.kinematics = prof_state.kinematics; 101 | output.curvature = curvature; 102 | output.dcurvature = dcurvature; 103 | 104 | output.finished = false; 105 | return output; 106 | } 107 | 108 | private: 109 | // TODO: We can store the last known curve to make lookup faster, but will cause random-access slowdown. 110 | // That's a fixable problem with a settable parameter to optimize for either sequential or random 111 | // access. 112 | template 113 | inline path::curve<2> *find_curve(double targ_len, const iterator_curve_t curve_begin, 114 | const iterator_curve_t curve_end, double &curve_len_out, 115 | double &total_len_out) { 116 | path::curve<2> *curve_out = nullptr; 117 | curve_len_out = targ_len; 118 | total_len_out = 0; 119 | 120 | for (iterator_curve_t it = curve_begin; it != curve_end; it++) { 121 | path::curve<2> &curr = *it; 122 | 123 | double len = curr.length(); 124 | // If we haven't found a curve, and the current length of the curve will put us ahead 125 | // of our distance target. 126 | if (curve_out == nullptr && (len + total_len_out) >= targ_len) { 127 | curve_len_out = targ_len - total_len_out; 128 | curve_out = &curr; 129 | } 130 | total_len_out += len; 131 | } 132 | return curve_out; 133 | } 134 | }; 135 | } // namespace coupled 136 | } // namespace pf 137 | } // namespace grpl -------------------------------------------------------------------------------- /Pathfinder/src/include/grpl/pf/coupled/state.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace grpl { 6 | namespace pf { 7 | /** 8 | * Coupled / tank / differential drivetrain models. 9 | * 10 | * The grpl::pf::coupled namespace contains all models and other structures for use 11 | * with coupled-style drivetrains. 12 | * 13 | * A coupled-style drivetrain defines a drivetrain with individually controlled, parallel 14 | * left and right sides, also known as a tank drive or differential drive. May also refer 15 | * to skid-steer systems. 16 | */ 17 | namespace coupled { 18 | 19 | /** 20 | * Drivetrain configuration state, describing the configuration of the chassis. 21 | * 22 | * Formally, the 'configuration' of a body is all variables required to fully define the position 23 | * of the body and its manipulators. For a coupled drivetrain, this is simply its position in the 24 | * the work space, as well as its heading. In the case of the coupled drivetrain, this may be referred 25 | * to as the "transform" of the chassis. 26 | * 27 | * @param x The x position of the centre of the drivetrain, in metres. 28 | * @param y The y position of the centre of the drivetrain, in metres. 29 | * @param heading The heading of the drivetrain, in radians. 30 | */ 31 | using configuration_state = Eigen::Vector3d; 32 | 33 | /** 34 | * Drivetrain kinematic state, describing the movement and motion of the chassis. 35 | * 36 | * @param distance The distance covered by the drivetrain, in metres. 37 | * @param velocity The linear velocity of the drivetrain, in metres per second (ms^-1). 38 | * @param acceleration The linear acceleration of the drivetrain, in metres per second 39 | * per second (ms^-2). 40 | */ 41 | using kinematic_state = Eigen::Vector3d; 42 | 43 | /** 44 | * The state of a coupled drivetrain at any point in time, as a single state within a 45 | * trajectory. 46 | */ 47 | struct state { 48 | //! The time point of this state, in seconds. 49 | double time = 0; 50 | //! The instantaneous curvature of the state, in m^-1 51 | double curvature = 0; 52 | //! The instantaneous change in curvature of the state, in m^-2 (dk/ds) 53 | double dcurvature = 0; 54 | //! The configuration of the chassis at the time of the state. 55 | configuration_state config = configuration_state::Zero(); 56 | //! The kinematics of the chassis at the time of the state. 57 | kinematic_state kinematics = kinematic_state::Zero(); 58 | bool finished = false; 59 | }; 60 | 61 | /** 62 | * The state of a wheel (side) of the coupled drivetrain at any point in time, primarily 63 | * for use with encoders / other following regimes. 64 | */ 65 | struct wheel_state { 66 | /** 67 | * Position vector of the wheel 68 | * 69 | * @param x The x position of the wheel, in metres. 70 | * @param y The y position of the wheel, in metres. 71 | */ 72 | using vector_t = Eigen::Vector2d; 73 | 74 | //! The time point of this state, in seconds. 75 | double time = 0; 76 | //! The position of the wheel at the time of the state. 77 | vector_t position = vector_t::Zero(); 78 | //! The kinematics of the wheel at the time of the state. Note this is linear, not rotational. 79 | kinematic_state kinematics = kinematic_state::Zero(); 80 | //! The voltage applied to the transmission connected to this wheel, in Volts 81 | double voltage = 0; 82 | //! The current drawn by the transmission connected to this wheel, in Amperes. 83 | double current = 0; 84 | bool finished = false; 85 | }; 86 | } // namespace coupled 87 | } // namespace pf 88 | } // namespace grpl -------------------------------------------------------------------------------- /Pathfinder/src/include/grpl/pf/path/arc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "curve.h" 4 | #include "grpl/pf/constants.h" 5 | 6 | namespace grpl { 7 | namespace pf { 8 | namespace path { 9 | /** 10 | * A 2-Dimensional Circular Arc 11 | * 12 | * Implementation of a 2-Dimensional circular arc, parameterized to arc length 's'. 13 | */ 14 | class arc2d : public curve<2> { 15 | public: 16 | using vector_t = typename curve::vector_t; 17 | 18 | arc2d() {} 19 | 20 | /** 21 | * Create a circular arc from a set of 3 points (start, any, and end). 22 | * 23 | * @param start The start point of the curve, in x,y metres. 24 | * @param mid Any point along the curve sitting between start and end, 25 | * in x,y metres. 26 | * @param end The end point of the curve, in x,y metres. 27 | */ 28 | arc2d(vector_t start, vector_t mid, vector_t end) { from_three(start, mid, end); } 29 | 30 | vector_t position(const double s) const override { 31 | double curv = _curvature; 32 | if (curv != 0) { 33 | double angle = _angle_offset + (s * curv); 34 | return _ref + vector_t{cos(angle) / fabs(curv), sin(angle) / fabs(curv)}; 35 | } else { 36 | return _ref + _delta * (s / _length); 37 | } 38 | } 39 | 40 | vector_t derivative(const double s) const override { 41 | double curv = _curvature; 42 | if (curv != 0) { 43 | double sign = curv > 0 ? 1 : -1; 44 | double angle = _angle_offset + (s * curv); 45 | double off = sign * constants::PI / 2; 46 | return vector_t{cos(angle + off), sin(angle + off)}; 47 | } else { 48 | return _delta; 49 | } 50 | } 51 | 52 | double curvature(const double s) const override { return _curvature; } 53 | 54 | double dcurvature(const double s) const override { return 0; } 55 | 56 | double length() const override { return _length; } 57 | 58 | private: 59 | void from_three(vector_t start, vector_t mid, vector_t end) { 60 | Eigen::Matrix coeffmatrix; 61 | Eigen::Matrix rvec; 62 | 63 | coeffmatrix << 2 * (start[0] - end[0]), 2 * (start[1] - end[1]), 2 * (start[0] - mid[0]), 64 | 2 * (start[1] - mid[1]); 65 | 66 | if (coeffmatrix.determinant() == 0) { 67 | // start, mid and end are all colinear, therefore 68 | // this arc is, in fact, a straight line. 69 | _curvature = 0; 70 | _ref = start; 71 | _delta = end - start; 72 | _length = _delta.norm(); 73 | _angle_offset = atan2(_delta[1], _delta[0]); 74 | } else { 75 | rvec << start.squaredNorm() - end.squaredNorm(), start.squaredNorm() - mid.squaredNorm(); 76 | 77 | _ref = coeffmatrix.inverse() * rvec; 78 | 79 | _angle_offset = atan2((start - _ref)[1], (start - _ref)[0]); 80 | double angle1 = atan2((end - _ref)[1], (end - _ref)[0]); 81 | 82 | _curvature = 1.0 / (start - _ref).norm(); 83 | _length = fabs((angle1 - _angle_offset)) / _curvature; 84 | _curvature *= ((angle1 - _angle_offset) < 0 ? -1 : 1); 85 | } 86 | } 87 | 88 | // Line: Initial point, Arc: Centre Point 89 | vector_t _ref; 90 | // Line uses delta (more efficient, no trig calcs), 91 | // whilst circle uses angle offset. 92 | vector_t _delta; 93 | double _angle_offset; 94 | double _curvature; 95 | double _length; 96 | }; 97 | } // namespace path 98 | } // namespace pf 99 | } // namespace grpl -------------------------------------------------------------------------------- /Pathfinder/src/include/grpl/pf/path/augmented_arc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "arc.h" 4 | 5 | namespace grpl { 6 | namespace pf { 7 | namespace path { 8 | /** 9 | * Implementation of @ref arc2d with non-constant curvature. 10 | * 11 | * The Augmented Arc is an arc with a non-continuous curvature, slightly disobeying 12 | * the geometry of an arc segment. This class is used as the output of an approximation 13 | * done by @ref arc_parameterizer designed to approximate splines with continous curvature. 14 | * 15 | * The curvature is interpolated with respect to the arc length of the segment. This is 16 | * necessary for certain systems that require a parameterized spline. 17 | */ 18 | class augmented_arc2d : public arc2d { 19 | public: 20 | augmented_arc2d() : arc2d(){}; 21 | 22 | /** 23 | * Create a circular arc from a set of 3 points (start, any, and end). 24 | * 25 | * @param start The start point of the curve, in x,y metres. 26 | * @param mid Any point along the curve sitting between start and end, 27 | * in x,y metres. 28 | * @param end The end point of the curve, in x,y metres. 29 | */ 30 | augmented_arc2d(vector_t start, vector_t mid, vector_t end) : arc2d(start, mid, end) {} 31 | 32 | /** 33 | * Create a circular arc from a set of 3 points (start, any, and end), and the 34 | * start and end curvature. 35 | * 36 | * @param start The start point of the curve, in x,y metres. 37 | * @param mid Any point along the curve sitting between start and end, 38 | * in x,y metres. 39 | * @param end The end point of the curve, in x,y metres. 40 | * @param start_k The starting curvature value k in m^-1. 41 | * @param end_k The ending curvature value k in m^-1. 42 | */ 43 | augmented_arc2d(vector_t start, vector_t mid, vector_t end, double start_k, double end_k) 44 | : arc2d(start, mid, end) { 45 | set_curvature(start_k, end_k); 46 | } 47 | 48 | /** 49 | * Set the start and end curvature values for interpolation. 50 | * 51 | * @param start_k The starting curvature value k in m^-1. 52 | * @param end_k The ending curvature value k in m^-1. 53 | */ 54 | void set_curvature(double start_k, double end_k) { 55 | _curvature = start_k; 56 | _dk_ds = (end_k - start_k) / length(); 57 | _curvature_set = true; 58 | } 59 | 60 | double curvature(double s) const override { 61 | if (_curvature_set) 62 | return _curvature + s * _dk_ds; 63 | else 64 | return arc2d::curvature(s); 65 | } 66 | 67 | double dcurvature(double s) const override { return _dk_ds; } 68 | 69 | private: 70 | double _curvature, _dk_ds; 71 | bool _curvature_set = false; 72 | }; 73 | } // namespace path 74 | } // namespace pf 75 | } // namespace grpl -------------------------------------------------------------------------------- /Pathfinder/src/include/grpl/pf/path/curve.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace grpl { 6 | namespace pf { 7 | namespace path { 8 | /** 9 | * A position curve parameterized to arc length 's'. 10 | * 11 | * A curve parameterized to its arc length, allowing extremely quick 12 | * calculation of the length of the curve, primarily useful when using 13 | * curves to generate a trajectory, as the parameter is a real-world unit 14 | * relating directly to the state (distance travelled). 15 | * 16 | * @param DIM The number of dimensions of the curve, usually 2. 17 | */ 18 | template 19 | class curve { 20 | public: 21 | using vector_t = Eigen::Matrix; 22 | 23 | virtual ~curve() {} 24 | 25 | //! The number of dimensions of the curve 26 | static const size_t DIMENSIONS = DIM; 27 | 28 | /** 29 | * Calculate the position of a point on the curve, at any arc length 's'. 30 | * 31 | * @param s The distance along the arc. 32 | * @return The position of the curve at arc length 's', in m. 33 | */ 34 | virtual vector_t position(double s) const = 0; 35 | 36 | /** 37 | * Calculate the derivative of a point on the curve, at any arc length 's'. 38 | * 39 | * @param s The distance along the arc. 40 | * @return The derivative of the curve at arc length 's', unitless. 41 | */ 42 | virtual vector_t derivative(double s) const = 0; 43 | 44 | /** 45 | * Calculate the rotation of a point on the curve, at any arc length 's'. 46 | * This is the unit vector of @ref derivative(double) const. 47 | * 48 | * @param s The distance along the arc. 49 | * @return The rotation of the curve at arc length 's', unitless. 50 | */ 51 | virtual vector_t rotation(double s) { 52 | vector_t deriv = derivative(s); 53 | return deriv / deriv.norm(); // Normalize to unit vector 54 | }; 55 | 56 | /** 57 | * Calculate the curvature of the curve at any arc length 's'. 58 | * 59 | * @param s The distance along the arc 60 | * @return The curvature of the arc at arc length 's', in m^-1. 61 | */ 62 | virtual double curvature(double s) const = 0; 63 | 64 | /** 65 | * Calculate the derivative of curvature of the curve at any arc length 's'. 66 | * 67 | * @param s The distance along the arc 68 | * @return The derviative of curvature of the arc at arc length 's' (dk/ds), 69 | * in m^-2. 70 | */ 71 | virtual double dcurvature(double s) const = 0; 72 | 73 | /** 74 | * Calculate the total length of the arc. 75 | * 76 | * The arc length is what defines the range of parameter 's' used throughout this class. 77 | * 78 | * @return The total length of the curve, in metres. 79 | */ 80 | virtual double length() const = 0; 81 | }; 82 | } // namespace path 83 | } // namespace pf 84 | } // namespace grpl -------------------------------------------------------------------------------- /Pathfinder/src/include/grpl/pf/path/spline.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace grpl { 6 | namespace pf { 7 | /** 8 | * Path, spline and curve members. 9 | * 10 | * The grpl::pf::path namespace contains all classes to deal with pathing, be 11 | * it splines, curves or the utilities that accompany them. The classes in this 12 | * namespace deal only with position paths, and do not include parameterizations 13 | * to time (i.e. trajectories). 14 | */ 15 | namespace path { 16 | /** 17 | * A position curve parameterized to spline parameter 't'. 18 | * 19 | * A spline is simply a position curve parameterized to spline parameter 't' 20 | * (note that this is distinct to time), which lays in the range of 0 to 1, 21 | * representing the start and end of the spline respectively. 22 | * 23 | * @param DIM The number of dimensions of the spline, usually 2. 24 | */ 25 | template 26 | class spline { 27 | public: 28 | virtual ~spline() {} 29 | 30 | using vector_t = Eigen::Matrix; 31 | 32 | //! The number of dimensions of the spline 33 | static const size_t DIMENSIONS = DIM; 34 | 35 | /** 36 | * Calculate the position of a point on the spline, at any spline parameter 37 | * value 't' 38 | * 39 | * @param t The spline parameter, where 0 is the start and 1 is the end of the 40 | * spline. 41 | * @return The position at spline parameter 't', in m. 42 | */ 43 | virtual vector_t position(double t) = 0; 44 | 45 | /** 46 | * Calculate the derivative of a point on the spline, at any spline parameter 47 | * value 't' (the derivative of @ref position(double)) 48 | * 49 | * @param t The spline parameter, where 0 is the start and 1 is the end of the 50 | * spline. 51 | * @return The derivative at spline parameter 't', in m/t 52 | */ 53 | virtual vector_t derivative(double t) = 0; 54 | 55 | /** 56 | * Calculate the rotation of a point on the spline, at any spline parameter 57 | * value 't'. This is the unit vector of @ref derivative(double) 58 | * 59 | * @param t The spline parameter, where 0 is the start and 1 is the end of the 60 | * spline. 61 | * @return The rotation (unit derivative) at spline parameter 't', unitless. 62 | */ 63 | virtual vector_t rotation(double t) { 64 | vector_t deriv = derivative(t); 65 | return deriv / deriv.norm(); // Normalize to unit vectors 66 | }; 67 | 68 | /** 69 | * Calculate the curvature of the spline at any spline parameter value 't'. 70 | * 71 | * @param t The spline parameter, where 0 is the start and 1 is the end of the 72 | * spline. 73 | * @return The curvature at spline parameter 't', in m^-1. 74 | */ 75 | virtual double curvature(double t) = 0; 76 | }; 77 | } // namespace path 78 | } // namespace pf 79 | } // namespace grpl -------------------------------------------------------------------------------- /Pathfinder/src/include/grpl/pf/pathfinder.h: -------------------------------------------------------------------------------- 1 | // Coupled 2 | #include "coupled/causal_trajectory_generator.h" 3 | #include "coupled/chassis.h" 4 | #include "coupled/state.h" 5 | 6 | // Path 7 | #include "path/arc.h" 8 | #include "path/arc_parameterizer.h" 9 | #include "path/augmented_arc.h" 10 | #include "path/curve.h" 11 | #include "path/hermite.h" 12 | #include "path/spline.h" 13 | 14 | // Profile 15 | #include "profile/profile.h" 16 | #include "profile/trapezoidal.h" 17 | 18 | #include "constants.h" 19 | 20 | namespace grpl { 21 | /** 22 | * The Grapple Robotics Pathfinder library. 23 | */ 24 | namespace pf {} 25 | } // namespace grpl -------------------------------------------------------------------------------- /Pathfinder/src/include/grpl/pf/profile/profile.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "grpl/pf/constants.h" 5 | 6 | namespace grpl { 7 | namespace pf { 8 | /** 9 | * Motion profiles 10 | * 11 | * The grpl::pf::profile namespace contains all motion profile implementations. 12 | * Motion profiles are used to describe the shape of the position-time curve to the nth 13 | * degree. 14 | */ 15 | namespace profile { 16 | 17 | using kinematic_state = Eigen::Matrix; 18 | 19 | /** 20 | * A single state (sample point) of a motion profile. This contains the kinematics of 21 | * the profile sampled at a given time. 22 | * 23 | * The state kinematics goes up to a maximum order of @ref grpl::pf::constants::profile_kinematics_order, 24 | * although implementations of @ref grpl::pf::profile::profile may or may not fill the entire vector, 25 | * depending on their operating order (e.g. @ref grpl::pf::profile::trapezoidal will only fill up to 26 | * @ref grpl::pf::ACCELERATION, all higher orders will be an undefined value). 27 | */ 28 | struct state { 29 | //! The time point of this state, in seconds 30 | double time = 0; 31 | //! The kinematic state of the system at the time of the state, filled to the maximum order of 32 | //! the profile. All higher orders will be of an undefined value. 33 | kinematic_state kinematics = kinematic_state::Zero(); 34 | }; 35 | 36 | /** 37 | * Abstract base class for all motion profile types. 38 | * 39 | * A motion profile describes some function that forms the shape of the position-time curve 40 | * and its derivatives, allowing velocity, acceleration and jerk to be shaped in ways that may 41 | * be desirable, such as optimizing for speed, safety, or smooth operation. 42 | * 43 | * Profiles are predictive in Pathfinder, meaning each calculation does not require a full history 44 | * of the profile. This allows it to adjust to changing system conditions and limits during a 45 | * profile, increasing flexibility and efficiency. 46 | * 47 | * Since the system is predictive, it may result in a small oscillation or sudden deceleration 48 | * if a sufficient timestep is not used. For this reason, a timeslice mechanism is included in the 49 | * profile. 50 | */ 51 | class profile { 52 | public: 53 | using limits_t = Eigen::Matrix; 54 | 55 | virtual ~profile() {} 56 | 57 | /** 58 | * Get the index of the limited term (the highest order, non-infinite term). See constants in 59 | * @ref grpl::pf 60 | */ 61 | virtual const size_t limited_term() const = 0; 62 | 63 | /** 64 | * Set the goal (setpoint) of the profile. 65 | * 66 | * @param sp The goal (setpoint) of the profile, in metres. 67 | */ 68 | void set_goal(double sp) { _goal = sp; } 69 | 70 | /** 71 | * Get the goal (setpoint) of the profile. 72 | * 73 | * @return The goal (setpoint) of the profile, in metres. 74 | */ 75 | double get_goal() const { return _goal; } 76 | 77 | // TODO: Abstract timeslice? 78 | 79 | /** 80 | * Set the timeslice period. 81 | * 82 | * The timeslice is used in @ref calculate(state&, double), where a single 83 | * call will be translated into N smaller calls, where N is ceil(T_calc / T_slice), where T_calc is the 84 | * period at which @ref calculate(state&, double) is called, and T_slice is the timeslice period. 85 | * 86 | * Timeslice is used to counteract slow calls to @ref calculate(state&, double) in order to still 87 | * produce smooth curves. 88 | * 89 | * @param timeslice The timeslice period T_slice, in seconds. 90 | */ 91 | void set_timeslice(double timeslice) { _timeslice = timeslice; } 92 | 93 | /** 94 | * Get the timeslice period. 95 | * 96 | * @return The timeslice period, T_slice, in seconds. 97 | */ 98 | double get_timeslice() const { return _timeslice; } 99 | 100 | /** 101 | * Apply a constrained limit to the profile. This will limit the maximum and minimum value of 102 | * this term during the profile (e.g. maximum velocity / acceleration). 103 | * 104 | * These limits may be changed between calls to @ref calculate(state&, double), but if the maximum 105 | * or minimum is lower in magnitude, it may cause sudden decelerations. 106 | * 107 | * @param term The term to apply the limit to. See constants in @ref grpl::pf 108 | * @param min The minimum value of the term, in the units of the term 109 | * @param max The maximum value of the term, in the units of the term 110 | */ 111 | void apply_limit(int term, double min, double max) { 112 | _limits(0, term) = min; 113 | _limits(1, term) = max; 114 | } 115 | 116 | /** 117 | * Obtain the currently set limits of the profile 118 | * 119 | * @return The limits matrix. Row 0 is the minimum values, row 1 is the maximum. The column indices 120 | * match those of the terms (see constants in @ref grpl::pf) 121 | */ 122 | limits_t get_limits() { return _limits; } 123 | 124 | /** 125 | * Calculate a single state of the motion profile, in a predictive manner. 126 | * 127 | * This method function uses the last (current) state of the system in order to generate the following 128 | * state at the time given. It does this in a predictive manner, meaning it predicts when it must 129 | * begin slowing down in order to not overshoot the setpoint. This means the profile calculation does 130 | * not require a full history of the profile, allowing it to adjust to changing system conditions and 131 | * limits. 132 | */ 133 | virtual state calculate(state &last, double time) = 0; 134 | 135 | protected: 136 | double _goal, _timeslice = 0.001; 137 | limits_t _limits = limits_t::Zero(); 138 | }; 139 | 140 | } // namespace profile 141 | } // namespace pf 142 | } // namespace grpl -------------------------------------------------------------------------------- /Pathfinder/src/include/grpl/pf/profile/trapezoidal.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "profile.h" 4 | 5 | #include 6 | 7 | namespace grpl { 8 | namespace pf { 9 | namespace profile { 10 | /** 11 | * Implementation of a trapezoidal (limited acceleration) motion profile. 12 | * 13 | * A trapezoidal motion profile is a motion profile limited by acceleration (therefore, infinite jerk). 14 | * The profile can be described by three distinct sections: ramp-up, hold and ramp-down. 15 | * 16 | * During ramp-up, the system is accelerating towards its max velocity. 17 | * 18 | * During hold, the system is not accelerating, and holds its max velocity. Depending on the setpoint, 19 | * the system may not have time to reach hold before it must ramp-down, resulting in a trianglular 20 | * velocity profile. 21 | * 22 | * During ramp-down, the system is decelerating towards 0. 23 | * 24 | * See @ref grpl::pf::profile::profile 25 | */ 26 | class trapezoidal : public profile { 27 | public: 28 | const size_t limited_term() const override { return ACCELERATION; } 29 | 30 | state calculate(state &last, double time) override { 31 | double dt = time - last.time; 32 | double timestep = dt; 33 | int slice_count = 1; 34 | 35 | if (this->_timeslice > 0) { 36 | double slice_count_d = static_cast(dt / this->_timeslice); 37 | 38 | slice_count = static_cast(slice_count_d); 39 | if (slice_count_d - slice_count > 0.9) slice_count++; 40 | if (slice_count < 1) slice_count++; 41 | 42 | timestep = this->_timeslice; 43 | } 44 | 45 | double vel_min = this->_limits(0, 1); 46 | double vel_max = this->_limits(1, 1); 47 | double accel_min = this->_limits(0, 2); 48 | double accel_max = this->_limits(1, 2); 49 | 50 | state cur = last; 51 | 52 | double start_time = cur.time; 53 | 54 | for (int i = 1; i <= slice_count; i++) { 55 | double t = start_time + (i * timestep); 56 | if (t > time) t = time; 57 | dt = t - cur.time; 58 | 59 | auto &kin = cur.kinematics; 60 | 61 | double error = kin[POSITION] - this->_goal; 62 | double accel = (error < 0 ? accel_max : accel_min); 63 | 64 | // TODO: Find point at which we reach v_max and if it's less than dt, split 65 | // this slice into half. 66 | double v_projected = kin[VELOCITY] + accel * dt; 67 | v_projected = v_projected > vel_max ? vel_max : v_projected < vel_min ? vel_min : v_projected; 68 | 69 | double decel_time = v_projected / -accel_min; 70 | double decel_dist = v_projected * decel_time + 0.5 * accel_min * decel_time * decel_time; 71 | double decel_error = kin[POSITION] + decel_dist - this->_goal; 72 | 73 | // TODO: make this better 74 | // If we decelerate now, do we cross the zero of the error function? 75 | bool decel_cross_error_zeros = (error > 0 && decel_error < 0) || (error < 0 && decel_error > 0); 76 | // Are we not currently decelerating? 77 | bool decel_not_in_progress = (error < 0 && kin[VELOCITY] > 0) || (error > 0 && kin[VELOCITY] < 0); 78 | 79 | if (decel_cross_error_zeros && decel_not_in_progress) 80 | accel = (accel < 0 ? accel_max : accel_min); 81 | else if (fabs(kin[1] - vel_max) < constants::default_acceptable_error) 82 | accel = 0; 83 | else if (fabs(error) < constants::default_acceptable_error) 84 | accel = 0; 85 | 86 | double vel = kin[VELOCITY] + (accel * dt); 87 | kin[POSITION] = kin[POSITION] + (kin[VELOCITY] * dt) + (0.5 * accel * dt * dt); 88 | kin[VELOCITY] = vel > vel_max ? vel_max : vel < vel_min ? vel_min : vel; 89 | kin[ACCELERATION] = accel; 90 | cur.time = t; 91 | } 92 | return cur; 93 | } 94 | }; 95 | } // namespace profile 96 | } // namespace pf 97 | } // namespace grpl -------------------------------------------------------------------------------- /Pathfinder/src/include/grpl/pf/transmission/dc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace grpl { 4 | namespace pf { 5 | /** 6 | * Motor transmissions. 7 | * 8 | * The grpl::pf::transmission namespace contains implementations of transmissions, which convert 9 | * control signals to motion (e.g. motors). 10 | */ 11 | namespace transmission { 12 | 13 | /** 14 | * Base class of a DC electric transmission, usually @ref grpl::pf::transmission::dc_motor. 15 | */ 16 | class dc_transmission { 17 | public: 18 | // TODO: Need a better naming scheme 19 | virtual ~dc_transmission(){}; 20 | 21 | /** 22 | * Calculate the free (no load) speed of the transmission at an applied voltage. 23 | * 24 | * @param voltage The voltage applied to the transmission, in Volts 25 | * @return The free speed of the transmission, in rad/s 26 | */ 27 | virtual double get_free_speed(double voltage) const = 0; 28 | 29 | /** 30 | * Calculate the current draw of the transmission at an applied voltage and speed. 31 | * 32 | * @param voltage The voltage applied to the transmission, in Volts 33 | * @param speed The current speed of the transmission, in rad/s 34 | * @return The current drawn by the transmission, in Amps 35 | */ 36 | virtual double get_current(double voltage, double speed) const = 0; 37 | 38 | /** 39 | * Calculate the torque applied by the transmission at a given current draw. 40 | * 41 | * @param current The current drawn by the transmission, calculated in 42 | * @ref get_current(double, double) const, in Amps 43 | * @return The torque applied by the transmission, in Nm 44 | */ 45 | virtual double get_torque(double current) const = 0; 46 | 47 | /** 48 | * Calculate the component voltage applied to the transmission in order to obtain 49 | * a free speed. 50 | * 51 | * To obtain the full applied voltage, sum @ref get_free_voltage(double) const and 52 | * @ref get_current_voltage(double) const. 53 | * 54 | * @param speed The speed of the transmission, in rad/s 55 | * @return The free voltage component of the transmission, in Volts 56 | */ 57 | virtual double get_free_voltage(double speed) const = 0; 58 | 59 | /** 60 | * Calculate the component voltage applied to the transmission in order to draw 61 | * a current. 62 | * 63 | * Current is usually provided by @ref get_torque_current(double) const 64 | * 65 | * To obtain the full applied voltage, sum @ref get_free_voltage(double) const and 66 | * @ref get_current_voltage(double) const. 67 | * 68 | * @param current The current drawn by the transmission, in Amps 69 | * @return The current voltage component of the transmission, in Volts 70 | */ 71 | virtual double get_current_voltage(double current) const = 0; 72 | 73 | /** 74 | * Calculate the current draw of the transmission given a torque. 75 | * 76 | * @param torque The torque applied by the transmission, in Nm. 77 | * @return The current draw required to apply the torque, in Amps. 78 | */ 79 | virtual double get_torque_current(double torque) const = 0; 80 | 81 | /** 82 | * Get the nominal, operating voltage of the transmission. 83 | * 84 | * @return The nominal, operating voltage of the transmission, in Volts 85 | */ 86 | virtual double nominal_voltage() const = 0; 87 | }; 88 | 89 | /** 90 | * Mathematical Model of a DC Brushed Motor 91 | * 92 | * Basic DC Motor Model, dervied from the ideal resistive motor model with Back EMF 93 | * (+ ---[ R ]---( V_w )--- -), where R = V / I_stall (as V_w = 0 at stall), and 94 | * V_w = kv*w. 95 | */ 96 | class dc_motor : public dc_transmission { 97 | public: 98 | /** 99 | * Construct a DC Brushed Motor Model. 100 | * 101 | * @param v_nom The nominal operating voltage of the motor, in Volts 102 | * @param free_speed The maximum free (no load) speed of the motor at v_nom, in rad/s 103 | * @param free_current The current drawn when the motor is running at free_speed 104 | * with no load 105 | * @param stall_current The current drawn when v_nom is applied with a locked rotor (stalled) 106 | * @param stall_torque The torque applied by the motor at v_nom with a locked rotor (stalled) 107 | */ 108 | dc_motor(double v_nom, double free_speed, double free_current, double stall_current, 109 | double stall_torque) 110 | : _v_nom(v_nom), 111 | _free_speed(free_speed), 112 | _free_current(free_current), 113 | _stall_current(stall_current), 114 | _stall_torque(stall_torque) {} 115 | 116 | /** 117 | * Calculate the internal resistance of the motor windings 118 | * 119 | * @return the internal resistance of the motor, in Ohms 120 | */ 121 | inline double internal_resistance() const { return _v_nom / _stall_current; } 122 | 123 | /** 124 | * Calculate the speed-voltage coefficient of the motor (kv in V = kv*w) 125 | * 126 | * @return The speed-voltage coefficient of the motor, in Vs/rad 127 | */ 128 | inline double kv() const { return (_v_nom - _free_current * _v_nom / _stall_current) / _free_speed; } 129 | 130 | /** 131 | * Calculate the torque-current coefficient of the motor (kt in I = kt*t) 132 | * 133 | * @return The torque-current coefficient of the motor, in A/(Nm) 134 | */ 135 | inline double kt() const { return _stall_current / _stall_torque; } 136 | 137 | double nominal_voltage() const override { return _v_nom; } 138 | 139 | double get_current(double voltage, double speed) const override { 140 | // V_w = kv * w 141 | double vel_voltage = kv() * speed; 142 | // V = IR + kv*w = IR + V_vel 143 | // I = (V - V_vel) / R 144 | return (voltage - vel_voltage) / internal_resistance(); 145 | } 146 | 147 | double get_torque(double current) const override { 148 | // I = kt * t, t = I / kt 149 | return current / kt(); 150 | } 151 | 152 | double get_free_speed(double voltage) const override { 153 | // V = kv * w, w = V / kv 154 | return voltage / kv(); 155 | } 156 | 157 | double get_free_voltage(double speed) const override { 158 | // V_w = kv * w 159 | return kv() * speed; 160 | } 161 | 162 | double get_current_voltage(double current) const override { 163 | // V_I = IR 164 | return current * internal_resistance(); 165 | } 166 | 167 | double get_torque_current(double torque) const override { 168 | // I = kt * t 169 | return kt() * torque; 170 | } 171 | 172 | private: 173 | double _v_nom = 12; 174 | double _free_speed; 175 | double _free_current; 176 | double _stall_current; 177 | double _stall_torque; 178 | }; 179 | } // namespace transmission 180 | } // namespace pf 181 | } // namespace grpl -------------------------------------------------------------------------------- /Pathfinder/src/test/coupled/cdt.cpp: -------------------------------------------------------------------------------- 1 | #include "grpl/pf.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace grpl::pf; 10 | 11 | template 12 | void echo(std::ofstream &out, ST state, int id) { 13 | out << state.time << "," << state.config.x() << "," << state.config.y() << "," 14 | << state.config[2] << "," << state.kinematics[POSITION] << "," << state.kinematics[VELOCITY] 15 | << "," << state.kinematics[ACCELERATION] << "," << state.curvature << "," << id << "," 16 | << "" 17 | << "," << std::endl; 18 | } 19 | 20 | void echo_limits(std::ofstream &out, double time, profile::trapezoidal::limits_t lim) { 21 | auto vel = lim.col(1); 22 | auto acc = lim.col(2); 23 | 24 | out << time << ",,,,," << vel[0] << "," << acc[0] << ",,-1,," << std::endl; 25 | out << time << ",,,,," << vel[1] << "," << acc[1] << ",,-1,," << std::endl; 26 | } 27 | 28 | void echo_simulation(std::ofstream &out, double time, Eigen::Vector3d config) { 29 | out << time << "," << config.x() << "," << config.y() << "," << config[2] << ",,,,,3,," << std::endl; 30 | } 31 | 32 | template 33 | void echo_wheel(std::ofstream &out, ST state, int id) { 34 | out << state.time << "," << state.position.x() << "," << state.position.y() << "," 35 | << "" 36 | << "," 37 | << "" 38 | << "," << state.kinematics[VELOCITY] << "," << state.kinematics[ACCELERATION] << "," 39 | << "" 40 | << "," << id << "," << state.voltage << "," << state.current << std::endl; 41 | } 42 | 43 | TEST(CDT, basic) { 44 | using hermite_t = path::hermite_quintic; 45 | using profile_t = profile::trapezoidal; 46 | using state_t = coupled::state; 47 | using wheel_state_t = coupled::wheel_state; 48 | 49 | std::vector curves; 50 | std::array wps{hermite_t::waypoint{{0, 0}, {5, 0}, {0, 0}}, 51 | hermite_t::waypoint{{4, 4}, {0, 5}, {0, 0}}}; 52 | 53 | std::vector hermites; 54 | path::hermite_factory::generate(wps.begin(), wps.end(), std::back_inserter(hermites), 55 | hermites.max_size()); 56 | 57 | path::arc_parameterizer param; 58 | param.configure(0.01, 0.01); 59 | param.parameterize(hermites.begin(), hermites.end(), std::back_inserter(curves), curves.max_size()); 60 | 61 | profile_t profile; 62 | // profile.set_timeslice(0.00001); 63 | double G = 12.75; 64 | 65 | transmission::dc_motor dualCIM{12.0, 5330 * 2.0 * constants::PI / 60.0 / G, 2 * 2.7, 2 * 131.0, 66 | 2 * 2.41 * G}; 67 | // coupled_t model{dualCIM, dualCIM, 0.0762, 0.5, 25.0, 10.0}; 68 | coupled::chassis chassis{dualCIM, dualCIM, 0.0762, 0.5, 25.0}; 69 | coupled::causal_trajectory_generator gen; 70 | 71 | state_t state; 72 | 73 | std::ofstream pathfile("cdt.csv"); 74 | pathfile << "t,x,y,heading,distance,velocity,acceleration,curvature,path,voltage,current\n"; 75 | 76 | coupled::configuration_state centre{0, 0, 0}; 77 | 78 | for (double t = 0; !state.finished && t < 5; t += 0.01) { 79 | state = gen.generate(chassis, curves.begin(), curves.end(), profile, state, t); 80 | echo(pathfile, state, 0); 81 | 82 | echo_limits(pathfile, t, profile.get_limits()); 83 | 84 | std::pair split = chassis.split(state); 85 | echo_wheel(pathfile, split.first, 1); 86 | echo_wheel(pathfile, split.second, 2); 87 | 88 | double w = (split.second.kinematics[VELOCITY] - 89 | split.first.kinematics[VELOCITY]); // track radius = 0.5, track diam = 1 90 | double v = (split.second.kinematics[VELOCITY] + split.first.kinematics[VELOCITY]) / 2.0; 91 | 92 | // TODO: Checks 93 | 94 | centre[2] += w * 0.01; 95 | centre[0] += v * cos(centre[2]) * 0.01; 96 | centre[1] += v * sin(centre[2]) * 0.01; 97 | 98 | echo_simulation(pathfile, t, centre); 99 | } 100 | } -------------------------------------------------------------------------------- /Pathfinder/src/test/path/test_arc.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "grpl/pf/path/arc.h" 3 | 4 | #include 5 | #include 6 | 7 | using namespace grpl::pf; 8 | using namespace grpl::pf::path; 9 | 10 | TEST(Arc, ArcTest) { 11 | arc2d::vector_t start = {0, 0}, mid = {4, 2}, end = {5, 5}; 12 | 13 | arc2d arc(start, mid, end); 14 | 15 | std::ofstream outfile("arc.csv"); 16 | outfile << "t,x,y,vx,vy,curvature\n"; 17 | 18 | arc2d::vector_t position = start; 19 | 20 | // Check start and end points 21 | auto s0 = arc.position(0); 22 | auto s1 = arc.position(arc.length()); 23 | ASSERT_LT((s0 - start).norm(), 0.02) << s0; 24 | ASSERT_LT((s1 - end).norm(), 0.02) << s1; 25 | 26 | double curvature = arc.curvature(0); 27 | 28 | for (double s = 0; s < arc.length(); s += 0.01) { 29 | auto pos = arc.position(s); 30 | auto deriv = arc.derivative(s); 31 | 32 | position += deriv * 0.01; 33 | 34 | // Simulated position (from derivative) should 35 | // match the actual position calculated. Confirms 36 | // position and derivative are correct. 37 | ASSERT_LT((position - pos).norm(), 0.02); 38 | 39 | // Curvature of an arc (1/radius) is constant. 40 | ASSERT_DOUBLE_EQ(curvature, arc.curvature(s)); 41 | 42 | outfile << s << "," << pos[0] << "," << pos[1] << "," << deriv[0] << "," << deriv[1] << "," 43 | << arc.curvature(s) << std::endl; 44 | } 45 | } -------------------------------------------------------------------------------- /Pathfinder/src/test/path/test_arc_param.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "grpl/pf/path/arc_parameterizer.h" 3 | #include "grpl/pf/path/hermite.h" 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace grpl::pf; 13 | using namespace grpl::pf::path; 14 | 15 | TEST(ArcParam, Hermite) { 16 | using hermite_t = hermite_quintic; 17 | 18 | hermite_t::waypoint start{{2, 2}, {5, 0}, {0, 0}}, end{{5, 5}, {5, 5}, {0, 0}}; 19 | hermite_t hermite(start, end); 20 | 21 | std::vector curves; 22 | 23 | arc_parameterizer param; 24 | param.configure(0.1, 0.1); 25 | 26 | size_t numcurves_required = param.curve_count(hermite); 27 | size_t numcurves = param.parameterize(hermite, std::back_inserter(curves), curves.max_size()); 28 | 29 | ASSERT_EQ(numcurves, numcurves_required); 30 | ASSERT_FALSE(param.has_overrun()); 31 | 32 | // Check that curvature and tangent angle is continuous across curves 33 | for (size_t c = 1; c < numcurves; c++) { 34 | auto it = curves[c]; 35 | auto last = curves[c - 1]; 36 | ASSERT_DOUBLE_EQ(it.curvature(0), last.curvature(last.length())); 37 | } 38 | } 39 | 40 | TEST(ArcParam, Overrun) { 41 | using hermite_t = hermite_quintic; 42 | 43 | hermite_t::waypoint start{{2, 2}, {5, 0}, {0, 0}}, end{{5, 5}, {5, 5}, {0, 0}}; 44 | hermite_t hermite(start, end); 45 | 46 | std::array curves; 47 | 48 | arc_parameterizer param; 49 | param.configure(0.1, 0.1); 50 | 51 | size_t numcurves_required = param.curve_count(hermite); 52 | size_t numcurves = param.parameterize(hermite, curves.begin(), curves.max_size()); 53 | 54 | ASSERT_GT(numcurves_required, numcurves); 55 | ASSERT_TRUE(param.has_overrun()); 56 | } 57 | 58 | TEST(ArcParam, Multispline) { 59 | using hermite_t = hermite_quintic; 60 | 61 | std::array wps{ 62 | hermite_t::waypoint{{2, 2}, {5, 0}, {0, 0}}, hermite_t::waypoint{{3, 5}, {0, 5}, {0, 0}}, 63 | hermite_t::waypoint{{5, 7}, {2, 2}, {0, 0}}, hermite_t::waypoint{{7, 9}, {5, -5}, {0, 0}}}; 64 | 65 | std::vector hermites; 66 | hermite_factory::generate(wps.begin(), wps.end(), std::back_inserter(hermites), 67 | hermites.max_size()); 68 | 69 | std::vector curves; 70 | 71 | arc_parameterizer param; 72 | param.configure(0.5, 0.5); 73 | 74 | size_t numcurves_required = param.curve_count(hermites.begin(), hermites.end()); 75 | size_t numcurves = 76 | param.parameterize(hermites.begin(), hermites.end(), std::back_inserter(curves), curves.max_size()); 77 | 78 | ASSERT_EQ(numcurves, numcurves_required); 79 | ASSERT_FALSE(param.has_overrun()); 80 | 81 | std::ofstream outfile("arcparam.csv"); 82 | outfile << "curveid,s,x,y,curvature,angle\n"; 83 | 84 | double ps = 0; 85 | hermite_t::vector_t last_pos{2, 2}; 86 | for (auto it = hermites.begin(); it != hermites.end(); it++) { 87 | for (double t = 0; t < 1; t += 0.001) { 88 | auto pos = it->position(t); 89 | auto rot = it->rotation(t); 90 | ps += (pos - last_pos).norm(); 91 | outfile << -100.0 << "," << ps << "," << pos[0] << "," << pos[1] << "," << it->curvature(t) << "," 92 | << (atan2(rot.y(), rot.x()) * 180 / constants::PI) << std::endl; 93 | last_pos = pos; 94 | } 95 | } 96 | 97 | size_t curve = 0; 98 | double s = 0, si = 0; 99 | while (curve < numcurves) { 100 | auto c = curves[curve]; 101 | auto pos = c.position(si); 102 | auto curv = c.curvature(si); 103 | auto rot = c.rotation(si); 104 | auto angle = atan2(rot.y(), rot.x()); 105 | 106 | ASSERT_FALSE(std::isnan(c.length())); 107 | 108 | outfile << curve << "," << s << "," << pos[0] << "," << pos[1] << "," << curv << "," 109 | << (angle * 180 / constants::PI) << std::endl; 110 | 111 | si += 0.001; 112 | s += 0.001; 113 | if (si > c.length()) { 114 | curve++; 115 | si = 0; 116 | } 117 | } 118 | } -------------------------------------------------------------------------------- /Pathfinder/src/test/path/test_hermite.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "grpl/pf/path/hermite.h" 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace grpl::pf; 10 | using namespace grpl::pf::path; 11 | 12 | TEST(Hermite, Cubic) { 13 | using hermite_t = hermite_cubic; 14 | 15 | hermite_t::waypoint start{{2, 2}, {5, 0}}, end{{5, 5}, {0, 5}}; 16 | 17 | hermite_t hermite(start, end); 18 | 19 | std::ofstream outfile("hermite_cubic.csv"); 20 | outfile << "t,x,y,curvature\n"; 21 | 22 | hermite_t::vector_t position = start.position; 23 | 24 | // Check start and end points 25 | auto t0 = hermite.position(0); 26 | auto t1 = hermite.position(1); 27 | ASSERT_LT((t0 - start.position).norm(), 0.01) << t0; 28 | ASSERT_LT((t1 - end.position).norm(), 0.01) << t1; 29 | // Check start and end tangents 30 | auto t0_d = hermite.derivative(0); 31 | auto t1_d = hermite.derivative(1); 32 | ASSERT_LT((t0_d - start.tangent).norm(), 0.01) << t0; 33 | ASSERT_LT((t1_d - end.tangent).norm(), 0.01) << t1; 34 | 35 | for (double t = 0; t <= 1; t += 0.001) { 36 | auto pt = hermite.position(t); 37 | auto deriv = hermite.derivative(t); 38 | auto curv = hermite.curvature(t); 39 | 40 | position += deriv * 0.001; 41 | 42 | // Assert simulations match actual readings 43 | ASSERT_LT((position - pt).norm(), 0.02); 44 | 45 | ASSERT_GT(curv, 0); 46 | 47 | outfile << t << "," << pt[0] << "," << pt[1] << "," << curv << std::endl; 48 | } 49 | } 50 | 51 | TEST(Hermite, Quintic) { 52 | using hermite_t = hermite_quintic; 53 | 54 | hermite_t::waypoint start{{2, 2}, {5, 0}, {0, 0}}, end{{5, 5}, {0, 5}, {0, 0}}; 55 | 56 | hermite_t hermite(start, end); 57 | 58 | std::ofstream outfile("hermite_quintic.csv"); 59 | outfile << "t,x,y,curvature\n"; 60 | 61 | hermite_t::vector_t position = start.position, position_second = start.position; 62 | hermite_t::vector_t derivative = start.tangent; 63 | 64 | // Check start and end points 65 | auto t0 = hermite.position(0); 66 | auto t1 = hermite.position(1); 67 | ASSERT_LT((t0 - start.position).norm(), 0.01) << t0; 68 | ASSERT_LT((t1 - end.position).norm(), 0.01) << t1; 69 | // Check start and end tangents 70 | auto t0_d = hermite.derivative(0); 71 | auto t1_d = hermite.derivative(1); 72 | ASSERT_LT((t0_d - start.tangent).norm(), 0.01) << t0; 73 | ASSERT_LT((t1_d - end.tangent).norm(), 0.01) << t1; 74 | // Check start and end second derivatives 75 | auto t0_sd = hermite.derivative2(0); 76 | auto t1_sd = hermite.derivative2(1); 77 | ASSERT_LT((t0_sd - start.dtangent).norm(), 0.01) << t0; 78 | ASSERT_LT((t1_sd - end.dtangent).norm(), 0.01) << t1; 79 | 80 | for (double t = 0; t <= 1; t += 0.001) { 81 | auto pt = hermite.position(t); 82 | auto deriv = hermite.derivative(t); 83 | auto deriv2nd = hermite.derivative2(t); 84 | auto curv = hermite.curvature(t); 85 | 86 | derivative += deriv2nd * 0.001; 87 | 88 | position += deriv * 0.001; 89 | position_second += derivative * 0.001; 90 | 91 | // Assert simulations match actual readings 92 | ASSERT_LT((position - pt).norm(), 0.02); 93 | ASSERT_LT((derivative - deriv).norm(), 0.02); 94 | ASSERT_LT((position_second - pt).norm(), 0.02); 95 | 96 | ASSERT_GE(curv, 0); 97 | 98 | outfile << t << "," << pt[0] << "," << pt[1] << "," << curv << std::endl; 99 | } 100 | } 101 | 102 | TEST(Hermite, NegativeCurvature) { 103 | using hermite_t = hermite_cubic; 104 | 105 | hermite_t::waypoint start{{2, 2}, {5, 0}}, end{{5, -1}, {0, -5}}; 106 | 107 | hermite_t hermite(start, end); 108 | 109 | std::ofstream outfile("hermite_cubic_negcurv.csv"); 110 | outfile << "t,x,y,curvature\n"; 111 | 112 | hermite_t::vector_t position = start.position; 113 | 114 | // Check start and end points 115 | auto t0 = hermite.position(0); 116 | auto t1 = hermite.position(1); 117 | ASSERT_LT((t0 - start.position).norm(), 0.01) << t0; 118 | ASSERT_LT((t1 - end.position).norm(), 0.01) << t1; 119 | // Check start and end tangents 120 | auto t0_d = hermite.derivative(0); 121 | auto t1_d = hermite.derivative(1); 122 | ASSERT_LT((t0_d - start.tangent).norm(), 0.01) << t0; 123 | ASSERT_LT((t1_d - end.tangent).norm(), 0.01) << t1; 124 | 125 | for (double t = 0; t <= 1; t += 0.001) { 126 | auto pt = hermite.position(t); 127 | auto deriv = hermite.derivative(t); 128 | auto curv = hermite.curvature(t); 129 | 130 | position += deriv * 0.001; 131 | 132 | // Assert simulations match actual readings 133 | ASSERT_LT((position - pt).norm(), 0.02); 134 | 135 | ASSERT_LT(curv, 0); 136 | 137 | outfile << t << "," << pt[0] << "," << pt[1] << "," << curv << std::endl; 138 | } 139 | } 140 | 141 | template 142 | void multitest(std::string name) { 143 | using waypoint_t = typename hermite_t::waypoint; 144 | 145 | std::vector wps; 146 | wps.push_back(waypoint_t{{2, 2}, {5, 0}}); 147 | wps.push_back(waypoint_t{{3, 5}, {0, 5}}); 148 | wps.push_back(waypoint_t{{5, 7}, {2, 2}}); 149 | wps.push_back(waypoint_t{{7, 9}, {5, -5}}); 150 | 151 | std::vector hermites; 152 | 153 | size_t num_hermites = hermite_factory::generate( 154 | wps.begin(), wps.end(), std::back_inserter(hermites), hermites.max_size()); 155 | 156 | ASSERT_EQ(num_hermites, wps.size() - 1); 157 | 158 | std::ofstream outfile("hermite_" + name + ".csv"); 159 | outfile << "t,x,y,curvature\n"; 160 | 161 | for (size_t i = 0; i < num_hermites; i++) { 162 | for (double t = 0; t <= 1; t += 0.001) { 163 | auto pt = hermites[i].position(t); 164 | outfile << (t + i) << "," << pt[0] << "," << pt[1] << "," << hermites[i].curvature(t) << std::endl; 165 | } 166 | } 167 | } 168 | 169 | TEST(Hermite, MultiCubic) { 170 | multitest("multicubic"); 171 | } 172 | 173 | TEST(Hermite, MultiQuintic) { 174 | multitest("multiquintic"); 175 | } 176 | 177 | TEST(Hermite, MultiZeroWP) { 178 | using hermite_t = hermite_quintic; 179 | std::vector wps; 180 | wps.push_back(hermite_t::waypoint{}); 181 | 182 | std::array hermites; 183 | 184 | size_t num_hermites = 185 | hermite_factory::generate(wps.begin(), wps.end(), hermites.begin(), hermites.max_size()); 186 | 187 | ASSERT_EQ(num_hermites, 0); 188 | } -------------------------------------------------------------------------------- /Pathfinder/src/test/profile/test_trapezoidal.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "grpl/pf/profile/trapezoidal.h" 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace grpl::pf; 10 | using namespace grpl::pf::profile; 11 | using namespace std; 12 | 13 | TEST(Profile, Trapezoidal) { 14 | double sim_position = 0, sim_velocity = 0; 15 | double dt = 0.001; 16 | 17 | trapezoidal pr; 18 | pr.apply_limit(VELOCITY, -3, 3); // Velocity Limit = -3 to 3m/s 19 | pr.apply_limit(ACCELERATION, -3, 4); // Acceleration limit = -3 to 4m/s 20 | pr.set_goal(5); // Goal = 5m 21 | pr.set_timeslice(0); // No Timeslice 22 | 23 | state st; 24 | kinematic_state kin; 25 | 26 | ofstream outfile("profile_trap.csv"); 27 | ofstream outfile_sim("profile_trap_simulated.csv"); 28 | outfile << "time,dist,vel,acc\n"; 29 | outfile_sim << "time,dist,vel\n"; 30 | 31 | for (double t = 0; t < 7; t += dt) { 32 | st = pr.calculate(st, t); 33 | kin = st.kinematics; 34 | sim_velocity += kin[ACCELERATION] * dt; 35 | sim_position += sim_velocity * dt; 36 | 37 | // Assert Limits 38 | ASSERT_LE(abs(kin[1]), 3) << "Time: " << t; 39 | ASSERT_LE(abs(kin[2]), 4) << "Time: " << t; 40 | 41 | // Check simulation matches theoretical 42 | // TODO: the velocity error builds up here (residual from end of path) 43 | // ASSERT_LE(abs(kin[1] - sim_velocity), 0.01) << "Time: " << t; 44 | // ASSERT_LE(abs(kin[0] - sim_position), 0.01) << "Time: " << t; 45 | 46 | outfile_sim << st.time << "," << sim_position << "," << sim_velocity << "\n"; 47 | outfile << st.time << "," << kin[POSITION] << "," << kin[VELOCITY] << "," << kin[ACCELERATION] << "\n"; 48 | } 49 | 50 | // Check setpoint has been reached at end of profile 51 | ASSERT_NEAR(kin[0], 5, 0.001); 52 | } -------------------------------------------------------------------------------- /Pathfinder/src/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | int main(int argc, char **argv) { 7 | #ifdef EIGEN_RUNTIME_NO_MALLOC 8 | Eigen::internal::set_is_malloc_allowed(false); 9 | std::cout << "EIGEN RUNTIME MALLOC ASSERTS ENABLED!" << std::endl; 10 | #else 11 | std::cout << "WARN!!! EIGEN RUNTIME MALLOCS ARE NOT ASSERTED!" << std::endl; 12 | #endif 13 | ::testing::InitGoogleTest(&argc, argv); 14 | return RUN_ALL_TESTS(); 15 | } -------------------------------------------------------------------------------- /Pathfinder/src/test/test_util.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace testutil { 8 | template 9 | class pose_simulation { 10 | public: 11 | using v_t = Eigen::Matrix; 12 | void write(v_t value, size_t order, double dt) { 13 | for (size_t i = 0; i < order; i++) _oldPose[i] = _pose[i]; 14 | 15 | _pose[order] = value; 16 | for (size_t divLow = 1; divLow < order; divLow++) { 17 | size_t o = order - divLow; 18 | _pose[o] += _pose[o + 1] * dt; 19 | } 20 | 21 | for (size_t divHi = 1; divHi < (ORDER - order); divHi++) { 22 | size_t o = order + divHi; 23 | _pose[o] = (_pose[o - 1] - _oldPose[o - 1]) / dt; 24 | } 25 | } 26 | 27 | void zero() { 28 | for (size_t i = 0; i < ORDER; i++) _oldPose[i] = _pose[i] = {0, 0}; 29 | } 30 | 31 | v_t get(size_t order) { return _pose[order]; } 32 | 33 | private: 34 | v_t _pose[ORDER], _oldPose[ORDER]; 35 | }; 36 | } // namespace testutil -------------------------------------------------------------------------------- /Pathfinder/src/testplot/arc.plt: -------------------------------------------------------------------------------- 1 | set term png size 1800,600 enhanced 2 | set output "arc.png" 3 | set multiplot layout 1, 3 title "Arc" 4 | 5 | set xrange [-1:6] 6 | set yrange [0:7] 7 | 8 | set title "Path" 9 | plot "arc.csv" using 2:3:1 w l lc palette lw 2 10 | 11 | set title "Derivatives" 12 | plot "arc.csv" using 2:3:4:5 w vectors head filled lt 2 13 | 14 | set yrange [0:1] 15 | set autoscale x 16 | set autoscale ymax 17 | 18 | set title "Curvature" 19 | plot "arc.csv" using 1:6 w l lw 2, '' using 1:(1.0/$6) w l lw 2 title "Radius" 20 | 21 | unset multiplot -------------------------------------------------------------------------------- /Pathfinder/src/testplot/arc_param.plt: -------------------------------------------------------------------------------- 1 | set term png size 1800,600 enhanced 2 | set output "arc_param.png" 3 | set multiplot layout 1, 3 title "Arc Parameterization" 4 | 5 | set autoscale 6 | 7 | set title "Path" 8 | plot "arcparam.csv" using 3:4:1 w p ps 0.2 pt 7 lc palette lw 2 9 | 10 | set autoscale x 11 | set autoscale y 12 | 13 | set title "Curvature" 14 | plot "arcparam.csv" using 2:5:1 w l lc palette lw 2 15 | 16 | set title "Angle" 17 | plot "arcparam.csv" using 2:6:1 w l lc palette lw 2 18 | 19 | unset multiplot -------------------------------------------------------------------------------- /Pathfinder/src/testplot/cdt_model.plt: -------------------------------------------------------------------------------- 1 | set term png size 1800,1800 enhanced 2 | set output "cdt.png" 3 | set multiplot layout 3, 3 title "CDT Model (Centre)" 4 | 5 | set xrange [-2:6] 6 | set yrange [-2:6] 7 | 8 | set title "Position" 9 | plot "cdt.csv" using 2:3:9 w p pt 7 lc palette 10 | 11 | set xrange [*:*] 12 | set yrange [*:*] 13 | 14 | set title "Heading (Deg)" 15 | plot "cdt.csv" using 1:($4*180/3.1415):9 w p pt 7 lc palette 16 | 17 | set title "Distance" 18 | plot "cdt.csv" using 1:5:9 w p pt 7 lc palette 19 | 20 | set title "Velocity" 21 | plot "cdt.csv" using 1:6:9 w p pt 7 lc palette 22 | 23 | set title "Acceleration" 24 | plot "cdt.csv" using 1:7:9 w p pt 7 lc palette ps 0.5 25 | 26 | set title "Curvature" 27 | plot "cdt.csv" using 1:8:9 w p pt 7 lc palette 28 | 29 | set title "Voltage" 30 | plot "cdt.csv" using 1:10:9 w p pt 7 lc palette 31 | 32 | set title "Current" 33 | plot "cdt.csv" using 1:11:9 w p pt 7 lc palette -------------------------------------------------------------------------------- /Pathfinder/src/testplot/hermite.plt: -------------------------------------------------------------------------------- 1 | set term png size 1200,600 enhanced 2 | 3 | hermitetype1 = "cubic" 4 | hermitetype2 = "quintic" 5 | hermitefile = "hermite" 6 | load "hermite_base.gp" 7 | 8 | hermitetype1 = "cubic" 9 | hermitetype2 = "cubic_negcurv" 10 | hermitefile = "hermite_curv" 11 | load "hermite_base.gp" 12 | 13 | hermitetype1 = "multicubic" 14 | hermitetype2 = "multiquintic" 15 | hermitefile = "multihermite" 16 | load "hermite_base.gp" -------------------------------------------------------------------------------- /Pathfinder/src/testplot/hermite_base.gp: -------------------------------------------------------------------------------- 1 | set output "hermite_".hermitefile.".png" 2 | set multiplot layout 1, 2 title "Hermite ".hermitetype1." ".hermitetype2 3 | set title "Path" 4 | plot "hermite_".hermitetype1.".csv" u 2:3 w l lw 3 lc "red" title hermitetype1, \ 5 | "hermite_".hermitetype2.".csv" u 2:3 w l lw 3 lc "green" title hermitetype2 6 | 7 | set title "Curvature" 8 | plot "hermite_".hermitetype1.".csv" u 1:4 pt 7 ps 0.75 lc "red" title hermitetype1,\ 9 | "hermite_".hermitetype2.".csv" u 1:4 pt 7 ps 0.75 lc "green" title hermitetype2 10 | unset multiplot -------------------------------------------------------------------------------- /Pathfinder/src/testplot/settings.gp: -------------------------------------------------------------------------------- 1 | set datafile separator "," 2 | set key autotitle columnhead 3 | set size square 4 | set term png size 1200,1200 enhanced 5 | -------------------------------------------------------------------------------- /Pathfinder/src/testplot/trapezoidal.plt: -------------------------------------------------------------------------------- 1 | set size noratio 2 | 3 | set output "profile_trap.png" 4 | set multiplot layout 2, 1 title "Trapezoidal Profile" 5 | set title "Distance" 6 | plot "profile_trap.csv" u 1:2 w l lw 2 lc "red", "profile_trap_simulated.csv" u 1:2 w l lw 2 lc "green" title "dist (sim)" 7 | 8 | set title "Velocity / Accel" 9 | plot "profile_trap.csv" u 1:3 w l lw 2 lc "red", "profile_trap_simulated.csv" u 1:3 w l lw 2 lc "green" title "vel (sim)", "profile_trap.csv" u 1:4 w l lw 2 lc "blue" 10 | unset multiplot -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Pathfinder 2 | ==== 3 | 4 | [![Azure Status](https://dev.azure.com/grapplerobotics/Pathfinder/_apis/build/status/GrappleRobotics.Pathfinder?branchName=master)](https://dev.azure.com/grapplerobotics/Pathfinder/_build/latest?definitionId=1) 5 | 6 | 7 | Pathfinder is a robotic motion library, with logic for motion profiling, path planning and more. 8 | 9 | Pathfinder is still under heavy development, and the structure is still very volatile, please see the issues tab if you'd like to contribute to making Pathfinder great. 10 | 11 | ## Cloning (IMPORTANT!) 12 | If you wish to clone pathfinder, ensure you do so with `--recurse-submodules`. Under `libs/`, we have a number of submodules (including Eigen and GoogleTest) which are used in the library. 13 | 14 | Eigen is the only runtime dependency. All other dependencies are used exclusively during unit testing. 15 | 16 | ## Building Pathfinder 17 | The requirements to build Pathfinder are as follows: 18 | - A C++ Compiler (C++14 or later). Clang is preferred over GCC as our testing system includes the use of the Clang address sanitizer 19 | - A Java Compiler (JDK 9+ recommended). 20 | - GNUPlot Recommended. After unit testing, gnuplot is automatically run to generate graphs of the simulated test cases. 21 | 22 | If you wish, you can run a build with benchmarks by passing `-PwithBench` to your gradle command. 23 | 24 | ## A note on formatting 25 | For C++ files, we include a .clang-format file in the project root. Please ensure all files are formatted using clang-format (or through your IDE if it supports the use of .clang-format) before committing. 26 | -------------------------------------------------------------------------------- /appveyor.yml: -------------------------------------------------------------------------------- 1 | # Run tests with Visual Studio 2017 2 | image: Visual Studio 2017 3 | 4 | environment: 5 | matrix: 6 | - CI_COMPILER: MSVC 7 | 8 | # Install submodules 9 | install: 10 | - git submodule update --init 11 | 12 | build_script: 13 | - gradlew.bat build --stacktrace --console=plain -PwithBench 14 | 15 | # Cache gradle downloads 16 | cache: 17 | - C:\Users\appveyor\.gradle -------------------------------------------------------------------------------- /azure-init.yml: -------------------------------------------------------------------------------- 1 | # Azure has a gradle init file that doesn't work in Gradle 5.0, so we have to delete it. 2 | steps: 3 | - task: DeleteFiles@1 4 | inputs: 5 | sourceFolder: '/Users/vsts/.gradle/init.d/' 6 | contents: 'log-gradle-version-plugin.gradle' -------------------------------------------------------------------------------- /azure-pipelines.yml: -------------------------------------------------------------------------------- 1 | resources: 2 | containers: 3 | - container: linux 4 | image: jaci/azure-native-linux:2018.12.06 5 | options: --cap-add SYS_PTRACE 6 | # ^ Need SYS_PTRACE for Clang ASAN 7 | 8 | jobs: 9 | - job: Windows 10 | pool: 11 | vmImage: 'VS2017-Win2016' 12 | variables: 13 | PlatformName: "win" 14 | steps: 15 | - template: azure-init.yml 16 | - template: azure-steps.yml 17 | parameters: 18 | env: { } 19 | 20 | - job: macOS 21 | pool: 22 | vmImage: 'macOS-10.13' 23 | variables: 24 | PlatformName: "mac" 25 | steps: 26 | - template: azure-init.yml 27 | - template: azure-steps.yml 28 | parameters: 29 | env: { } 30 | 31 | - job: LinuxGCC 32 | pool: 33 | vmImage: 'Ubuntu-16.04' 34 | container: linux 35 | variables: 36 | PlatformName: "linuxGCC" 37 | steps: 38 | - template: azure-steps.yml 39 | parameters: 40 | env: { "CI_COMPILER": "GCC" } 41 | 42 | - job: LinuxClang 43 | pool: 44 | vmImage: 'Ubuntu-16.04' 45 | container: linux 46 | variables: 47 | PlatformName: "linuxClang" 48 | steps: 49 | - template: azure-steps.yml 50 | parameters: 51 | env: { "CI_COMPILER": "CLANG" } 52 | 53 | - job: LinuxRoboRIO 54 | pool: 55 | vmImage: 'Ubuntu-16.04' 56 | container: linux 57 | variables: 58 | PlatformName: "roborio" 59 | steps: 60 | - task: Gradle@2 61 | inputs: 62 | workingDirectory: '' 63 | gradleWrapperFile: 'gradlew' 64 | tasks: 'installRoboRioToolchain' 65 | options: '--scan --stacktrace' 66 | - bash: git apply ../bench-athena.patch 67 | workingDirectory: libs/googlebench 68 | - template: azure-steps.yml 69 | parameters: 70 | env: { "CI_COMPILER": "ROBORIO" } 71 | args: '-xcheck' 72 | has_tests: false 73 | 74 | - job: LinuxRaspbian 75 | pool: 76 | vmImage: 'Ubuntu-16.04' 77 | container: linux 78 | variables: 79 | PlatformName: "raspbian" 80 | steps: 81 | - task: Gradle@2 82 | inputs: 83 | workingDirectory: '' 84 | gradleWrapperFile: 'gradlew' 85 | tasks: 'installRaspbianToolchain' 86 | options: '--scan --stacktrace' 87 | - template: azure-steps.yml 88 | parameters: 89 | env: { "CI_COMPILER": "RASPBIAN" } 90 | args: '-xcheck' 91 | has_tests: false -------------------------------------------------------------------------------- /azure-steps.yml: -------------------------------------------------------------------------------- 1 | parameters: 2 | env: {} 3 | args: '' 4 | has_tests: true 5 | 6 | steps: 7 | - task: Gradle@2 8 | inputs: 9 | workingDirectory: '' 10 | gradleWrapperFile: 'gradlew' 11 | gradleOptions: '-Xmx3072m' 12 | publishJUnitResults: false 13 | testResultsFiles: '**/TEST-*.xml' 14 | tasks: 'clean build doxygen javadoc publish' 15 | options: ${{ parameters.args }} -PwithBench --scan --stacktrace 16 | env: ${{ parameters.env }} 17 | 18 | - task: CopyFiles@2 19 | inputs: 20 | sourceFolder: 'Pathfinder/build' 21 | contents: '**/*' 22 | targetFolder: $(Build.ArtifactStagingDirectory)/build/pf 23 | 24 | - ${{ if eq(parameters.has_tests, 'true') }}: 25 | - task: PublishBuildArtifacts@1 26 | inputs: 27 | artifactName: 'test-results/$(PlatformName)' 28 | pathtoPublish: $(Build.ArtifactStagingDirectory)/build/pf/test-results 29 | 30 | - task: CopyFiles@2 31 | inputs: 32 | sourceFolder: 'build/mvnDistRepo' 33 | contents: '**/*' 34 | targetFolder: $(Build.ArtifactStagingDirectory)/mvnDistRepo 35 | 36 | - task: PublishBuildArtifacts@1 37 | inputs: 38 | artifactName: 'maven' 39 | pathtoPublish: $(Build.ArtifactStagingDirectory)/mvnDistRepo -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GrappleRobotics/Pathfinder/e9a5bd098637e351b497d1016655ad94f981bf8c/gradle/wrapper/gradle-wrapper.jar -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.properties: -------------------------------------------------------------------------------- 1 | distributionBase=GRADLE_USER_HOME 2 | distributionPath=wrapper/dists 3 | distributionUrl=https\://services.gradle.org/distributions/gradle-5.0-bin.zip 4 | zipStoreBase=GRADLE_USER_HOME 5 | zipStorePath=wrapper/dists 6 | -------------------------------------------------------------------------------- /gradlew: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | 3 | ############################################################################## 4 | ## 5 | ## Gradle start up script for UN*X 6 | ## 7 | ############################################################################## 8 | 9 | # Attempt to set APP_HOME 10 | # Resolve links: $0 may be a link 11 | PRG="$0" 12 | # Need this for relative symlinks. 13 | while [ -h "$PRG" ] ; do 14 | ls=`ls -ld "$PRG"` 15 | link=`expr "$ls" : '.*-> \(.*\)$'` 16 | if expr "$link" : '/.*' > /dev/null; then 17 | PRG="$link" 18 | else 19 | PRG=`dirname "$PRG"`"/$link" 20 | fi 21 | done 22 | SAVED="`pwd`" 23 | cd "`dirname \"$PRG\"`/" >/dev/null 24 | APP_HOME="`pwd -P`" 25 | cd "$SAVED" >/dev/null 26 | 27 | APP_NAME="Gradle" 28 | APP_BASE_NAME=`basename "$0"` 29 | 30 | # Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 31 | DEFAULT_JVM_OPTS='"-Xmx64m"' 32 | 33 | # Use the maximum available, or set MAX_FD != -1 to use that value. 34 | MAX_FD="maximum" 35 | 36 | warn () { 37 | echo "$*" 38 | } 39 | 40 | die () { 41 | echo 42 | echo "$*" 43 | echo 44 | exit 1 45 | } 46 | 47 | # OS specific support (must be 'true' or 'false'). 48 | cygwin=false 49 | msys=false 50 | darwin=false 51 | nonstop=false 52 | case "`uname`" in 53 | CYGWIN* ) 54 | cygwin=true 55 | ;; 56 | Darwin* ) 57 | darwin=true 58 | ;; 59 | MINGW* ) 60 | msys=true 61 | ;; 62 | NONSTOP* ) 63 | nonstop=true 64 | ;; 65 | esac 66 | 67 | CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar 68 | 69 | # Determine the Java command to use to start the JVM. 70 | if [ -n "$JAVA_HOME" ] ; then 71 | if [ -x "$JAVA_HOME/jre/sh/java" ] ; then 72 | # IBM's JDK on AIX uses strange locations for the executables 73 | JAVACMD="$JAVA_HOME/jre/sh/java" 74 | else 75 | JAVACMD="$JAVA_HOME/bin/java" 76 | fi 77 | if [ ! -x "$JAVACMD" ] ; then 78 | die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME 79 | 80 | Please set the JAVA_HOME variable in your environment to match the 81 | location of your Java installation." 82 | fi 83 | else 84 | JAVACMD="java" 85 | which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 86 | 87 | Please set the JAVA_HOME variable in your environment to match the 88 | location of your Java installation." 89 | fi 90 | 91 | # Increase the maximum file descriptors if we can. 92 | if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then 93 | MAX_FD_LIMIT=`ulimit -H -n` 94 | if [ $? -eq 0 ] ; then 95 | if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then 96 | MAX_FD="$MAX_FD_LIMIT" 97 | fi 98 | ulimit -n $MAX_FD 99 | if [ $? -ne 0 ] ; then 100 | warn "Could not set maximum file descriptor limit: $MAX_FD" 101 | fi 102 | else 103 | warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" 104 | fi 105 | fi 106 | 107 | # For Darwin, add options to specify how the application appears in the dock 108 | if $darwin; then 109 | GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" 110 | fi 111 | 112 | # For Cygwin, switch paths to Windows format before running java 113 | if $cygwin ; then 114 | APP_HOME=`cygpath --path --mixed "$APP_HOME"` 115 | CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` 116 | JAVACMD=`cygpath --unix "$JAVACMD"` 117 | 118 | # We build the pattern for arguments to be converted via cygpath 119 | ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` 120 | SEP="" 121 | for dir in $ROOTDIRSRAW ; do 122 | ROOTDIRS="$ROOTDIRS$SEP$dir" 123 | SEP="|" 124 | done 125 | OURCYGPATTERN="(^($ROOTDIRS))" 126 | # Add a user-defined pattern to the cygpath arguments 127 | if [ "$GRADLE_CYGPATTERN" != "" ] ; then 128 | OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" 129 | fi 130 | # Now convert the arguments - kludge to limit ourselves to /bin/sh 131 | i=0 132 | for arg in "$@" ; do 133 | CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` 134 | CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option 135 | 136 | if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition 137 | eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` 138 | else 139 | eval `echo args$i`="\"$arg\"" 140 | fi 141 | i=$((i+1)) 142 | done 143 | case $i in 144 | (0) set -- ;; 145 | (1) set -- "$args0" ;; 146 | (2) set -- "$args0" "$args1" ;; 147 | (3) set -- "$args0" "$args1" "$args2" ;; 148 | (4) set -- "$args0" "$args1" "$args2" "$args3" ;; 149 | (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; 150 | (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; 151 | (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; 152 | (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; 153 | (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; 154 | esac 155 | fi 156 | 157 | # Escape application args 158 | save () { 159 | for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done 160 | echo " " 161 | } 162 | APP_ARGS=$(save "$@") 163 | 164 | # Collect all arguments for the java command, following the shell quoting and substitution rules 165 | eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" 166 | 167 | # by default we should be in the correct project dir, but when run from Finder on Mac, the cwd is wrong 168 | if [ "$(uname)" = "Darwin" ] && [ "$HOME" = "$PWD" ]; then 169 | cd "$(dirname "$0")" 170 | fi 171 | 172 | exec "$JAVACMD" "$@" 173 | -------------------------------------------------------------------------------- /gradlew.bat: -------------------------------------------------------------------------------- 1 | @if "%DEBUG%" == "" @echo off 2 | @rem ########################################################################## 3 | @rem 4 | @rem Gradle startup script for Windows 5 | @rem 6 | @rem ########################################################################## 7 | 8 | @rem Set local scope for the variables with windows NT shell 9 | if "%OS%"=="Windows_NT" setlocal 10 | 11 | set DIRNAME=%~dp0 12 | if "%DIRNAME%" == "" set DIRNAME=. 13 | set APP_BASE_NAME=%~n0 14 | set APP_HOME=%DIRNAME% 15 | 16 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 17 | set DEFAULT_JVM_OPTS="-Xmx64m" 18 | 19 | @rem Find java.exe 20 | if defined JAVA_HOME goto findJavaFromJavaHome 21 | 22 | set JAVA_EXE=java.exe 23 | %JAVA_EXE% -version >NUL 2>&1 24 | if "%ERRORLEVEL%" == "0" goto init 25 | 26 | echo. 27 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 28 | echo. 29 | echo Please set the JAVA_HOME variable in your environment to match the 30 | echo location of your Java installation. 31 | 32 | goto fail 33 | 34 | :findJavaFromJavaHome 35 | set JAVA_HOME=%JAVA_HOME:"=% 36 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe 37 | 38 | if exist "%JAVA_EXE%" goto init 39 | 40 | echo. 41 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 42 | echo. 43 | echo Please set the JAVA_HOME variable in your environment to match the 44 | echo location of your Java installation. 45 | 46 | goto fail 47 | 48 | :init 49 | @rem Get command-line arguments, handling Windows variants 50 | 51 | if not "%OS%" == "Windows_NT" goto win9xME_args 52 | 53 | :win9xME_args 54 | @rem Slurp the command line arguments. 55 | set CMD_LINE_ARGS= 56 | set _SKIP=2 57 | 58 | :win9xME_args_slurp 59 | if "x%~1" == "x" goto execute 60 | 61 | set CMD_LINE_ARGS=%* 62 | 63 | :execute 64 | @rem Setup the command line 65 | 66 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar 67 | 68 | @rem Execute Gradle 69 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% 70 | 71 | :end 72 | @rem End local scope for the variables with windows NT shell 73 | if "%ERRORLEVEL%"=="0" goto mainEnd 74 | 75 | :fail 76 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of 77 | rem the _cmd.exe /c_ return code! 78 | if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 79 | exit /b 1 80 | 81 | :mainEnd 82 | if "%OS%"=="Windows_NT" endlocal 83 | 84 | :omega 85 | -------------------------------------------------------------------------------- /libs/bench-athena.patch: -------------------------------------------------------------------------------- 1 | diff --git a/src/sysinfo.cc b/src/sysinfo.cc 2 | index d4e5336..9ead4c5 100644 3 | --- a/src/sysinfo.cc 4 | +++ b/src/sysinfo.cc 5 | @@ -269,9 +269,9 @@ std::vector GetCacheSizesFromKVFS() { 6 | if (!ReadFromFile(StrCat(FPath, "level"), &info.level)) 7 | PrintErrorAndDie("Failed to read from file ", FPath, "level"); 8 | std::string map_str; 9 | - if (!ReadFromFile(StrCat(FPath, "shared_cpu_map"), &map_str)) 10 | - PrintErrorAndDie("Failed to read from file ", FPath, "shared_cpu_map"); 11 | - info.num_sharing = CountSetBitsInCPUMap(map_str); 12 | + // if (!ReadFromFile(StrCat(FPath, "shared_cpu_map"), &map_str)) 13 | + // PrintErrorAndDie("Failed to read from file ", FPath, "shared_cpu_map"); 14 | + // info.num_sharing = CountSetBitsInCPUMap(map_str); 15 | res.push_back(info); 16 | } 17 | 18 | -------------------------------------------------------------------------------- /libs/build.gradle: -------------------------------------------------------------------------------- 1 | apply plugin: "cpp" 2 | 3 | ext.libroot = new File(rootProject.rootDir, "libs") 4 | ext.gtest_root = new File(libroot, "googletest/googletest") 5 | ext.gbench_root = new File(libroot, "googlebench") 6 | ext.eigen_root = new File(libroot, "eigen") 7 | 8 | ext.eigen_version = "3.3.5" 9 | ext.googlebench_version = "1.4.1" 10 | ext.googletest_version = "1.8.1" 11 | 12 | model { 13 | buildTypes { 14 | release 15 | debug 16 | } 17 | 18 | components { 19 | googleTest(NativeLibrarySpec) { 20 | sources.cpp { 21 | source { 22 | srcDir new File(gtest_root, "src") 23 | include "**/gtest-all.cc" 24 | } 25 | exportedHeaders { 26 | srcDirs gtest_root, new File(gtest_root, "include") 27 | include "**/*.hpp", "**/*.h" 28 | } 29 | } 30 | } 31 | 32 | googleBench(NativeLibrarySpec) { 33 | binaries.all { 34 | if (toolChain instanceof GccCompatibleToolChain) { 35 | cppCompiler.args << '-std=c++14' << '-Wno-deprecated-declarations' 36 | linker.args << '-pthread' 37 | } else { 38 | // MSVC 39 | cppCompiler.args << '/std:c++14' 40 | linker.args << 'shlwapi.lib' 41 | } 42 | } 43 | 44 | sources.cpp { 45 | source { 46 | srcDir new File(gbench_root, "src") 47 | include "**/*.cc" 48 | exclude "**/benchmark_main.cc" 49 | } 50 | exportedHeaders { 51 | srcDirs new File(gbench_root, "src"), new File(gbench_root, "include") 52 | include "**/*.hpp", "**/*.h" 53 | } 54 | lib project: ':libs', library: "googleTest", linkage: "static" 55 | } 56 | } 57 | 58 | eigen(NativeLibrarySpec) { 59 | sources.cpp { 60 | exportedHeaders { 61 | srcDirs eigen_root 62 | include "Eigen/**/*" 63 | } 64 | } 65 | } 66 | } 67 | // repositories { 68 | // libs(PrebuiltLibraries) { 69 | // eigen { 70 | // headers.srcDir eigen_root 71 | // headers.include "**/*" 72 | // } 73 | // } 74 | // } 75 | } 76 | 77 | task zipEigenHeaders(type: Zip) { 78 | from fileTree(eigen_root) { 79 | include "Eigen/**/*" 80 | } 81 | 82 | baseName = "Eigen" 83 | classifier = "headers" 84 | version = eigen_version 85 | } 86 | 87 | task zipGoogleTestHeaders(type: Zip) { 88 | from fileTree(new File(gtest_root, "include")) { 89 | include "**/*.hpp", "**/*.h" 90 | } 91 | 92 | baseName = "GTest" 93 | classifier = "headers" 94 | version = googletest_version 95 | } 96 | 97 | task zipGoogleBenchHeaders(type: Zip) { 98 | from fileTree(new File(gbench_root, "src")) { 99 | include "**/*.hpp", "**/*.h" 100 | } 101 | 102 | from fileTree(new File(gbench_root, "include")) { 103 | include "**/*.hpp", "**/*.h" 104 | } 105 | 106 | baseName = "GBench" 107 | classifier = "headers" 108 | version = googlebench_version 109 | } 110 | 111 | publishing { 112 | publications { 113 | eigen(MavenPublication) { 114 | groupId 'grpl.thirdparty.tuxfamily.eigen' 115 | artifactId 'Eigen' 116 | version eigen_version 117 | 118 | artifact zipEigenHeaders { 119 | classifier 'headers' 120 | } 121 | } 122 | googleBench(MavenPublication) { 123 | groupId 'grpl.thirdparty.google.test' 124 | artifactId 'GoogleBench' 125 | version googlebench_version 126 | 127 | artifact zipGoogleBenchHeaders { 128 | classifier 'headers' 129 | } 130 | 131 | binaryArtifacts(it, 'googleBench', false) 132 | } 133 | googleTest(MavenPublication) { 134 | groupId 'grpl.thirdparty.google.test' 135 | artifactId 'GoogleTest' 136 | version googlebench_version 137 | 138 | artifact zipGoogleTestHeaders { 139 | classifier 'headers' 140 | } 141 | 142 | binaryArtifacts(it, 'googleTest', false) 143 | } 144 | } 145 | } -------------------------------------------------------------------------------- /settings.gradle: -------------------------------------------------------------------------------- 1 | pluginManagement { 2 | repositories { 3 | mavenLocal() 4 | gradlePluginPortal() 5 | } 6 | } 7 | 8 | rootProject.name = 'Pathfinder' 9 | 10 | include 'libs', 'Pathfinder', 'Pathfinder-Java' -------------------------------------------------------------------------------- /vendordeps.gradle: -------------------------------------------------------------------------------- 1 | import groovy.json.JsonOutput 2 | 3 | Map vendorJson() { 4 | return [ 5 | fileName: "Pathfinder.json", 6 | name: "Pathfinder", 7 | version: project.version, 8 | uuid: "44237bb3-7675-43ba-894a-302083a37bd8", 9 | mavenUrls: [ 10 | "https://dev.imjac.in/maven" 11 | ], 12 | jsonUrl: "https://dev.imjac.in/maven/grpl/pathfinder/Pathfinder-latest.json", 13 | cppDependencies: [ 14 | [ 15 | groupId: project.group, 16 | artifactId: "Pathfinder", 17 | version: project.version, 18 | libName: "Pathfinder", 19 | configuration: "native_pathfinder", 20 | headerClassifier: "headers", 21 | binaryPlatforms: [] 22 | ], 23 | [ 24 | groupId: "grpl.thirdparty.tuxfamily.eigen", 25 | artifactId: "Eigen", 26 | version: project(':libs').eigen_version, 27 | libName: "Eigen", 28 | configuration: "native_pathfinder", 29 | headerClassifier: "headers", 30 | binaryPlatforms: [] 31 | ] 32 | ], 33 | javaDependencies: [ 34 | [ 35 | groupId: project.group, 36 | artifactId: "Pathfinder-Java", 37 | version: project.version 38 | ] 39 | ], 40 | jniDependencies: [ 41 | [ 42 | groupId: project.group, 43 | artifactId: "Pathfinder-JNI", 44 | version: project.version, 45 | isJar: true, 46 | skipInvalidPlatforms: true, 47 | validPlatforms: [ 48 | "linuxx86-64", 49 | "windowsx86-64", 50 | "osxx86-64", 51 | "linuxx86", 52 | "windowsx86", 53 | "osxx86", 54 | "linuxathena", 55 | "linuxraspbian" 56 | ] 57 | ] 58 | ] 59 | ] 60 | } 61 | 62 | String vendorJsonString() { 63 | return JsonOutput.prettyPrint(JsonOutput.toJson(vendorJson())) 64 | } 65 | 66 | task generateVendorDepsJson() { 67 | def outfile = new File(buildDir, "Pathfinder.json") 68 | outputs.file(outfile) 69 | 70 | doLast { 71 | outfile.text = vendorJsonString() 72 | } 73 | } 74 | 75 | publishing { 76 | publications { 77 | vendordeps(MavenPublication) { 78 | artifactId 'Pathfinder-FRCDeps' 79 | artifact(generateVendorDepsJson.outputs.files.files[0]) { 80 | builtBy generateVendorDepsJson 81 | } 82 | } 83 | } 84 | } 85 | 86 | afterEvaluate { 87 | publishing.repositories.all { 88 | def vendorTask = task "writeLatestVendorDepsTo${name}"() { 89 | def outfile = new File(new File(url), "grpl/pathfinder/Pathfinder-latest.json") 90 | outputs.file(outfile) 91 | 92 | doLast { 93 | outfile.text = vendorJsonString() 94 | } 95 | } 96 | 97 | tasks.withType(PublishToMavenRepository).all { 98 | dependsOn vendorTask 99 | } 100 | } 101 | } --------------------------------------------------------------------------------