From 94f1d9b4a820cf7de2342e77a9889fb2bc6e67fd Mon Sep 17 00:00:00 2001 From: Mathew Halm Date: Tue, 19 Nov 2024 11:18:01 -0800 Subject: [PATCH 1/2] [trajectories] Add PiecewiseConstantCuravtureTrajectory Adding a trajectory type modeling concatenation of linear segments and circular arcs within a plane, to be used in curvilinear joints (#22196). --- common/trajectories/BUILD.bazel | 24 ++ ...piecewise_constant_curvature_trajectory.cc | 167 +++++++++++ .../piecewise_constant_curvature_trajectory.h | 211 ++++++++++++++ .../test/piecewise_constant_curvature_test.cc | 259 ++++++++++++++++++ 4 files changed, 661 insertions(+) create mode 100644 common/trajectories/piecewise_constant_curvature_trajectory.cc create mode 100644 common/trajectories/piecewise_constant_curvature_trajectory.h create mode 100644 common/trajectories/test/piecewise_constant_curvature_test.cc diff --git a/common/trajectories/BUILD.bazel b/common/trajectories/BUILD.bazel index dbfb344ef9a3..443ff2655ef6 100644 --- a/common/trajectories/BUILD.bazel +++ b/common/trajectories/BUILD.bazel @@ -19,6 +19,7 @@ drake_cc_package_library( ":discrete_time_trajectory", ":function_handle_trajectory", ":path_parameterized_trajectory", + ":piecewise_constant_curvature_trajectory", ":piecewise_polynomial", ":piecewise_pose", ":piecewise_quaternion", @@ -144,6 +145,20 @@ drake_cc_library( ], ) +drake_cc_library( + name = "piecewise_constant_curvature_trajectory", + srcs = ["piecewise_constant_curvature_trajectory.cc"], + hdrs = ["piecewise_constant_curvature_trajectory.h"], + deps = [ + ":piecewise_trajectory", + "//common:essential", + "//common:pointer_cast", + "//math:geometric_transform", + "//multibody/math:spatial_algebra", + "//systems/framework:system_scalar_converter", + ], +) + drake_cc_library( name = "piecewise_pose", srcs = ["piecewise_pose.cc"], @@ -285,6 +300,15 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "piecewise_constant_curvature_test", + deps = [ + ":piecewise_constant_curvature_trajectory", + ":piecewise_quaternion", + "//common/test_utilities:eigen_matrix_compare", + ], +) + drake_cc_googletest( name = "piecewise_polynomial_generation_test", # Test timeout increased to not timeout when run with Valgrind. diff --git a/common/trajectories/piecewise_constant_curvature_trajectory.cc b/common/trajectories/piecewise_constant_curvature_trajectory.cc new file mode 100644 index 000000000000..f34e463ac59c --- /dev/null +++ b/common/trajectories/piecewise_constant_curvature_trajectory.cc @@ -0,0 +1,167 @@ +#include "drake/common/trajectories/piecewise_constant_curvature_trajectory.h" + +#include +#include + +namespace drake { +namespace trajectories { + +template +PiecewiseConstantCurvatureTrajectory::PiecewiseConstantCurvatureTrajectory( + const std::vector& segment_breaks, + const std::vector& segment_curvatures, + const Vector3& curve_tangent_axis, const Vector3& plane_normal_axis) + : PiecewiseTrajectory(segment_breaks), + base_frame_(MakeBaseFrame(curve_tangent_axis, plane_normal_axis)), + segment_curvatures_(segment_curvatures), + segment_local_frames_(MakeSegmentLocalFrames( + base_frame_, segment_curvatures, segment_breaks)) {} + +template +std::unique_ptr> PiecewiseConstantCurvatureTrajectory::Clone() + const { + return std::make_unique>( + this->breaks(), segment_curvatures_, + base_frame_.col(kCurveTangentDimension), + base_frame_.col(kPlaneNormalDimension)); +} + +template +math::RigidTransform PiecewiseConstantCurvatureTrajectory::GetPose( + const T& distance) const { + int segment_index = this->get_segment_index(distance); + math::RigidTransform X_SiT = + CalcPoseInLocalFrame(segment_curvatures_[segment_index], + distance - this->start_time(segment_index)); + return segment_local_frames_[segment_index] * X_SiT; +} + +template +multibody::SpatialVelocity +PiecewiseConstantCurvatureTrajectory::GetVelocity(const T& distance) const { + math::RotationMatrix R_BT = GetPose(distance).rotation(); + T curvature = segment_curvatures_[this->get_segment_index(distance)]; + + multibody::SpatialVelocity spatial_velocity; + // Anuglar velocity is about plane normal axis. The curvature is defined + // as the ratio of the angular velocity to the linear velocity, and the + // latter is fixed to the unit vector along the curve tangent. + spatial_velocity.rotational() = R_BT.col(kPlaneNormalDimension) * curvature; + spatial_velocity.translational() = R_BT.col(kCurveTangentDimension); + return spatial_velocity; +} + +template +multibody::SpatialAcceleration +PiecewiseConstantCurvatureTrajectory::GetAcceleration( + const T& distance) const { + math::RotationMatrix R_BT = GetPose(distance).rotation(); + T curvature = segment_curvatures_[this->get_segment_index(distance)]; + + multibody::SpatialAcceleration spatial_acceleration; + // As the trajectory has piecewise constant curvature, it has piecewise + // constant angular velocity and thus zero angular acceleration almost + // everywhere. The linear acceleration is simply the centripetal + // accleration, which points along the curve normal axis. + spatial_acceleration.rotational() = Vector3::Zero(); + spatial_acceleration.translational() = + R_BT.col(kCurveNormalDimension) * curvature; + return spatial_acceleration; +} + +template +boolean PiecewiseConstantCurvatureTrajectory::IsNearlyPeriodic( + double tolerance) const { + return GetPose(this->start_time()) + .IsNearlyEqualTo(GetPose(this->end_time()), tolerance); +} + +template +math::RigidTransform +PiecewiseConstantCurvatureTrajectory::CalcPoseInLocalFrame( + const T& curvature, const T& length) { + Vector3 translation = Vector3::Zero(); + // Calculate rotation angle + T theta = length * curvature; + math::RotationMatrix rotation = + math::RotationMatrix::MakeZRotation(theta); + if (curvature == T(0)) { + // Casel 1: zero curvature (straight line) + translation(kCurveTangentDimension) = length; + } else { + // Case 2: non-zero curvature (circular arc) + // Calculate position on the circular arc, which + // is about the centerpoint 1/curvature * normal_axis with + // radius 1/curvature + translation(kCurveNormalDimension) = (T(1) - cos(theta)) / curvature; + translation(kCurveTangentDimension) = sin(theta) / curvature; + } + return math::RigidTransform(rotation, translation); +} + +template +math::RotationMatrix PiecewiseConstantCurvatureTrajectory::MakeBaseFrame( + const Vector3& curve_tangent_axis, const Vector3& plane_normal_axis) { + double kEpsilon = std::sqrt(std::numeric_limits::epsilon()); + // NOTE: assumes right hand rule for tangent \times normal = plane + const bool right_hand = + (kPlaneNormalDimension + 1) % 3 == kCurveTangentDimension; + DRAKE_DEMAND(right_hand); + const Vector3 normal_axis = plane_normal_axis.cross(curve_tangent_axis); + + DRAKE_DEMAND(curve_tangent_axis.norm() >= kEpsilon); + DRAKE_DEMAND(normal_axis.norm() >= kEpsilon); + DRAKE_DEMAND(plane_normal_axis.norm() >= kEpsilon); + Matrix3 base_frame_matrix = Matrix3::Zero(); + + // Fill in the rotation matrix + base_frame_matrix.col(kCurveNormalDimension) = normal_axis.normalized(); + base_frame_matrix.col(kCurveTangentDimension) = + curve_tangent_axis.normalized(); + base_frame_matrix.col(kPlaneNormalDimension) = plane_normal_axis.normalized(); + return math::RotationMatrix(base_frame_matrix); +} + +template +std::vector> +PiecewiseConstantCurvatureTrajectory::MakeSegmentLocalFrames( + const math::RotationMatrix& base_frame, + const std::vector& segment_curvatures, + const std::vector& segment_breaks) { + size_t num_breaks = segment_breaks.size(); + size_t num_segments = num_breaks - 1; + + // calculate angular change and length of each segment. + const VectorX segment_breaks_eigen = + Eigen::Map>(segment_breaks.data(), num_breaks); + const VectorX curvatures_eigen = + Eigen::Map>(segment_curvatures.data(), num_segments); + const VectorX segment_lengths = segment_breaks_eigen.tail(num_segments) - + segment_breaks_eigen.head(num_segments); + const VectorX segment_rotation_angles = + segment_lengths.cwiseProduct(curvatures_eigen); + VectorX segment_cumulative_rotations = VectorX::Zero(num_breaks); + std::partial_sum(segment_rotation_angles.begin(), + segment_rotation_angles.end(), + segment_cumulative_rotations.begin() + 1); + + // build frames for the start of each segment. + std::vector> segment_local_frames; + segment_local_frames.reserve(num_segments); + segment_local_frames.push_back(math::RigidTransform(base_frame)); + + for (size_t i = 0; i < num_segments; i++) { + math::RigidTransform X_SprevSi = + CalcPoseInLocalFrame(segment_curvatures[i], segment_lengths[i]); + segment_local_frames.push_back(segment_local_frames.back() * X_SprevSi); + } + + return segment_local_frames; +} + +// Explicit instantiation for the types specified in the header +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::trajectories::PiecewiseConstantCurvatureTrajectory); + +} // namespace trajectories +} // namespace drake diff --git a/common/trajectories/piecewise_constant_curvature_trajectory.h b/common/trajectories/piecewise_constant_curvature_trajectory.h new file mode 100644 index 000000000000..d59238d318b7 --- /dev/null +++ b/common/trajectories/piecewise_constant_curvature_trajectory.h @@ -0,0 +1,211 @@ +#pragma once + +#include +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/common/trajectories/piecewise_trajectory.h" +#include "drake/math/rigid_transform.h" +#include "drake/multibody/math/spatial_algebra.h" +#include "drake/systems/framework/scalar_conversion_traits.h" + +namespace drake { +namespace trajectories { + +/** + * Represents a piecewise constant-curvature trajectory within a plane embdedded + * in 3D space. + * + * This class defines a trajectory composed of segments with constant curvature: + * circular arcs and line segments. It provides methods to calcualte poses, + * velocities, and accelerations along the trajectory based on the distance + * traveled. + * + * The curve is path-length paramaterized; as such the linear velocity is always + * a unit vector. + * + * @tparam_default_scalar + */ +template +class PiecewiseConstantCurvatureTrajectory final + : public PiecewiseTrajectory { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PiecewiseConstantCurvatureTrajectory); + + /** + * Constructs an empty piecewise constant curvature trajectory. + */ + PiecewiseConstantCurvatureTrajectory() {} + + /** + * Clones this trajectory to a trajectory with a different scalar type. + * + * @tparam ToScalar The scalar type to which to clone the trajectory. + * @return The cloned trajectory. + */ + template + PiecewiseConstantCurvatureTrajectory CloneToScalar() const { + std::vector segment_curvatures; + std::vector segment_breaks; + systems::scalar_conversion::ValueConverter converter; + for (const T& curvature : segment_curvatures_) { + segment_curvatures.push_back(converter(curvature)); + } + for (const T& segment_break : this->breaks()) { + segment_breaks.push_back(converter(segment_break)); + } + return PiecewiseConstantCurvatureTrajectory( + segment_breaks, segment_curvatures, + base_frame_.col(kCurveTangentDimension).template cast(), + base_frame_.col(kPlaneNormalDimension).template cast()); + } + + /** + * Constructs a piecewise constant curvature trajectory. + * + * @param segment_breaks A vector of distance break points between segments. + * @param segment_curvatures A vector of curvatures for each segment. + * @param curve_tangent_axis The tangent of the curve at path length 0. + * @param plane_normal_axis The normal axis of the 2D plane in which the curve + * lies. + */ + PiecewiseConstantCurvatureTrajectory(const std::vector& segment_breaks, + const std::vector& segment_curvatures, + const Vector3& curve_tangent_axis, + const Vector3& plane_normal_axis); + + /** + * @return The number of rows in the trajectory's output. + */ + Eigen::Index rows() const override { return 4; } + + /** + * @return The number of columns in the trajectory's output. + */ + Eigen::Index cols() const override { return 4; } + + /** + * Returns the interpolated pose at the given distance along the trajectory. + * + * @param distance The distance along the trajectory. + * @return The interpolated pose as a RigidTransform. + */ + math::RigidTransform GetPose(const T& distance) const; + + /** + * Returns the interpolated pose at the given distance along the trajectory as + * an Eigen matrix. + * + * @param t The distance along the trajectory. + * @return The 4x4 homogeneous transform matrix representing the pose. + */ + MatrixX value(const T& t) const override { + return GetPose(t).GetAsMatrix4(); + } + + /** + * Creates a deep copy of this trajectory. + * + * @return A unique pointer to the cloned trajectory. + */ + std::unique_ptr> Clone() const override; + + /** + * Returns the interpolated spatial velocity at the given distance along the + * trajectory. + * + * @param distance The distance along the trajectory. + * @return The interpolated spatial velocity. + */ + multibody::SpatialVelocity GetVelocity(const T& distance) const; + + /** + * Returns the interpolated spatial acceleration at the given distance along + * the trajectory. + * + * @param distance The distance along the trajectory. + * @return The interpolated spatial acceleration. + */ + multibody::SpatialAcceleration GetAcceleration(const T& distance) const; + + /** + * Checks if the trajectory is nearly periodic within the given tolerance. + * + * Periodicity is defined as the beginning and end poses being equal up to the + * same tolerance. + * + * @param tolerance The tolerance for periodicity check. + * @return True if the trajectory is nearly periodic, false otherwise. + */ + boolean IsNearlyPeriodic(double tolerance) const; + + /** + * Returns the frame of the trajectory at distance zero, which defines + * the orientation of the plane in the embedded 3D space. + * + * @return The base frame as a RotationMatrix. + */ + const math::RotationMatrix& get_base_frame() const { return base_frame_; } + + /** + * Returns the normal axis of the plane in which the curve lies + * + * @return The plane normal axis. + */ + const Eigen::Ref> get_plane_normal_axis() const { + return base_frame_.col(kPlaneNormalDimension); + } + + private: + /** + * Calculates the relative transform between the start of a constant-curavtue + * segment and the pose at a given length of travel within the segment. + * + * @param curvature The curvature of the segment. + * @param length The length of travel in the segment. + * @return The pose relative to length 0 along the curve. + */ + static math::RigidTransform CalcPoseInLocalFrame(const T& curvature, + const T& length); + + /** + * Builds the base frame from the given tangent and plane axes. + * + * @param curve_tangent_axis The tangent of the curve at distance 0. + * @param plane_normal_axis The normal axis of the plane. + * + * @return The base frame as a RotationMatrix. + */ + static math::RotationMatrix MakeBaseFrame( + const Vector3& curve_tangent_axis, + const Vector3& plane_normal_axis); + + /** + * Builds the curve from the given curvatures and segment breaks. + * + * @param base_frame The base frame of the trajectory. + * @param segment_curvatures A vector of curvatures for each segment. + * @param segment_breaks A vector of break points between segments. + * + * @return The local frames of each segment. + */ + static std::vector> MakeSegmentLocalFrames( + const math::RotationMatrix& base_frame, + const std::vector& segment_curvatures, + const std::vector& segment_breaks); + + math::RotationMatrix base_frame_; + std::vector segment_curvatures_; + std::vector> segment_local_frames_; + + static inline constexpr size_t kCurveTangentDimension = 0; + static inline constexpr size_t kCurveNormalDimension = 1; + static inline constexpr size_t kPlaneNormalDimension = 2; +}; + +} // namespace trajectories +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::trajectories::PiecewiseConstantCurvatureTrajectory); diff --git a/common/trajectories/test/piecewise_constant_curvature_test.cc b/common/trajectories/test/piecewise_constant_curvature_test.cc new file mode 100644 index 000000000000..b574117a5aca --- /dev/null +++ b/common/trajectories/test/piecewise_constant_curvature_test.cc @@ -0,0 +1,259 @@ +#include + +#include +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/trajectories/piecewise_constant_curvature_trajectory.h" +#include "drake/common/trajectories/piecewise_polynomial.h" + +using Eigen::Vector3d; +using Eigen::VectorXd; + +static double kTolerance = std::sqrt(std::numeric_limits::epsilon()); + +namespace drake { +namespace trajectories { +namespace { + +// Generates a vector of random orientation using randomized axis angles. +template +std::vector GenerateRandomCurvatures(std::vector times, + std::default_random_engine* generator, + double max_angle = (10 * M_PI)) { + int size = times.size() - 1; + DRAKE_DEMAND(size >= 0); + std::vector curavtures(size); + std::uniform_real_distribution travel_angle(-max_angle, max_angle); + std::bernoulli_distribution is_linear(0.25); + for (int i = 0; i < size; ++i) { + double duration = times[i + 1] - times[i]; + if (is_linear(*generator)) { + curavtures[i] = T(0); + } else { + // pick a curvature so the angle change on the segment is in (-pi, pi) + curavtures[i] = T(travel_angle(*generator) / duration); + } + } + return curavtures; +} + +template +Quaternion GenerateRandomQuaternion( + std::default_random_engine* generator) { + std::uniform_real_distribution uniform(-10, 10); + Quaternion ret; + return Quaternion(AngleAxis( + uniform(*generator), + Vector3(uniform(*generator), uniform(*generator), + uniform(*generator)))); +} + +GTEST_TEST(TestPiecewiseQuaternionSlerp, TestAnalytical) { + double r = 2; + double kappa = 1 / r; + int num_segments = 10000; + std::vector segment_breaks(num_segments + 1); + std::vector curvatures(num_segments); + segment_breaks[0] = 0; + for (int i = 0; i < num_segments; ++i) { + segment_breaks[i + 1] = 2 * M_PI * r * (i + 1.) / num_segments; + curvatures[i] = kappa; + } + Vector3d plane_normal = Vector3d::UnitZ(); + Vector3d curve_tangent = Vector3d::UnitX(); + PiecewiseConstantCurvatureTrajectory trajectory( + segment_breaks, curvatures, curve_tangent, plane_normal); + + for (double l = segment_breaks.front(); l < segment_breaks.back(); + l += 0.01) { + math::RigidTransformd expected_pose = math::RigidTransformd( + math::RotationMatrixd::MakeZRotation(l * kappa), + Vector3d(r * sin(l * kappa), r * (1 - cos(l * kappa)), 0)); + multibody::SpatialVelocity expected_velocity( + kappa * Vector3d::UnitZ(), Vector3d(cos(l * kappa), sin(l * kappa), 0)); + multibody::SpatialAcceleration expected_acceleration( + Vector3d::Zero(), + Vector3d(-kappa * sin(l * kappa), kappa * cos(l * kappa), 0)); + + auto actual_pose = trajectory.GetPose(l); + auto actual_velocity = trajectory.GetVelocity(l); + auto actual_acceleration = trajectory.GetAcceleration(l); + + EXPECT_TRUE(actual_pose.IsNearlyEqualTo(expected_pose, kTolerance)); + EXPECT_TRUE(actual_velocity.IsApprox(expected_velocity, kTolerance)); + EXPECT_TRUE( + actual_acceleration.IsApprox(expected_acceleration, kTolerance)); + } +} + +// Tests CheckSlerpInterpolation for PiecewiseQuaternionSlerp +// generated from random breaks and samples. +GTEST_TEST(TestPiecewiseConstantCuravatureTrajectory, + TestRandomizedTrajectory) { + std::default_random_engine generator(123); + int num_segments = 10000; + std::vector breaks = + PiecewiseTrajectory::RandomSegmentTimes(num_segments, generator); + std::vector curves = + GenerateRandomCurvatures(breaks, &generator); + VectorXd segment_times_vector = + Eigen::Map(breaks.data(), breaks.size()); + VectorXd segment_curves_vector = + Eigen::Map(curves.data(), curves.size()); + VectorXd segment_durations = segment_times_vector.tail(num_segments) - + segment_times_vector.head(num_segments); + VectorXd segment_angles = + segment_curves_vector.cwiseProduct(segment_durations); + std::vector> cumulative_angles(num_segments + 1); + cumulative_angles[0] = MatrixX::Zero(1, 1); + for (int i = 1; i < num_segments + 1; ++i) { + cumulative_angles[i] = + cumulative_angles[i - 1] + Vector1d(segment_angles[i - 1]); + } + auto initial_quat = GenerateRandomQuaternion(&generator); + auto initial_rotation = drake::math::RotationMatrixd(initial_quat); + Eigen::Vector3d initial_plane_normal = initial_rotation.col(2); + Eigen::Vector3d initial_curve_tangent = initial_rotation.col(0); + + auto angle_spline = + PiecewisePolynomial::FirstOrderHold(breaks, cumulative_angles); + PiecewiseConstantCurvatureTrajectory curve_spline( + breaks, curves, initial_curve_tangent, initial_plane_normal); + auto initial_position = curve_spline.GetPose(breaks.front()).translation(); + // Check dense interpolated quaternions. + for (double l = breaks.front(); l < breaks.back(); l += 0.01) { + auto curve_pose = curve_spline.GetPose(l); + auto curve_rotation = curve_pose.rotation(); + auto curve_position = curve_pose.translation(); + + auto curve_velocity = curve_spline.GetVelocity(l); + auto translational_velocity = curve_velocity.translational(); + auto rotational_velocity = curve_velocity.rotational(); + + auto curve_acceleration = curve_spline.GetAcceleration(l); + auto translational_acceleration = curve_acceleration.translational(); + auto rotational_acceleration = curve_acceleration.rotational(); + + // Rotation should just be cumulative angle rotation about plane axis + auto relative_rotation_expected = drake::math::RotationMatrixd( + AngleAxis(angle_spline.value(l)(0, 0), initial_plane_normal)); + auto curve_rotation_expected = + relative_rotation_expected * initial_rotation; + EXPECT_TRUE( + curve_rotation.IsNearlyEqualTo(curve_rotation_expected, kTolerance)); + + // Displacement should be within plane + auto displacement = curve_position - initial_position; + EXPECT_NEAR(displacement.dot(initial_plane_normal), 0, kTolerance); + + // Translational velocity should be in-plane and unit norm + EXPECT_NEAR(translational_velocity.dot(initial_plane_normal), 0, + kTolerance); + EXPECT_NEAR(translational_velocity.norm(), 1, kTolerance); + + // Rotational velocity should be normal to plane + EXPECT_NEAR(std::abs(rotational_velocity.dot(initial_plane_normal)), + rotational_velocity.norm(), kTolerance); + + // Translational acceleration should in-plane and orthogonal to velocity + EXPECT_NEAR(translational_acceleration.dot(initial_plane_normal), 0, + kTolerance); + EXPECT_NEAR(translational_acceleration.dot(translational_velocity), 0, + kTolerance); + + // Rotational acceleration should be 0 almost everywhere + EXPECT_NEAR(rotational_acceleration.dot(initial_plane_normal), 0, + kTolerance); + } +} + +GTEST_TEST(TestPiecewiseConstantCuravatureTrajectory, TestPeriodicity) { + double r = 2; + double kappa = 1 / r; + int num_segments = 10000; + std::vector segment_breaks_periodic(num_segments + 1); + std::vector segment_breaks_aperiodic(num_segments + 1); + std::vector curvatures(num_segments); + + segment_breaks_periodic[0] = 0; + segment_breaks_aperiodic[0] = 0; + + for (int i = 0; i < num_segments; ++i) { + segment_breaks_periodic[i + 1] = 2 * M_PI * r * (i + 1.) / num_segments; + segment_breaks_aperiodic[i + 1] = M_PI * r * (i + 1.) / num_segments; + curvatures[i] = kappa; + } + Vector3d plane_normal = Vector3d::UnitZ(); + Vector3d curve_tangent = Vector3d::UnitX(); + PiecewiseConstantCurvatureTrajectory periodic_trajectory( + segment_breaks_periodic, curvatures, curve_tangent, plane_normal); + + PiecewiseConstantCurvatureTrajectory aperiodic_trajectory( + segment_breaks_aperiodic, curvatures, curve_tangent, plane_normal); + + EXPECT_TRUE(periodic_trajectory.IsNearlyPeriodic(kTolerance)); + EXPECT_FALSE(aperiodic_trajectory.IsNearlyPeriodic(kTolerance)); +} + +GTEST_TEST(TestPiecewiseConstantCuravatureTrajectory, TestScalarConversion) { + std::vector times{1, 2, 3, 4}; + std::vector curavtures{-1, 0, 1}; + Vector3d plane_normal = Vector3d::UnitZ(); + Vector3d curve_tangent = Vector3d::UnitX(); + PiecewiseConstantCurvatureTrajectory double_trajectory( + times, curavtures, curve_tangent, plane_normal); + PiecewiseConstantCurvatureTrajectory autodiff_trajectory = + double_trajectory.template CloneToScalar(); + PiecewiseConstantCurvatureTrajectory + expression_trajectory = + double_trajectory.template CloneToScalar(); + + for (double l = times.front(); l < times.back(); l += 0.01) { + math::RigidTransform double_pose = double_trajectory.GetPose(l); + math::RigidTransform autodiff_pose = + double_pose.template cast(); + math::RigidTransform expression_pose = + double_pose.template cast(); + + multibody::SpatialVelocity double_velocity = + double_trajectory.GetVelocity(l); + multibody::SpatialVelocity autodiff_velocity( + double_velocity.rotational().template cast(), + double_velocity.translational().template cast()); + multibody::SpatialVelocity expression_velocity( + double_velocity.rotational().template cast(), + double_velocity.translational().template cast()); + + multibody::SpatialAcceleration double_acceleration = + double_trajectory.GetAcceleration(l); + multibody::SpatialAcceleration autodiff_acceleration( + double_acceleration.rotational().template cast(), + double_acceleration.translational().template cast()); + multibody::SpatialAcceleration + expression_acceleration(double_acceleration.rotational() + .template cast(), + double_acceleration.translational() + .template cast()); + + EXPECT_TRUE(autodiff_pose.IsNearlyEqualTo(autodiff_trajectory.GetPose(l), + kTolerance)); + EXPECT_TRUE(expression_pose.IsNearlyEqualTo( + expression_trajectory.GetPose(l), kTolerance)); + + EXPECT_TRUE(autodiff_velocity.IsApprox(autodiff_trajectory.GetVelocity(l), + kTolerance)); + EXPECT_TRUE(expression_velocity.IsApprox( + expression_trajectory.GetVelocity(l), kTolerance)); + + EXPECT_TRUE(autodiff_acceleration.IsApprox( + autodiff_trajectory.GetAcceleration(l), kTolerance)); + EXPECT_TRUE(expression_acceleration.IsApprox( + expression_trajectory.GetAcceleration(l), kTolerance)); + } +} + +} // namespace +} // namespace trajectories +} // namespace drake From 6762785fefec7af0df562d17de8f67c087dca508 Mon Sep 17 00:00:00 2001 From: Mathew Halm Date: Fri, 29 Nov 2024 13:23:25 -0800 Subject: [PATCH 2/2] address comments on PR rev 1 --- ...piecewise_constant_curvature_trajectory.cc | 213 +++++++------ .../piecewise_constant_curvature_trajectory.h | 283 ++++++++++++------ .../test/piecewise_constant_curvature_test.cc | 29 +- 3 files changed, 323 insertions(+), 202 deletions(-) diff --git a/common/trajectories/piecewise_constant_curvature_trajectory.cc b/common/trajectories/piecewise_constant_curvature_trajectory.cc index f34e463ac59c..edafd313447a 100644 --- a/common/trajectories/piecewise_constant_curvature_trajectory.cc +++ b/common/trajectories/piecewise_constant_curvature_trajectory.cc @@ -8,64 +8,86 @@ namespace trajectories { template PiecewiseConstantCurvatureTrajectory::PiecewiseConstantCurvatureTrajectory( - const std::vector& segment_breaks, - const std::vector& segment_curvatures, - const Vector3& curve_tangent_axis, const Vector3& plane_normal_axis) - : PiecewiseTrajectory(segment_breaks), - base_frame_(MakeBaseFrame(curve_tangent_axis, plane_normal_axis)), - segment_curvatures_(segment_curvatures), - segment_local_frames_(MakeSegmentLocalFrames( - base_frame_, segment_curvatures, segment_breaks)) {} + const std::vector& breaks, const std::vector& turning_rates, + const Vector3& initial_curve_tangent, const Vector3& plane_normal) + : PiecewiseTrajectory(breaks), + segment_turning_rates_(turning_rates), + segment_start_poses_(MakeSegmentStartPoses( + MakeBaseFrame(initial_curve_tangent, plane_normal), turning_rates, + breaks)) { + if (turning_rates.size() != breaks.size() - 1) { + throw std::invalid_argument( + "The number of turning rates must be equal to the number of segments."); + } +} template std::unique_ptr> PiecewiseConstantCurvatureTrajectory::Clone() const { + auto base_frame = segment_start_poses_[0].rotation(); return std::make_unique>( - this->breaks(), segment_curvatures_, - base_frame_.col(kCurveTangentDimension), - base_frame_.col(kPlaneNormalDimension)); + this->breaks(), segment_turning_rates_, + base_frame.col(kCurveTangentIndex), base_frame.col(kPlaneNormalIndex)); } template math::RigidTransform PiecewiseConstantCurvatureTrajectory::GetPose( - const T& distance) const { - int segment_index = this->get_segment_index(distance); - math::RigidTransform X_SiT = - CalcPoseInLocalFrame(segment_curvatures_[segment_index], - distance - this->start_time(segment_index)); - return segment_local_frames_[segment_index] * X_SiT; + const T& t) const { + int segment_index = this->get_segment_index(t); + const math::RigidTransform X_FiF = + CalcRelativePoseInSegment(segment_turning_rates_[segment_index], + t - this->start_time(segment_index)); + return segment_start_poses_[segment_index] * X_FiF; } template multibody::SpatialVelocity -PiecewiseConstantCurvatureTrajectory::GetVelocity(const T& distance) const { - math::RotationMatrix R_BT = GetPose(distance).rotation(); - T curvature = segment_curvatures_[this->get_segment_index(distance)]; +PiecewiseConstantCurvatureTrajectory::CalcSpatialVelocity(const T& t) const { + const math::RotationMatrix R_AF = GetPose(t).rotation(); + const T& k_i = segment_turning_rates_[this->get_segment_index(t)]; multibody::SpatialVelocity spatial_velocity; - // Anuglar velocity is about plane normal axis. The curvature is defined - // as the ratio of the angular velocity to the linear velocity, and the - // latter is fixed to the unit vector along the curve tangent. - spatial_velocity.rotational() = R_BT.col(kPlaneNormalDimension) * curvature; - spatial_velocity.translational() = R_BT.col(kCurveTangentDimension); + // The linear speed ds/dt along the curve is always defined to be 1 [m/s]. + // The curvature is defined to be abs(kᵢ). + // + // From Frenet-Serret analysis and the class doc for + // PiecewiseConstantCurvatureTrajectory, the rotational velocity is equal to + // the Darboux vector ds/dt * curvature * b̂ = kᵢ * ẑ_F_A. + spatial_velocity.rotational() = k_i * R_AF.col(kPlaneNormalIndex); + + // The translational velocity is also then equal to ds/dt * t̂ = x̂_F_A(t). + spatial_velocity.translational() = R_AF.col(kCurveTangentIndex); + return spatial_velocity; } template multibody::SpatialAcceleration -PiecewiseConstantCurvatureTrajectory::GetAcceleration( - const T& distance) const { - math::RotationMatrix R_BT = GetPose(distance).rotation(); - T curvature = segment_curvatures_[this->get_segment_index(distance)]; +PiecewiseConstantCurvatureTrajectory::CalcSpatialAcceleration( + const T& t) const { + const math::RotationMatrix R_AF = GetPose(t).rotation(); + const T& k_i = segment_turning_rates_[this->get_segment_index(t)]; multibody::SpatialAcceleration spatial_acceleration; - // As the trajectory has piecewise constant curvature, it has piecewise - // constant angular velocity and thus zero angular acceleration almost - // everywhere. The linear acceleration is simply the centripetal - // accleration, which points along the curve normal axis. + // As ds/dt = 1 [m/s], we have ds²/dt² = 0. + // Thus, there is no azumithal/transverse/Euler acceleration component. + // + // As the curve does not have continuous acceleration at the break times, by + // convention we set the acceleration at the break time tᵢ to be the limit + // as approached from the right -- i.e. the acceleration is continuous on each + // segoment domain [tᵢ, tᵢ₊₁) (see implementation of + // PiecewiseTrajectory::get_segment_index for convention on the segment index + // of tᵢ). + // + // From the Frenet-Serrat analysis in the class doc for + // PiecewiseConstantCurvatureTrajectory, we know that the angular velocity is + // kᵢ * ẑ_F_A. As ẑ_F_A and kᵢ are constant everywhere except the break times, + // the angular acceleration is then equal to zero. spatial_acceleration.rotational() = Vector3::Zero(); - spatial_acceleration.translational() = - R_BT.col(kCurveNormalDimension) * curvature; + + // We also know that the translational velocity is x̂_F_A and thus the + // translational acceleration is dx̂_F_A/dt = kᵢ * ŷ_F_A(t). + spatial_acceleration.translational() = k_i * R_AF.col(kCurveNormalIndex); return spatial_acceleration; } @@ -78,85 +100,86 @@ boolean PiecewiseConstantCurvatureTrajectory::IsNearlyPeriodic( template math::RigidTransform -PiecewiseConstantCurvatureTrajectory::CalcPoseInLocalFrame( - const T& curvature, const T& length) { - Vector3 translation = Vector3::Zero(); +PiecewiseConstantCurvatureTrajectory::CalcRelativePoseInSegment( + const T& k_i, const T& dt) { + Vector3 p_FioFo_Fi = Vector3::Zero(); // Calculate rotation angle - T theta = length * curvature; - math::RotationMatrix rotation = - math::RotationMatrix::MakeZRotation(theta); - if (curvature == T(0)) { - // Casel 1: zero curvature (straight line) - translation(kCurveTangentDimension) = length; + const T theta = dt * k_i; + math::RotationMatrix R_FiF = math::RotationMatrix::MakeZRotation(theta); + if (k_i == T(0)) { + // Case 1: zero curvature (straight line) + // + // The tangent axis is constant, and the tangential speed is 1 [m/s]. + // Thus the displacement p_FioFo_Fi is just t̂ * Δt. + p_FioFo_Fi(kCurveTangentIndex) = dt; } else { // Case 2: non-zero curvature (circular arc) - // Calculate position on the circular arc, which - // is about the centerpoint 1/curvature * normal_axis with - // radius 1/curvature - translation(kCurveNormalDimension) = (T(1) - cos(theta)) / curvature; - translation(kCurveTangentDimension) = sin(theta) / curvature; + // + // The entire trajectory lies in a plan with normal ẑ, and thus + // the arc is embedded in the x-y plane. + // + // ŷ_Fᵢ + // ↑ + // C o x + // │\ x + // │ \ x + // │ \ x + // │ \ x + // │ \ x + // │ \ xx + // │ p_Fₒ ox + // │ xxx + // └xxx───────────────→ x̂_Fᵢ + // p_Fᵢₒ + // + // The circular arc's centerpoint C is located at p_FᵢₒC = (1/kᵢ) * ŷ_Fᵢ, + // with initial direction x̂_Fᵢ, radius abs(1/kᵢ), and constant tagential + // speed 1 [m/s]. Thus the angle traveled along the arc is θ = kᵢ * Δt, + // with the sign of kᵢ handeling the clockwise/counterclockwise direction. + // The kᵢ > 0 case is shown above. + p_FioFo_Fi(kCurveNormalIndex) = (T(1) - cos(theta)) / k_i; + p_FioFo_Fi(kCurveTangentIndex) = sin(theta) / k_i; } - return math::RigidTransform(rotation, translation); + return math::RigidTransform(R_FiF, p_FioFo_Fi); } template math::RotationMatrix PiecewiseConstantCurvatureTrajectory::MakeBaseFrame( - const Vector3& curve_tangent_axis, const Vector3& plane_normal_axis) { - double kEpsilon = std::sqrt(std::numeric_limits::epsilon()); - // NOTE: assumes right hand rule for tangent \times normal = plane - const bool right_hand = - (kPlaneNormalDimension + 1) % 3 == kCurveTangentDimension; - DRAKE_DEMAND(right_hand); - const Vector3 normal_axis = plane_normal_axis.cross(curve_tangent_axis); - - DRAKE_DEMAND(curve_tangent_axis.norm() >= kEpsilon); - DRAKE_DEMAND(normal_axis.norm() >= kEpsilon); - DRAKE_DEMAND(plane_normal_axis.norm() >= kEpsilon); - Matrix3 base_frame_matrix = Matrix3::Zero(); - - // Fill in the rotation matrix - base_frame_matrix.col(kCurveNormalDimension) = normal_axis.normalized(); - base_frame_matrix.col(kCurveTangentDimension) = - curve_tangent_axis.normalized(); - base_frame_matrix.col(kPlaneNormalDimension) = plane_normal_axis.normalized(); - return math::RotationMatrix(base_frame_matrix); + const Vector3& initial_curve_tangent, const Vector3& plane_normal) { + const Vector3 initial_y_hat_F_A = + plane_normal.cross(initial_curve_tangent); + + return math::RotationMatrix::MakeFromOrthonormalColumns( + initial_curve_tangent.normalized(), initial_y_hat_F_A.normalized(), + plane_normal.normalized()); } template std::vector> -PiecewiseConstantCurvatureTrajectory::MakeSegmentLocalFrames( +PiecewiseConstantCurvatureTrajectory::MakeSegmentStartPoses( const math::RotationMatrix& base_frame, - const std::vector& segment_curvatures, - const std::vector& segment_breaks) { - size_t num_breaks = segment_breaks.size(); - size_t num_segments = num_breaks - 1; + const std::vector& turning_rates, const std::vector& breaks) { + const size_t num_breaks = breaks.size(); + const size_t num_segments = num_breaks - 1; // calculate angular change and length of each segment. - const VectorX segment_breaks_eigen = - Eigen::Map>(segment_breaks.data(), num_breaks); - const VectorX curvatures_eigen = - Eigen::Map>(segment_curvatures.data(), num_segments); - const VectorX segment_lengths = segment_breaks_eigen.tail(num_segments) - - segment_breaks_eigen.head(num_segments); - const VectorX segment_rotation_angles = - segment_lengths.cwiseProduct(curvatures_eigen); - VectorX segment_cumulative_rotations = VectorX::Zero(num_breaks); - std::partial_sum(segment_rotation_angles.begin(), - segment_rotation_angles.end(), - segment_cumulative_rotations.begin() + 1); + const VectorX breaks_eigen = + Eigen::Map>(breaks.data(), num_breaks); + const VectorX segment_durations = + breaks_eigen.tail(num_segments) - breaks_eigen.head(num_segments); // build frames for the start of each segment. - std::vector> segment_local_frames; - segment_local_frames.reserve(num_segments); - segment_local_frames.push_back(math::RigidTransform(base_frame)); - - for (size_t i = 0; i < num_segments; i++) { - math::RigidTransform X_SprevSi = - CalcPoseInLocalFrame(segment_curvatures[i], segment_lengths[i]); - segment_local_frames.push_back(segment_local_frames.back() * X_SprevSi); + std::vector> segment_start_poses; + segment_start_poses.reserve(num_segments); + segment_start_poses.push_back(math::RigidTransform(base_frame)); + + for (size_t i = 0; i < (num_segments - 1); i++) { + math::RigidTransform X_FiFip1 = + CalcRelativePoseInSegment(turning_rates[i], segment_durations[i]); + segment_start_poses.push_back(segment_start_poses.back() * X_FiFip1); } - return segment_local_frames; + return segment_start_poses; } // Explicit instantiation for the types specified in the header diff --git a/common/trajectories/piecewise_constant_curvature_trajectory.h b/common/trajectories/piecewise_constant_curvature_trajectory.h index d59238d318b7..27ad071e3cc7 100644 --- a/common/trajectories/piecewise_constant_curvature_trajectory.h +++ b/common/trajectories/piecewise_constant_curvature_trajectory.h @@ -14,16 +14,60 @@ namespace drake { namespace trajectories { /** - * Represents a piecewise constant-curvature trajectory within a plane embdedded - * in 3D space. + * Represents a piecewise-constant-curvature, continuously-differentiable + * trajectory within a 2D plane embedded in 3D space, i.e. a trajectory composed + * of circular arcs and line segments. * - * This class defines a trajectory composed of segments with constant curvature: - * circular arcs and line segments. It provides methods to calcualte poses, - * velocities, and accelerations along the trajectory based on the distance - * traveled. + * This class tracks X_AF(t), a pose F relative to a reference frame A as a + * funciton of time t [s], with position r(t) = p_AoFo_A and R(t) = R_AF. The + * initial position r(t₀) is assumed to be equal to the zero vector. r(t) is the + * nominal value of the trajectory, and the full pose and its derivatives are + * available through CalcPose(), CalcSpatialVelocity(), and + * CalcSpatialAcceleration(). * - * The curve is path-length paramaterized; as such the linear velocity is always - * a unit vector. + * The trajectory is arclength-parameterized; this means velocity along the + * curve tangent is always 1 [m/s], and t̂(t) = dr(t)/dt is the curve's tangent + * unit vector (in the Frenet-Serrat sence). + * + * The plane has a normal axis p̂. The curvatures are defined piecewise + * leveraging the "segment times" concept from PiecewiseTrajectory. The segment + * break times t₀ [s] < t₁ [s] < ... < tₙ [s] define the finite list of + * points at which the curvature instantly changes. Like all PiecewiseTrajectory + * types, tᵢ - tᵢ₋₁ must be at least PiecewiseTrajectory::kEpsilonTime. On + * "segment" i, defined as [tᵢ₋₁, tᵢ], the curvature is parameterized by a + * constant "turning rate" kᵢ [1/m], setting the angular velocity of F in W + * (equivalently the Darboux vector) as w_AF = kᵢ * p̂, and the curvature is + * equal to the magnitude of the turning rate abs(k). Thus, + * + * * kᵢ > 0 corresponds to a counterclockwise turn along a circular arc of + * radius 1/kᵢ [m]. + * * kᵢ = 0 corresponds to a straight line segment. + * * kᵢ < 0 corresponds to a clockwise turn along a circular arc of radius + * -1/kᵢ [m]. + * + * The angular velocity w_AF is finite and constant within the segments. Thus + * the angular acceleration alpha_AF is zero within the segments and undefined + * at the segment endpoints. R(t) is continuous. + * + * For brevity, we refer to the pose at the start of each segment as Fᵢ, and + * the associated transfrom X_AF(tᵢ) as X_AFᵢ. + * + * Before t₀ and after tₙ, the pose, velocity, and acceleration are + * continuously extrapolated with constant curvatures k_1 and k_N respectively. + * + * The orientation of the curve R(t) = [x̂_F_A(t), ŷ_F_A(t), ẑ_F_A(t)] rotates + * with the direction of the curve, similar to but distinct from the + * Frenet-Serret/tangent-normal-binormal (TNB) frame [t̂, n̂, b̂]: + * + * * x̂_F_A(t) = dr(t)/dt = t̂ is the curve tangent at time t + * * ẑ_F_A(t) = ẑ_F_A(0) = p̂, the plane normal. ẑ = sign(kᵢ) * b̂. + * * ŷ_F_A(t) = ẑ_F_A x x̂_F_A = sign(k) * n̂. + * + * From the Frenet-Serret formulas, we know: + * + * * dx̂_F_A(t)/dt = kᵢ * ŷ_F_A(t) + * * dŷ_F_A(t)/dt = -kᵢ * x̂_F_A(t) + * * dẑ_F_A(t)/dt = 0 * * @tparam_default_scalar */ @@ -36,72 +80,90 @@ class PiecewiseConstantCurvatureTrajectory final /** * Constructs an empty piecewise constant curvature trajectory. */ - PiecewiseConstantCurvatureTrajectory() {} + PiecewiseConstantCurvatureTrajectory() = default; /** - * Clones this trajectory to a trajectory with a different scalar type. + * Constructs a piecewise constant curvature trajectory. * - * @tparam ToScalar The scalar type to which to clone the trajectory. - * @return The cloned trajectory. + * Endpoints of each constant-curvature segments are defined by break times + * t₀ [s] < t₁ [s] < ... < tₙ [s] passed through `breaks`. N is the + * number of segments. The turning rates k₁ [1/m], ... kₙ [1/m] are passed + * through `turning_rates`. There must be exactly 1 turning rate provided per + * segment (len(turning_rates) = len(breaks) -1). + * + * The parent class, PiecewiseTrajectory, will internally enforce that the + * break times increase by at least PiecewiseTrajectory::kEpsilonTime. + * + * The initial rotation in the reference frame R_AF(t₀) is set with + * initial_curve_tangent as x̂_F_A(t₀); and plane_normal as the ŷ_F_A(t); and + * ŷ_F_A(0) set by right-hand rule. Thus if the axis are not unit-norm and + * orthogonal, construction of R_AF may fail; see MakeBaseFrame for details. + * + * @param breaks A vector of time break points between segments. + * @param turning_rates A vector of curvatures for each segment. + * @param initial_curve_tangent The initial tangent of the curve expressed in + * the parent frame, [t̂(t₀)]_A. + * @param plane_normal The normal axis of the 2D plane in which the curve + * lies, expressed in the parent frame, [p̂]_A. lies, + * + * @throws std::invalid_argument if the number of turning rates does not + * match the number of segments. */ - template - PiecewiseConstantCurvatureTrajectory CloneToScalar() const { - std::vector segment_curvatures; - std::vector segment_breaks; - systems::scalar_conversion::ValueConverter converter; - for (const T& curvature : segment_curvatures_) { - segment_curvatures.push_back(converter(curvature)); - } - for (const T& segment_break : this->breaks()) { - segment_breaks.push_back(converter(segment_break)); - } - return PiecewiseConstantCurvatureTrajectory( - segment_breaks, segment_curvatures, - base_frame_.col(kCurveTangentDimension).template cast(), - base_frame_.col(kPlaneNormalDimension).template cast()); - } + PiecewiseConstantCurvatureTrajectory(const std::vector& breaks, + const std::vector& turning_rates, + const Vector3& initial_curve_tangent, + const Vector3& plane_normal); /** - * Constructs a piecewise constant curvature trajectory. + * Constructs a PiecewiseConstantCurvatureTrajectory from another scalar + * type. * - * @param segment_breaks A vector of distance break points between segments. - * @param segment_curvatures A vector of curvatures for each segment. - * @param curve_tangent_axis The tangent of the curve at path length 0. - * @param plane_normal_axis The normal axis of the 2D plane in which the curve - * lies. + * @tparam_default_scalar + * @return The cloned trajectory. */ - PiecewiseConstantCurvatureTrajectory(const std::vector& segment_breaks, - const std::vector& segment_curvatures, - const Vector3& curve_tangent_axis, - const Vector3& plane_normal_axis); + template + explicit PiecewiseConstantCurvatureTrajectory( + const PiecewiseConstantCurvatureTrajectory other) + : PiecewiseConstantCurvatureTrajectory( + CloneSegmentDataFromScalar(other.get_segment_times()), + CloneSegmentDataFromScalar(other.segment_turning_rates_), + other.segment_start_poses_[0] + .rotation() + .col(kCurveTangentIndex) + .template cast(), + other.segment_start_poses_[0] + .rotation() + .col(kPlaneNormalIndex) + .template cast()) {} /** * @return The number of rows in the trajectory's output. */ - Eigen::Index rows() const override { return 4; } + Eigen::Index rows() const override { return 3; } /** * @return The number of columns in the trajectory's output. */ - Eigen::Index cols() const override { return 4; } + Eigen::Index cols() const override { return 1; } /** - * Returns the interpolated pose at the given distance along the trajectory. + * Returns the trajectory's pose X_AF(t) at the given time t [s] + * (equivalently, arclength [m]). * - * @param distance The distance along the trajectory. - * @return The interpolated pose as a RigidTransform. + * @param t The query time in seconds (equivalently, arclenth in meters). + * @return The corresponding pose X_AF(t) as a RigidTransform. */ - math::RigidTransform GetPose(const T& distance) const; + math::RigidTransform GetPose(const T& t) const; /** - * Returns the interpolated pose at the given distance along the trajectory as - * an Eigen matrix. + * Returns the trajectory position p_AoFo_A(t) at the given time t [s] + * (equivalently, arclength [m]). * - * @param t The distance along the trajectory. - * @return The 4x4 homogeneous transform matrix representing the pose. + * @param t The query time in seconds (equivalently, arclenth in meters). + * @return The 3x1 position vector. */ MatrixX value(const T& t) const override { - return GetPose(t).GetAsMatrix4(); + return GetPose(t).translation(); } /** @@ -112,28 +174,29 @@ class PiecewiseConstantCurvatureTrajectory final std::unique_ptr> Clone() const override; /** - * Returns the interpolated spatial velocity at the given distance along the - * trajectory. + * Returns the spatial velocity V_AF_A(t) at the given time t [s] + * (equivalently, arclength [m]). * - * @param distance The distance along the trajectory. - * @return The interpolated spatial velocity. + * @param t The query time in seconds (equivalently, arclenth in meters). + * @return The correponding spatial velocity V_AF_A(t). */ - multibody::SpatialVelocity GetVelocity(const T& distance) const; + multibody::SpatialVelocity CalcSpatialVelocity(const T& t) const; /** - * Returns the interpolated spatial acceleration at the given distance along - * the trajectory. + * Returns the spatial acceleration A_AF_A(t) at the given time t [s] + * (equivalently, arclength [m]). * - * @param distance The distance along the trajectory. - * @return The interpolated spatial acceleration. + * @param t The query time in seconds (equivalently, arclenth in meters). + * @return The correponding spatial acceleration A_AF_A(t). */ - multibody::SpatialAcceleration GetAcceleration(const T& distance) const; + multibody::SpatialAcceleration CalcSpatialAcceleration(const T& t) const; /** * Checks if the trajectory is nearly periodic within the given tolerance. * - * Periodicity is defined as the beginning and end poses being equal up to the - * same tolerance. + * Periodicity is defined as the beginning and end poses X_AF(t₀) and + * X_AF(tₙ) being equal up to the same tolerance, checked via + * RigidTransform::IsNearlyEqualTo with the same tolerance. * * @param tolerance The tolerance for periodicity check. * @return True if the trajectory is nearly periodic, false otherwise. @@ -141,67 +204,103 @@ class PiecewiseConstantCurvatureTrajectory final boolean IsNearlyPeriodic(double tolerance) const; /** - * Returns the frame of the trajectory at distance zero, which defines - * the orientation of the plane in the embedded 3D space. + * Gives the intial orientation of the trajectory in its reference frame, + * R_AF(t₀). * * @return The base frame as a RotationMatrix. */ - const math::RotationMatrix& get_base_frame() const { return base_frame_; } + const math::RotationMatrix& get_base_frame() const { + return segment_start_poses_[0].rotation(); + } /** - * Returns the normal axis of the plane in which the curve lies + * Gives the normal axis of the plane in which the curve lies, [p̂]_A. * - * @return The plane normal axis. + * @return The plane normal axis, [p̂]_A. */ const Eigen::Ref> get_plane_normal_axis() const { - return base_frame_.col(kPlaneNormalDimension); + return segment_start_poses_[0].rotation().col(kPlaneNormalIndex); } private: + template + friend class PiecewiseConstantCurvatureTrajectory; + /** - * Calculates the relative transform between the start of a constant-curavtue - * segment and the pose at a given length of travel within the segment. + * Helper function to convert segment data (break times, turning rates) scalar + * types, to aid conversion of the trajectory to different scalar types. * - * @param curvature The curvature of the segment. - * @param length The length of travel in the segment. - * @return The pose relative to length 0 along the curve. + * @param segment_data The vector of segment data to be converted. + * @return The segment data converted to new scalar type. + * @tparam_default_scalar */ - static math::RigidTransform CalcPoseInLocalFrame(const T& curvature, - const T& length); + template + static std::vector CloneSegmentDataFromScalar( + const std::vector& segment_data) { + std::vector converted_segment_data; + systems::scalar_conversion::ValueConverter converter; + for (const U& segment : segment_data) { + converted_segment_data.push_back(converter(segment)); + } + return converted_segment_data; + } + /** + * Calculates the relative transform between the start of a constant-curvature + * segment tᵢ [s] and the pose after a given duration Δt [s] (equivalently, + * arclength [s]) within the segment. + * + * This transform is equal to X_AFᵢ⁻¹ * X_AF(tᵢ + Δt). As X_AF(t) + * defines the normal of the plane as the z axis of F, this displacement + * consists of a circular arc or line segment in the x-y plane, and a + * corresponding z-axis rotation. + * + * @param k_i The turning rate of the segment. + * @param dt The duration [s] within the segment. + * @return The pose relative to to the start of the segment. + */ + static math::RigidTransform CalcRelativePoseInSegment(const T& k_i, + const T& dt); /** - * Builds the base frame from the given tangent and plane axes. + * Builds the base frame, R_AF(t₀), from the given tangent and plane axes. + * + * The plane normal p̂ is defined as the z axis ẑ_F_A; the initial heading is + * the x axis x̂_F_A; and the y axis is determined by cross product. R_AF is + * constructed by passing normalized versions of these vectors to + * RotationMatrix::MakeFromOrthonormalColumns, which may fail if the provided + * axes are not unit norm and orthogonal. * - * @param curve_tangent_axis The tangent of the curve at distance 0. - * @param plane_normal_axis The normal axis of the plane. + * @param initial_curve_tangent The tangent of the curve at time t₀. + * @param plane_normal The normal axis of the plane. * * @return The base frame as a RotationMatrix. */ static math::RotationMatrix MakeBaseFrame( - const Vector3& curve_tangent_axis, - const Vector3& plane_normal_axis); + const Vector3& initial_curve_tangent, const Vector3& plane_normal); /** - * Builds the curve from the given curvatures and segment breaks. + * Calculates the curve's pose at the beginning of each segment. + * + * For each segment i, the returned vector's i-th element contains the + * relative transform X_AFᵢ = X_AF(tᵢ). * * @param base_frame The base frame of the trajectory. - * @param segment_curvatures A vector of curvatures for each segment. - * @param segment_breaks A vector of break points between segments. + * @param turning_rates THe vector of turning rates [1/m] for each segment. + * @param breaks The vector of break points [s] between segments tᵢ. * - * @return The local frames of each segment. + * @return Vector (of length num_segments) of poses at the beggining of each + * segment. */ - static std::vector> MakeSegmentLocalFrames( + static std::vector> MakeSegmentStartPoses( const math::RotationMatrix& base_frame, - const std::vector& segment_curvatures, - const std::vector& segment_breaks); + const std::vector& turning_rates, const std::vector& breaks); - math::RotationMatrix base_frame_; - std::vector segment_curvatures_; - std::vector> segment_local_frames_; + std::vector segment_turning_rates_; + std::vector> segment_start_poses_; - static inline constexpr size_t kCurveTangentDimension = 0; - static inline constexpr size_t kCurveNormalDimension = 1; - static inline constexpr size_t kPlaneNormalDimension = 2; + static inline constexpr size_t kCurveTangentIndex = 0; + static inline constexpr size_t kCurveNormalIndex = 1; + static inline constexpr size_t kPlaneNormalIndex = 2; }; } // namespace trajectories diff --git a/common/trajectories/test/piecewise_constant_curvature_test.cc b/common/trajectories/test/piecewise_constant_curvature_test.cc index b574117a5aca..f6d8aa673748 100644 --- a/common/trajectories/test/piecewise_constant_curvature_test.cc +++ b/common/trajectories/test/piecewise_constant_curvature_test.cc @@ -78,8 +78,8 @@ GTEST_TEST(TestPiecewiseQuaternionSlerp, TestAnalytical) { Vector3d(-kappa * sin(l * kappa), kappa * cos(l * kappa), 0)); auto actual_pose = trajectory.GetPose(l); - auto actual_velocity = trajectory.GetVelocity(l); - auto actual_acceleration = trajectory.GetAcceleration(l); + auto actual_velocity = trajectory.CalcSpatialVelocity(l); + auto actual_acceleration = trajectory.CalcSpatialAcceleration(l); EXPECT_TRUE(actual_pose.IsNearlyEqualTo(expected_pose, kTolerance)); EXPECT_TRUE(actual_velocity.IsApprox(expected_velocity, kTolerance)); @@ -128,11 +128,11 @@ GTEST_TEST(TestPiecewiseConstantCuravatureTrajectory, auto curve_rotation = curve_pose.rotation(); auto curve_position = curve_pose.translation(); - auto curve_velocity = curve_spline.GetVelocity(l); + auto curve_velocity = curve_spline.CalcSpatialVelocity(l); auto translational_velocity = curve_velocity.translational(); auto rotational_velocity = curve_velocity.rotational(); - auto curve_acceleration = curve_spline.GetAcceleration(l); + auto curve_acceleration = curve_spline.CalcSpatialAcceleration(l); auto translational_acceleration = curve_acceleration.translational(); auto rotational_acceleration = curve_acceleration.rotational(); @@ -204,11 +204,10 @@ GTEST_TEST(TestPiecewiseConstantCuravatureTrajectory, TestScalarConversion) { Vector3d curve_tangent = Vector3d::UnitX(); PiecewiseConstantCurvatureTrajectory double_trajectory( times, curavtures, curve_tangent, plane_normal); - PiecewiseConstantCurvatureTrajectory autodiff_trajectory = - double_trajectory.template CloneToScalar(); + PiecewiseConstantCurvatureTrajectory autodiff_trajectory( + double_trajectory); PiecewiseConstantCurvatureTrajectory - expression_trajectory = - double_trajectory.template CloneToScalar(); + expression_trajectory(double_trajectory); for (double l = times.front(); l < times.back(); l += 0.01) { math::RigidTransform double_pose = double_trajectory.GetPose(l); @@ -218,7 +217,7 @@ GTEST_TEST(TestPiecewiseConstantCuravatureTrajectory, TestScalarConversion) { double_pose.template cast(); multibody::SpatialVelocity double_velocity = - double_trajectory.GetVelocity(l); + double_trajectory.CalcSpatialVelocity(l); multibody::SpatialVelocity autodiff_velocity( double_velocity.rotational().template cast(), double_velocity.translational().template cast()); @@ -227,7 +226,7 @@ GTEST_TEST(TestPiecewiseConstantCuravatureTrajectory, TestScalarConversion) { double_velocity.translational().template cast()); multibody::SpatialAcceleration double_acceleration = - double_trajectory.GetAcceleration(l); + double_trajectory.CalcSpatialAcceleration(l); multibody::SpatialAcceleration autodiff_acceleration( double_acceleration.rotational().template cast(), double_acceleration.translational().template cast()); @@ -242,15 +241,15 @@ GTEST_TEST(TestPiecewiseConstantCuravatureTrajectory, TestScalarConversion) { EXPECT_TRUE(expression_pose.IsNearlyEqualTo( expression_trajectory.GetPose(l), kTolerance)); - EXPECT_TRUE(autodiff_velocity.IsApprox(autodiff_trajectory.GetVelocity(l), - kTolerance)); + EXPECT_TRUE(autodiff_velocity.IsApprox( + autodiff_trajectory.CalcSpatialVelocity(l), kTolerance)); EXPECT_TRUE(expression_velocity.IsApprox( - expression_trajectory.GetVelocity(l), kTolerance)); + expression_trajectory.CalcSpatialVelocity(l), kTolerance)); EXPECT_TRUE(autodiff_acceleration.IsApprox( - autodiff_trajectory.GetAcceleration(l), kTolerance)); + autodiff_trajectory.CalcSpatialAcceleration(l), kTolerance)); EXPECT_TRUE(expression_acceleration.IsApprox( - expression_trajectory.GetAcceleration(l), kTolerance)); + expression_trajectory.CalcSpatialAcceleration(l), kTolerance)); } }