Run autoformat
This commit is contained in:
parent
3dff114fd5
commit
bdc8053235
|
@ -18,8 +18,8 @@ AlwaysBreakAfterDefinitionReturnType: None
|
||||||
AlwaysBreakAfterReturnType: None
|
AlwaysBreakAfterReturnType: None
|
||||||
AlwaysBreakBeforeMultilineStrings: true
|
AlwaysBreakBeforeMultilineStrings: true
|
||||||
AlwaysBreakTemplateDeclarations: true
|
AlwaysBreakTemplateDeclarations: true
|
||||||
BinPackArguments: true
|
BinPackArguments: false
|
||||||
BinPackParameters: true
|
BinPackParameters: false
|
||||||
BraceWrapping:
|
BraceWrapping:
|
||||||
AfterClass: false
|
AfterClass: false
|
||||||
AfterControlStatement: false
|
AfterControlStatement: false
|
||||||
|
|
|
@ -82,8 +82,8 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
|
||||||
pose.translation_ *
|
pose.translation_ *
|
||||||
Rotation2d{half_theta_by_tan_of_half_dtheta, -half_dtheta, false};
|
Rotation2d{half_theta_by_tan_of_half_dtheta, -half_dtheta, false};
|
||||||
|
|
||||||
return Twist2d{translation_part.X(), translation_part.Y(),
|
return Twist2d{
|
||||||
pose.rotation_.Radians()};
|
translation_part.X(), translation_part.Y(), pose.rotation_.Radians()};
|
||||||
}
|
}
|
||||||
|
|
||||||
static Pose2d FromTwist(const Twist2d& twist) {
|
static Pose2d FromTwist(const Twist2d& twist) {
|
||||||
|
|
|
@ -38,7 +38,8 @@ class Rotation2d final {
|
||||||
|
|
||||||
Rotation2d operator+(const Rotation2d& other) const {
|
Rotation2d operator+(const Rotation2d& other) const {
|
||||||
return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(),
|
return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(),
|
||||||
Cos() * other.Sin() + Sin() * other.Cos(), true};
|
Cos() * other.Sin() + Sin() * other.Cos(),
|
||||||
|
true};
|
||||||
}
|
}
|
||||||
|
|
||||||
// Accessors
|
// Accessors
|
||||||
|
|
|
@ -45,9 +45,11 @@ class DistanceTrajectory : public Trajectory<double, S> {
|
||||||
return TrajectorySamplePoint<S>(s, i, i);
|
return TrajectorySamplePoint<S>(s, i, i);
|
||||||
}
|
}
|
||||||
return TrajectorySamplePoint<S>(
|
return TrajectorySamplePoint<S>(
|
||||||
prev_s.Interpolate(s, (interpolant - distances_[i - 1]) /
|
prev_s.Interpolate(s,
|
||||||
|
(interpolant - distances_[i - 1]) /
|
||||||
(distances_[i] - distances_[i - 1])),
|
(distances_[i] - distances_[i - 1])),
|
||||||
i - 1, i);
|
i - 1,
|
||||||
|
i);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
throw - 1;
|
throw - 1;
|
||||||
|
|
|
@ -7,18 +7,16 @@ namespace frc5190 {
|
||||||
template <typename S>
|
template <typename S>
|
||||||
class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
|
class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
|
||||||
public:
|
public:
|
||||||
TimedEntry(const S& state, const double t, const double velocity,
|
TimedEntry(const S& state,
|
||||||
|
const double t,
|
||||||
|
const double velocity,
|
||||||
const double acceleration)
|
const double acceleration)
|
||||||
: state_(state),
|
: state_(state),
|
||||||
t_(t),
|
t_(t),
|
||||||
velocity_(velocity),
|
velocity_(velocity),
|
||||||
acceleration_(acceleration) {}
|
acceleration_(acceleration) {}
|
||||||
|
|
||||||
TimedEntry()
|
TimedEntry() : t_(0), velocity_(0), acceleration_(0) {}
|
||||||
: t_(0),
|
|
||||||
velocity_(0),
|
|
||||||
acceleration_(0) {
|
|
||||||
}
|
|
||||||
|
|
||||||
TimedEntry<S> Interpolate(const TimedEntry<S>& end_value,
|
TimedEntry<S> Interpolate(const TimedEntry<S>& end_value,
|
||||||
double t) const override {
|
double t) const override {
|
||||||
|
@ -38,7 +36,9 @@ class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
|
||||||
return TimedEntry{
|
return TimedEntry{
|
||||||
state_.Interpolate(end_value.state_,
|
state_.Interpolate(end_value.state_,
|
||||||
new_s / state_.Distance(end_value.state_)),
|
new_s / state_.Distance(end_value.state_)),
|
||||||
new_t, new_v, acceleration_};
|
new_t,
|
||||||
|
new_v,
|
||||||
|
acceleration_};
|
||||||
}
|
}
|
||||||
|
|
||||||
double Distance(const TimedEntry<S>& other) const override {
|
double Distance(const TimedEntry<S>& other) const override {
|
||||||
|
@ -100,7 +100,8 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
|
||||||
prev_s.state.Interpolate(s.state,
|
prev_s.state.Interpolate(s.state,
|
||||||
(interpolant - prev_s.state.T()) /
|
(interpolant - prev_s.state.T()) /
|
||||||
(s.state.T() - prev_s.state.T())),
|
(s.state.T() - prev_s.state.T())),
|
||||||
i - 1, i);
|
i - 1,
|
||||||
|
i);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
throw - 1;
|
throw - 1;
|
||||||
|
|
|
@ -13,24 +13,20 @@ struct TrajectoryPoint {
|
||||||
|
|
||||||
template <typename S>
|
template <typename S>
|
||||||
struct TrajectorySamplePoint {
|
struct TrajectorySamplePoint {
|
||||||
|
|
||||||
S state;
|
S state;
|
||||||
int index_floor;
|
int index_floor;
|
||||||
int index_ceil;
|
int index_ceil;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
explicit TrajectorySamplePoint(TrajectoryPoint<S> point)
|
explicit TrajectorySamplePoint(TrajectoryPoint<S> point)
|
||||||
: state(point.state),
|
: state(point.state), index_floor(point.index), index_ceil(point.index) {}
|
||||||
index_floor(point.index),
|
|
||||||
index_ceil(point.index) {}
|
|
||||||
|
|
||||||
TrajectorySamplePoint(S state, int index_floor, int index_ceil)
|
TrajectorySamplePoint(S state, int index_floor, int index_ceil)
|
||||||
: state(std::move(state)), index_floor(index_floor), index_ceil(index_ceil) {}
|
: state(std::move(state)),
|
||||||
|
index_floor(index_floor),
|
||||||
|
index_ceil(index_ceil) {}
|
||||||
|
|
||||||
TrajectorySamplePoint()
|
TrajectorySamplePoint() : index_floor(0), index_ceil(0) {}
|
||||||
: index_floor(0),
|
|
||||||
index_ceil(0) {
|
|
||||||
}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
template <typename U, typename S>
|
template <typename U, typename S>
|
||||||
|
|
|
@ -1,6 +1,5 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <array>
|
|
||||||
#include "../geometry/Pose2dWithCurvature.h"
|
#include "../geometry/Pose2dWithCurvature.h"
|
||||||
#include "../spline/ParametricQuinticHermiteSpline.h"
|
#include "../spline/ParametricQuinticHermiteSpline.h"
|
||||||
#include "../spline/SplineGenerator.h"
|
#include "../spline/SplineGenerator.h"
|
||||||
|
@ -16,8 +15,11 @@ class TrajectoryGenerator {
|
||||||
static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
|
static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
|
||||||
std::vector<Pose2d> waypoints,
|
std::vector<Pose2d> waypoints,
|
||||||
const std::vector<TimingConstraint<Pose2dWithCurvature>*>& constraints,
|
const std::vector<TimingConstraint<Pose2dWithCurvature>*>& constraints,
|
||||||
const double start_velocity, const double end_velocity,
|
const double start_velocity,
|
||||||
const double max_velocity, const double max_acceleration, bool reversed) {
|
const double end_velocity,
|
||||||
|
const double max_velocity,
|
||||||
|
const double max_acceleration,
|
||||||
|
const bool reversed) {
|
||||||
const auto flipped_position =
|
const auto flipped_position =
|
||||||
Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
|
Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
|
||||||
|
|
||||||
|
@ -35,19 +37,27 @@ class TrajectoryGenerator {
|
||||||
if (reversed) {
|
if (reversed) {
|
||||||
for (auto& point : points) {
|
for (auto& point : points) {
|
||||||
point = Pose2dWithCurvature{point.Pose().TransformBy(flipped_position),
|
point = Pose2dWithCurvature{point.Pose().TransformBy(flipped_position),
|
||||||
-point.Curvature(), point.Dkds()};
|
-point.Curvature(),
|
||||||
|
point.Dkds()};
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return TimeParameterizeTrajectory(
|
return TimeParameterizeTrajectory(
|
||||||
DistanceTrajectory<Pose2dWithCurvature>(points), constraints,
|
DistanceTrajectory<Pose2dWithCurvature>(points),
|
||||||
start_velocity, end_velocity, max_velocity, max_acceleration, 0.051,
|
constraints,
|
||||||
|
start_velocity,
|
||||||
|
end_velocity,
|
||||||
|
max_velocity,
|
||||||
|
max_acceleration,
|
||||||
|
0.051,
|
||||||
reversed);
|
reversed);
|
||||||
}
|
}
|
||||||
|
|
||||||
static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
|
static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
|
||||||
const std::vector<Pose2d>& waypoints, const double max_dx,
|
const std::vector<Pose2d>& waypoints,
|
||||||
const double max_dy, const double max_dtheta) {
|
const double max_dx,
|
||||||
|
const double max_dy,
|
||||||
|
const double max_dtheta) {
|
||||||
auto size = static_cast<int>(waypoints.size());
|
auto size = static_cast<int>(waypoints.size());
|
||||||
std::vector<ParametricSpline*> splines(size - 1);
|
std::vector<ParametricSpline*> splines(size - 1);
|
||||||
for (int i = 1; i < waypoints.size(); ++i) {
|
for (int i = 1; i < waypoints.size(); ++i) {
|
||||||
|
@ -55,8 +65,8 @@ class TrajectoryGenerator {
|
||||||
new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]);
|
new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]);
|
||||||
}
|
}
|
||||||
auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
|
auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
|
||||||
SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy,
|
SplineGenerator::ParameterizeSplines(
|
||||||
max_dtheta));
|
splines, max_dx, max_dy, max_dtheta));
|
||||||
|
|
||||||
for (auto ptr : splines) {
|
for (auto ptr : splines) {
|
||||||
delete ptr;
|
delete ptr;
|
||||||
|
@ -76,7 +86,8 @@ class TrajectoryGenerator {
|
||||||
|
|
||||||
template <typename S>
|
template <typename S>
|
||||||
static void EnforceAccelerationLimits(
|
static void EnforceAccelerationLimits(
|
||||||
bool reverse, std::vector<TimingConstraint<S>*> constraints,
|
bool reverse,
|
||||||
|
std::vector<TimingConstraint<S>*> constraints,
|
||||||
ConstrainedPose<S>* constrained_pose) {
|
ConstrainedPose<S>* constrained_pose) {
|
||||||
for (const auto& constraint : constraints) {
|
for (const auto& constraint : constraints) {
|
||||||
auto min_max_accel = constraint->MinMaxAcceleration(
|
auto min_max_accel = constraint->MinMaxAcceleration(
|
||||||
|
@ -101,8 +112,12 @@ class TrajectoryGenerator {
|
||||||
static TimedTrajectory<S> TimeParameterizeTrajectory(
|
static TimedTrajectory<S> TimeParameterizeTrajectory(
|
||||||
DistanceTrajectory<S> distance_trajectory,
|
DistanceTrajectory<S> distance_trajectory,
|
||||||
std::vector<TimingConstraint<Pose2dWithCurvature>*> constraints,
|
std::vector<TimingConstraint<Pose2dWithCurvature>*> constraints,
|
||||||
double start_velocity, double end_velocity, double max_velocity,
|
double start_velocity,
|
||||||
double max_acceleration, double step_size, bool reversed) {
|
double end_velocity,
|
||||||
|
double max_velocity,
|
||||||
|
double max_acceleration,
|
||||||
|
double step_size,
|
||||||
|
bool reversed) {
|
||||||
const auto num_states = static_cast<int>(
|
const auto num_states = static_cast<int>(
|
||||||
std::ceil(distance_trajectory.LastInterpolant() / step_size + 1));
|
std::ceil(distance_trajectory.LastInterpolant() / step_size + 1));
|
||||||
|
|
||||||
|
@ -126,8 +141,8 @@ class TrajectoryGenerator {
|
||||||
|
|
||||||
std::vector<ConstrainedPose<S>> constrained_poses(num_states);
|
std::vector<ConstrainedPose<S>> constrained_poses(num_states);
|
||||||
|
|
||||||
auto _predecessor = ConstrainedPose<S>{states[0], 0.0, start_velocity,
|
auto _predecessor = ConstrainedPose<S>{
|
||||||
-max_acceleration, max_acceleration};
|
states[0], 0.0, start_velocity, -max_acceleration, max_acceleration};
|
||||||
ConstrainedPose<S>* predecessor = &_predecessor;
|
ConstrainedPose<S>* predecessor = &_predecessor;
|
||||||
|
|
||||||
for (int i = 0; i < states.size(); ++i) {
|
for (int i = 0; i < states.size(); ++i) {
|
||||||
|
@ -196,7 +211,9 @@ class TrajectoryGenerator {
|
||||||
auto _successor =
|
auto _successor =
|
||||||
ConstrainedPose<S>{states[states.size() - 1],
|
ConstrainedPose<S>{states[states.size() - 1],
|
||||||
constrained_poses[states.size() - 1].distance,
|
constrained_poses[states.size() - 1].distance,
|
||||||
end_velocity, -max_acceleration, max_acceleration};
|
end_velocity,
|
||||||
|
-max_acceleration,
|
||||||
|
max_acceleration};
|
||||||
ConstrainedPose<S>* successor = &_successor;
|
ConstrainedPose<S>* successor = &_successor;
|
||||||
|
|
||||||
for (int i = states.size() - 1; i >= 0; --i) {
|
for (int i = states.size() - 1; i >= 0; --i) {
|
||||||
|
@ -268,8 +285,9 @@ class TrajectoryGenerator {
|
||||||
|
|
||||||
v = constrained_pose.max_velocity;
|
v = constrained_pose.max_velocity;
|
||||||
s = constrained_pose.distance;
|
s = constrained_pose.distance;
|
||||||
timed_states[i] =
|
timed_states[i] = TimedEntry<S>{constrained_pose.state,
|
||||||
TimedEntry<S>{constrained_pose.state, t, reversed ? -v : v,
|
t,
|
||||||
|
reversed ? -v : v,
|
||||||
reversed ? -accel : accel};
|
reversed ? -accel : accel};
|
||||||
|
|
||||||
t += dt;
|
t += dt;
|
||||||
|
|
|
@ -19,16 +19,16 @@ class TrajectoryIterator {
|
||||||
}
|
}
|
||||||
|
|
||||||
TrajectorySamplePoint<S> Advance(U amount) {
|
TrajectorySamplePoint<S> Advance(U amount) {
|
||||||
progress_ =
|
progress_ = Clamp(Addition(progress_, amount),
|
||||||
Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(),
|
trajectory_->FirstInterpolant(),
|
||||||
trajectory_->LastInterpolant());
|
trajectory_->LastInterpolant());
|
||||||
sample_ = trajectory_->Sample(progress_);
|
sample_ = trajectory_->Sample(progress_);
|
||||||
return sample_;
|
return sample_;
|
||||||
}
|
}
|
||||||
|
|
||||||
TrajectorySamplePoint<S> Preview(U amount) {
|
TrajectorySamplePoint<S> Preview(U amount) {
|
||||||
auto progress =
|
auto progress = Clamp(Addition(progress_, amount),
|
||||||
Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(),
|
trajectory_->FirstInterpolant(),
|
||||||
trajectory_->LastInterpolant());
|
trajectory_->LastInterpolant());
|
||||||
return trajectory_->Sample(progress);
|
return trajectory_->Sample(progress);
|
||||||
}
|
}
|
||||||
|
|
|
@ -13,7 +13,6 @@ class AngularAccelerationConstraint final
|
||||||
|
|
||||||
~AngularAccelerationConstraint() = default;
|
~AngularAccelerationConstraint() = default;
|
||||||
|
|
||||||
|
|
||||||
double MaxVelocity(const Pose2dWithCurvature& state) const override {
|
double MaxVelocity(const Pose2dWithCurvature& state) const override {
|
||||||
/**
|
/**
|
||||||
* We don't want v^2 * dk/ds alone to go over the max angular acceleration.
|
* We don't want v^2 * dk/ds alone to go over the max angular acceleration.
|
||||||
|
|
|
@ -7,7 +7,6 @@ namespace frc5190 {
|
||||||
|
|
||||||
class CentripetalAccelerationConstraint final
|
class CentripetalAccelerationConstraint final
|
||||||
: public TimingConstraint<Pose2dWithCurvature> {
|
: public TimingConstraint<Pose2dWithCurvature> {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
explicit CentripetalAccelerationConstraint(
|
explicit CentripetalAccelerationConstraint(
|
||||||
const double max_centripetal_acceleration)
|
const double max_centripetal_acceleration)
|
||||||
|
|
|
@ -8,7 +8,8 @@ namespace frc5190 {
|
||||||
class VelocityLimitRadiusConstraint
|
class VelocityLimitRadiusConstraint
|
||||||
: public TimingConstraint<Pose2dWithCurvature> {
|
: public TimingConstraint<Pose2dWithCurvature> {
|
||||||
public:
|
public:
|
||||||
VelocityLimitRadiusConstraint(const Translation2d& point, const double radius,
|
VelocityLimitRadiusConstraint(const Translation2d& point,
|
||||||
|
const double radius,
|
||||||
const double max_velocity)
|
const double max_velocity)
|
||||||
: point_(point), radius_(radius), max_velocity_(max_velocity) {}
|
: point_(point), radius_(radius), max_velocity_(max_velocity) {}
|
||||||
|
|
||||||
|
|
|
@ -9,7 +9,8 @@ class Interpolatable {
|
||||||
virtual ~Interpolatable() = default;
|
virtual ~Interpolatable() = default;
|
||||||
virtual T Interpolate(const T& end_value, double t) const = 0;
|
virtual T Interpolate(const T& end_value, double t) const = 0;
|
||||||
|
|
||||||
static constexpr double Lerp(const double start_value, const double end_value,
|
static constexpr double Lerp(const double start_value,
|
||||||
|
const double end_value,
|
||||||
const double t) {
|
const double t) {
|
||||||
return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0);
|
return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0);
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,6 +1,5 @@
|
||||||
#include "pch.h"
|
#include "pch.h"
|
||||||
#include "../FalconLibraryCPP/src/FalconLibrary.h"
|
#include "../FalconLibraryCPP/src/FalconLibrary.h"
|
||||||
#include "../FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h"
|
|
||||||
|
|
||||||
constexpr double kTestEpsilon = 1E-6;
|
constexpr double kTestEpsilon = 1E-6;
|
||||||
|
|
||||||
|
@ -8,25 +7,30 @@ class TrajectoryTest : public ::testing::Test {
|
||||||
public:
|
public:
|
||||||
TrajectoryTest() {}
|
TrajectoryTest() {}
|
||||||
|
|
||||||
void Run(frc5190::Pose2d initial, frc5190::Pose2d final,
|
void Run(frc5190::Pose2d initial,
|
||||||
double max_velocity = 3, double max_acceleration = 2,
|
frc5190::Pose2d final,
|
||||||
|
double max_velocity = 3,
|
||||||
|
double max_acceleration = 2,
|
||||||
bool backwards = false) {
|
bool backwards = false) {
|
||||||
auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory(
|
auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory(
|
||||||
std::vector<frc5190::Pose2d>{initial, final},
|
std::vector<frc5190::Pose2d>{initial, final},
|
||||||
std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{
|
std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{},
|
||||||
new frc5190::CentripetalAccelerationConstraint{100.0}},
|
0.0,
|
||||||
0.0, 0.0, max_velocity, max_acceleration, backwards);
|
0.0,
|
||||||
|
max_velocity,
|
||||||
|
max_acceleration,
|
||||||
|
backwards);
|
||||||
|
|
||||||
auto pose = trajectory.Sample(0.0).state.State().Pose();
|
auto pose = trajectory.Sample(0.0).state.State().Pose();
|
||||||
|
|
||||||
EXPECT_FALSE(false);
|
EXPECT_FALSE(false);
|
||||||
|
|
||||||
EXPECT_NEAR(pose.Translation().X(), initial.Translation().X(),
|
EXPECT_NEAR(
|
||||||
kTestEpsilon);
|
pose.Translation().X(), initial.Translation().X(), kTestEpsilon);
|
||||||
EXPECT_NEAR(pose.Translation().Y(), initial.Translation().Y(),
|
EXPECT_NEAR(
|
||||||
kTestEpsilon);
|
pose.Translation().Y(), initial.Translation().Y(), kTestEpsilon);
|
||||||
EXPECT_NEAR(pose.Rotation().Radians(), initial.Rotation().Radians(),
|
EXPECT_NEAR(
|
||||||
kTestEpsilon);
|
pose.Rotation().Radians(), initial.Rotation().Radians(), kTestEpsilon);
|
||||||
|
|
||||||
const auto iterator = trajectory.Iterator();
|
const auto iterator = trajectory.Iterator();
|
||||||
|
|
||||||
|
@ -51,8 +55,8 @@ class TrajectoryTest : public ::testing::Test {
|
||||||
|
|
||||||
EXPECT_NEAR(pose1.Translation().X(), final.Translation().X(), kTestEpsilon);
|
EXPECT_NEAR(pose1.Translation().X(), final.Translation().X(), kTestEpsilon);
|
||||||
EXPECT_NEAR(pose1.Translation().Y(), final.Translation().Y(), kTestEpsilon);
|
EXPECT_NEAR(pose1.Translation().Y(), final.Translation().Y(), kTestEpsilon);
|
||||||
EXPECT_NEAR(pose1.Rotation().Radians(), final.Rotation().Radians(),
|
EXPECT_NEAR(
|
||||||
kTestEpsilon);
|
pose1.Rotation().Radians(), final.Rotation().Radians(), kTestEpsilon);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue
Block a user