92 for (
size_t i = 0; i < modulesToApply.size(); ++i) {
108 this->DriveRequestType = driveRequestType;
119 this->SteerRequestType = steerRequestType;
196 Translation2d tmp{toApplyX * 1_s, toApplyY * 1_s};
198 toApplyX = tmp.X() / 1_s;
199 toApplyY = tmp.Y() / 1_s;
202 if (units::math::hypot(toApplyX, toApplyY) <
Deadband) {
207 toApplyOmega = 0_rad_per_s;
218 for (
size_t i = 0; i < modulesToApply.size(); ++i) {
235 this->VelocityX = velocityX;
249 this->VelocityY = velocityY;
263 this->RotationalRate = rotationalRate;
275 this->Deadband = deadband;
286 this->RotationalDeadband = rotationalDeadband;
309 this->DriveRequestType = driveRequestType;
320 this->SteerRequestType = steerRequestType;
333 this->DesaturateWheelSpeeds = desaturateWheelSpeeds;
345 this->ForwardReference = forwardReference;
437 Translation2d tmp{toApplyX * 1_s, toApplyY * 1_s};
439 toApplyX = tmp.X() / 1_s;
440 toApplyY = tmp.Y() / 1_s;
446 angleToFace.Radians().value(), parameters.
timestamp)};
447 if (units::math::hypot(toApplyX, toApplyY) <
Deadband) {
452 toApplyOmega = 0_rad_per_s;
463 for (
size_t i = 0; i < modulesToApply.size(); ++i) {
480 this->VelocityX = velocityX;
494 this->VelocityY = velocityY;
521 this->Deadband = deadband;
532 this->RotationalDeadband = rotationalDeadband;
555 this->DriveRequestType = driveRequestType;
566 this->SteerRequestType = steerRequestType;
579 this->DesaturateWheelSpeeds = desaturateWheelSpeeds;
591 this->ForwardReference = forwardReference;
617 for (
size_t i = 0; i < modulesToApply.size(); ++i) {
646 this->DriveRequestType = driveRequestType;
657 this->SteerRequestType = steerRequestType;
728 if (units::math::hypot(toApplyX, toApplyY) <
Deadband) {
733 toApplyOmega = 0_rad_per_s;
742 for (
size_t i = 0; i < modulesToApply.size(); ++i) {
759 this->VelocityX = velocityX;
773 this->VelocityY = velocityY;
787 this->RotationalRate = rotationalRate;
799 this->Deadband = deadband;
810 this->RotationalDeadband = rotationalDeadband;
833 this->DriveRequestType = driveRequestType;
844 this->SteerRequestType = steerRequestType;
857 this->DesaturateWheelSpeeds = desaturateWheelSpeeds;
896 for (
size_t i = 0; i < modulesToApply.size(); ++i) {
934 this->DriveRequestType = driveRequestType;
945 this->SteerRequestType = steerRequestType;
958 this->DesaturateWheelSpeeds = desaturateWheelSpeeds;
976 for (
size_t i = 0; i < modulesToApply.size(); ++i) {
990 this->VoltsToApply = volts;
1008 for (
size_t i = 0; i < modulesToApply.size(); ++i) {
1023 this->VoltsToApply = volts;
1041 for (
size_t i = 0; i < modulesToApply.size(); ++i) {
1055 this->VoltsToApply = volts;
its the or e mails sent from or on behalf of the Company are free of trojan timebombs or other harmful components Some jurisdictions do not allow the exclusion of certain types of warranties or limitations on applicable statutory rights of a so some or all of the above exclusions and limitations may not apply to You But in such a case the exclusions and limitations set forth in this section shall be applied to the greatest extent enforceable under applicable law To the extent any warranty exists under law that cannot be the Company shall be solely responsible for such warranty Severability and Waiver Severability If any provision of this Agreement is held to be unenforceable or such provision will be changed and interpreted to accomplish the objectives of such provision to the greatest extent possible under applicable law and the remaining provisions will continue in full force and effect Waiver Except as provided the failure to exercise a right or to require performance of an obligation under this Agreement shall not affect a party s ability to exercise such right or require such performance at any time thereafter nor shall the waiver of a breach constitute waiver of any subsequent breach Product Claims The Company does not make any warranties concerning the Software United States Legal Compliance You represent and warrant or that has been designated by the United States government as a terrorist supporting at its sole to modify or replace this Agreement at any time If a revision is material we will provide at least days notice prior to any new terms taking effect What constitutes a material change will be determined at the sole discretion of the Company By continuing to access or use the Software after any revisions become You agree to be bound by the revised terms If You do not agree to the new You are no longer authorized to use the Software Governing Law The laws of the State of United States of excluding its conflicts of law shall govern this Agreement and your use of the Software Your use of the Software may also be subject to other state
Definition: CTRE_LICENSE.txt:283
Request PID to target position with voltage feedforward.
Definition: PositionVoltage.hpp:31
Request a specified voltage.
Definition: VoltageOut.hpp:29
Phoenix-centric PID controller taken from WPI's frc::PIDController class.
Definition: PhoenixPIDController.hpp:23
double Calculate(double measurement, double setpoint, units::second_t currentTimestamp)
Returns the next output of the PID controller.
WheelSpeeds ToSwerveModuleStates(ChassisSpeeds const &speeds, Translation2d const ¢erOfRotation=Translation2d{})
Performs inverse kinematics to return the module states from a desired chassis velocity.
static void DesaturateWheelSpeeds(WheelSpeeds *moduleStates, units::meters_per_second_t attainableMaxSpeed)
Renormalizes the wheel speeds if any individual speed is above the specified maximum.
DriveRequestType
All possible control requests for the module drive motor.
Definition: SwerveModuleImpl.hpp:48
@ OpenLoopVoltage
Control the drive motor using an open-loop voltage request.
SteerRequestType
All possible control requests for the module steer motor.
Definition: SwerveModuleImpl.hpp:32
@ MotionMagicExpo
Control the drive motor using a Motion Magic® Expo request.
Accepts a generic ChassisSpeeds to apply to the drivetrain.
Definition: SwerveRequest.hpp:865
ApplyChassisSpeeds & WithDriveRequestType(impl::SwerveModuleImpl::DriveRequestType driveRequestType)
Sets the type of control request to use for the drive motor.
Definition: SwerveRequest.hpp:932
bool DesaturateWheelSpeeds
Whether to desaturate wheel speeds before applying.
Definition: SwerveRequest.hpp:887
ApplyChassisSpeeds & WithSpeeds(impl::ChassisSpeeds speeds)
Sets the chassis speeds to apply to the drivetrain.
Definition: SwerveRequest.hpp:909
ApplyChassisSpeeds & WithDesaturateWheelSpeeds(bool desaturateWheelSpeeds)
Sets whether to desaturate wheel speeds before applying.
Definition: SwerveRequest.hpp:956
impl::ChassisSpeeds Speeds
The chassis speeds to apply to the drivetrain.
Definition: SwerveRequest.hpp:870
impl::SwerveModuleImpl::DriveRequestType DriveRequestType
The type of control request to use for the drive motor.
Definition: SwerveRequest.hpp:878
ApplyChassisSpeeds & WithCenterOfRotation(Translation2d centerOfRotation)
Sets the center of rotation to rotate around.
Definition: SwerveRequest.hpp:920
Translation2d CenterOfRotation
The center of rotation to rotate around.
Definition: SwerveRequest.hpp:874
ctre::phoenix::StatusCode Apply(SwerveRequest::ControlParameters const ¶meters, std::vector< std::unique_ptr< impl::SwerveModuleImpl > > const &modulesToApply) override
Applies this swerve request to the given modules.
Definition: SwerveRequest.hpp:889
ApplyChassisSpeeds & WithSteerRequestType(impl::SwerveModuleImpl::SteerRequestType steerRequestType)
Sets the type of control request to use for the steer motor.
Definition: SwerveRequest.hpp:943
impl::SwerveModuleImpl::SteerRequestType SteerRequestType
The type of control request to use for the steer motor.
Definition: SwerveRequest.hpp:882
Drives the swerve drivetrain in a field-centric manner, maintaining a specified heading angle to ensu...
Definition: SwerveRequest.hpp:365
bool DesaturateWheelSpeeds
Whether to desaturate wheel speeds before applying.
Definition: SwerveRequest.hpp:413
FieldCentricFacingAngle & WithDeadband(units::meters_per_second_t deadband)
Sets the allowable deadband of the request.
Definition: SwerveRequest.hpp:519
FieldCentricFacingAngle & WithSteerRequestType(impl::SwerveModuleImpl::SteerRequestType steerRequestType)
Sets the type of control request to use for the steer motor.
Definition: SwerveRequest.hpp:564
Rotation2d TargetDirection
The desired direction to face.
Definition: SwerveRequest.hpp:385
units::meters_per_second_t VelocityX
The velocity in the X direction.
Definition: SwerveRequest.hpp:372
impl::SwerveModuleImpl::DriveRequestType DriveRequestType
The type of control request to use for the drive motor.
Definition: SwerveRequest.hpp:404
FieldCentricFacingAngle & WithRotationalDeadband(units::radians_per_second_t rotationalDeadband)
Sets the rotational deadband of the request.
Definition: SwerveRequest.hpp:530
FieldCentricFacingAngle & WithDriveRequestType(impl::SwerveModuleImpl::DriveRequestType driveRequestType)
Sets the type of control request to use for the drive motor.
Definition: SwerveRequest.hpp:553
FieldCentricFacingAngle & WithCenterOfRotation(Translation2d centerOfRotation)
Sets the center of rotation of the request.
Definition: SwerveRequest.hpp:541
FieldCentricFacingAngle & WithTargetDirection(Rotation2d targetDirection)
Sets the desired direction to face.
Definition: SwerveRequest.hpp:507
PhoenixPIDController HeadingController
The PID controller used to maintain the desired heading.
Definition: SwerveRequest.hpp:428
impl::SwerveModuleImpl::SteerRequestType SteerRequestType
The type of control request to use for the steer motor.
Definition: SwerveRequest.hpp:408
ForwardReferenceValue ForwardReference
The perspective to use when determining which direction is forward.
Definition: SwerveRequest.hpp:418
Translation2d CenterOfRotation
The center of rotation the robot should rotate around.
Definition: SwerveRequest.hpp:399
units::meters_per_second_t Deadband
The allowable deadband of the request.
Definition: SwerveRequest.hpp:390
ctre::phoenix::StatusCode Apply(SwerveRequest::ControlParameters const ¶meters, std::vector< std::unique_ptr< impl::SwerveModuleImpl > > const &modulesToApply) override
Applies this swerve request to the given modules.
Definition: SwerveRequest.hpp:430
units::meters_per_second_t VelocityY
The velocity in the Y direction.
Definition: SwerveRequest.hpp:378
FieldCentricFacingAngle & WithVelocityY(units::meters_per_second_t velocityY)
Sets the velocity in the Y direction.
Definition: SwerveRequest.hpp:492
FieldCentricFacingAngle & WithVelocityX(units::meters_per_second_t velocityX)
Sets the velocity in the X direction.
Definition: SwerveRequest.hpp:478
units::radians_per_second_t RotationalDeadband
The rotational deadband of the request.
Definition: SwerveRequest.hpp:394
FieldCentricFacingAngle & WithForwardReference(ForwardReferenceValue forwardReference)
Sets the perspective to use when determining which direction is forward.
Definition: SwerveRequest.hpp:589
FieldCentricFacingAngle & WithDesaturateWheelSpeeds(bool desaturateWheelSpeeds)
Sets whether to desaturate wheel speeds before applying.
Definition: SwerveRequest.hpp:577
Drives the swerve drivetrain in a field-centric manner.
Definition: SwerveRequest.hpp:136
FieldCentric & WithVelocityY(units::meters_per_second_t velocityY)
Sets the velocity in the Y direction.
Definition: SwerveRequest.hpp:247
impl::SwerveModuleImpl::DriveRequestType DriveRequestType
The type of control request to use for the drive motor.
Definition: SwerveRequest.hpp:173
FieldCentric & WithDriveRequestType(impl::SwerveModuleImpl::DriveRequestType driveRequestType)
Sets the type of control request to use for the drive motor.
Definition: SwerveRequest.hpp:307
units::meters_per_second_t Deadband
The allowable deadband of the request.
Definition: SwerveRequest.hpp:159
units::radians_per_second_t RotationalRate
The angular rate to rotate at.
Definition: SwerveRequest.hpp:155
units::meters_per_second_t VelocityY
The velocity in the Y direction.
Definition: SwerveRequest.hpp:149
ForwardReferenceValue ForwardReference
The perspective to use when determining which direction is forward.
Definition: SwerveRequest.hpp:187
ctre::phoenix::StatusCode Apply(SwerveRequest::ControlParameters const ¶meters, std::vector< std::unique_ptr< impl::SwerveModuleImpl > > const &modulesToApply) override
Applies this swerve request to the given modules.
Definition: SwerveRequest.hpp:190
units::meters_per_second_t VelocityX
The velocity in the X direction.
Definition: SwerveRequest.hpp:143
FieldCentric & WithSteerRequestType(impl::SwerveModuleImpl::SteerRequestType steerRequestType)
Sets the type of control request to use for the steer motor.
Definition: SwerveRequest.hpp:318
Translation2d CenterOfRotation
The center of rotation the robot should rotate around.
Definition: SwerveRequest.hpp:168
FieldCentric & WithVelocityX(units::meters_per_second_t velocityX)
Sets the velocity in the X direction.
Definition: SwerveRequest.hpp:233
bool DesaturateWheelSpeeds
Whether to desaturate wheel speeds before applying.
Definition: SwerveRequest.hpp:182
FieldCentric & WithRotationalDeadband(units::radians_per_second_t rotationalDeadband)
Sets the rotational deadband of the request.
Definition: SwerveRequest.hpp:284
impl::SwerveModuleImpl::SteerRequestType SteerRequestType
The type of control request to use for the steer motor.
Definition: SwerveRequest.hpp:177
FieldCentric & WithForwardReference(ForwardReferenceValue forwardReference)
Sets the perspective to use when determining which direction is forward.
Definition: SwerveRequest.hpp:343
FieldCentric & WithRotationalRate(units::radians_per_second_t rotationalRate)
The angular rate to rotate at.
Definition: SwerveRequest.hpp:261
FieldCentric & WithCenterOfRotation(Translation2d centerOfRotation)
Sets the center of rotation of the request.
Definition: SwerveRequest.hpp:295
units::radians_per_second_t RotationalDeadband
The rotational deadband of the request.
Definition: SwerveRequest.hpp:163
FieldCentric & WithDesaturateWheelSpeeds(bool desaturateWheelSpeeds)
Sets whether to desaturate wheel speeds before applying.
Definition: SwerveRequest.hpp:331
FieldCentric & WithDeadband(units::meters_per_second_t deadband)
Sets the allowable deadband of the request.
Definition: SwerveRequest.hpp:273
Does nothing to the swerve module state.
Definition: SwerveRequest.hpp:66
ctre::phoenix::StatusCode Apply(SwerveRequest::ControlParameters const &, std::vector< std::unique_ptr< impl::SwerveModuleImpl > > const &) override
Applies this swerve request to the given modules.
Definition: SwerveRequest.hpp:68
Sets the swerve drive modules to point to a specified direction.
Definition: SwerveRequest.hpp:599
PointWheelsAt & WithModuleDirection(Rotation2d moduleDirection)
Sets the direction to point the modules toward.
Definition: SwerveRequest.hpp:632
impl::SwerveModuleImpl::SteerRequestType SteerRequestType
The type of control request to use for the steer motor.
Definition: SwerveRequest.hpp:613
ctre::phoenix::StatusCode Apply(SwerveRequest::ControlParameters const ¶meters, std::vector< std::unique_ptr< impl::SwerveModuleImpl > > const &modulesToApply) override
Applies this swerve request to the given modules.
Definition: SwerveRequest.hpp:615
PointWheelsAt & WithDriveRequestType(impl::SwerveModuleImpl::DriveRequestType driveRequestType)
Sets the type of control request to use for the drive motor.
Definition: SwerveRequest.hpp:644
PointWheelsAt & WithSteerRequestType(impl::SwerveModuleImpl::SteerRequestType steerRequestType)
Sets the type of control request to use for the steer motor.
Definition: SwerveRequest.hpp:655
Rotation2d ModuleDirection
The direction to point the modules toward.
Definition: SwerveRequest.hpp:605
impl::SwerveModuleImpl::DriveRequestType DriveRequestType
The type of control request to use for the drive motor.
Definition: SwerveRequest.hpp:609
Drives the swerve drivetrain in a robot-centric manner.
Definition: SwerveRequest.hpp:674
units::radians_per_second_t RotationalRate
The angular rate to rotate at.
Definition: SwerveRequest.hpp:693
bool DesaturateWheelSpeeds
Whether to desaturate wheel speeds before applying.
Definition: SwerveRequest.hpp:721
RobotCentric & WithCenterOfRotation(Translation2d centerOfRotation)
Sets the center of rotation of the request.
Definition: SwerveRequest.hpp:819
units::meters_per_second_t VelocityY
The velocity in the Y direction.
Definition: SwerveRequest.hpp:687
units::radians_per_second_t RotationalDeadband
The rotational deadband of the request.
Definition: SwerveRequest.hpp:702
RobotCentric & WithVelocityX(units::meters_per_second_t velocityX)
Sets the velocity in the X direction.
Definition: SwerveRequest.hpp:757
RobotCentric & WithDesaturateWheelSpeeds(bool desaturateWheelSpeeds)
Sets whether to desaturate wheel speeds before applying.
Definition: SwerveRequest.hpp:855
RobotCentric & WithSteerRequestType(impl::SwerveModuleImpl::SteerRequestType steerRequestType)
Sets the type of control request to use for the steer motor.
Definition: SwerveRequest.hpp:842
units::meters_per_second_t VelocityX
The velocity in the X direction.
Definition: SwerveRequest.hpp:681
Translation2d CenterOfRotation
The center of rotation the robot should rotate around.
Definition: SwerveRequest.hpp:707
RobotCentric & WithDriveRequestType(impl::SwerveModuleImpl::DriveRequestType driveRequestType)
Sets the type of control request to use for the drive motor.
Definition: SwerveRequest.hpp:831
units::meters_per_second_t Deadband
The allowable deadband of the request.
Definition: SwerveRequest.hpp:698
RobotCentric & WithDeadband(units::meters_per_second_t deadband)
Sets the allowable deadband of the request.
Definition: SwerveRequest.hpp:797
RobotCentric & WithVelocityY(units::meters_per_second_t velocityY)
Sets the velocity in the Y direction.
Definition: SwerveRequest.hpp:771
impl::SwerveModuleImpl::SteerRequestType SteerRequestType
The type of control request to use for the steer motor.
Definition: SwerveRequest.hpp:716
RobotCentric & WithRotationalRate(units::radians_per_second_t rotationalRate)
The angular rate to rotate at.
Definition: SwerveRequest.hpp:785
RobotCentric & WithRotationalDeadband(units::radians_per_second_t rotationalDeadband)
Sets the rotational deadband of the request.
Definition: SwerveRequest.hpp:808
ctre::phoenix::StatusCode Apply(SwerveRequest::ControlParameters const ¶meters, std::vector< std::unique_ptr< impl::SwerveModuleImpl > > const &modulesToApply) override
Applies this swerve request to the given modules.
Definition: SwerveRequest.hpp:723
impl::SwerveModuleImpl::DriveRequestType DriveRequestType
The type of control request to use for the drive motor.
Definition: SwerveRequest.hpp:712
Sets the swerve drive module states to point inward on the robot in an "X" fashion,...
Definition: SwerveRequest.hpp:79
SwerveDriveBrake & WithSteerRequestType(impl::SwerveModuleImpl::SteerRequestType steerRequestType)
Sets the type of control request to use for the steer motor.
Definition: SwerveRequest.hpp:117
impl::SwerveModuleImpl::SteerRequestType SteerRequestType
The type of control request to use for the steer motor.
Definition: SwerveRequest.hpp:88
impl::SwerveModuleImpl::DriveRequestType DriveRequestType
The type of control request to use for the drive motor.
Definition: SwerveRequest.hpp:84
ctre::phoenix::StatusCode Apply(SwerveRequest::ControlParameters const ¶meters, std::vector< std::unique_ptr< impl::SwerveModuleImpl > > const &modulesToApply) override
Applies this swerve request to the given modules.
Definition: SwerveRequest.hpp:90
SwerveDriveBrake & WithDriveRequestType(impl::SwerveModuleImpl::DriveRequestType driveRequestType)
Sets the type of control request to use for the drive motor.
Definition: SwerveRequest.hpp:106
Container for all the Swerve Requests.
Definition: SwerveRequest.hpp:45
virtual ~SwerveRequest()=default
virtual ctre::phoenix::StatusCode Apply(ControlParameters const ¶meters, std::vector< std::unique_ptr< impl::SwerveModuleImpl > > const &modulesToApply)=0
Applies this swerve request to the given modules.
SysId-specific SwerveRequest to characterize the rotational characteristics of a swerve drivetrain.
Definition: SwerveRequest.hpp:999
units::volt_t VoltsToApply
Voltage to apply to drive wheels.
Definition: SwerveRequest.hpp:1004
ctre::phoenix::StatusCode Apply(SwerveRequest::ControlParameters const ¶meters, std::vector< std::unique_ptr< impl::SwerveModuleImpl > > const &modulesToApply) override
Applies this swerve request to the given modules.
Definition: SwerveRequest.hpp:1006
SysIdSwerveRotation & WithVolts(units::volt_t volts)
Update the voltage to apply to the drive wheels.
Definition: SwerveRequest.hpp:1021
SysId-specific SwerveRequest to characterize the steer module characteristics of a swerve drivetrain.
Definition: SwerveRequest.hpp:1032
SysIdSwerveSteerGains & WithVolts(units::volt_t volts)
Update the voltage to apply to the drive wheels.
Definition: SwerveRequest.hpp:1053
units::volt_t VoltsToApply
Voltage to apply to drive wheels.
Definition: SwerveRequest.hpp:1037
ctre::phoenix::StatusCode Apply(SwerveRequest::ControlParameters const ¶meters, std::vector< std::unique_ptr< impl::SwerveModuleImpl > > const &modulesToApply) override
Applies this swerve request to the given modules.
Definition: SwerveRequest.hpp:1039
SysId-specific SwerveRequest to characterize the translational characteristics of a swerve drivetrain...
Definition: SwerveRequest.hpp:967
SysIdSwerveTranslation & WithVolts(units::volt_t volts)
Sets the voltage to apply to drive wheels.
Definition: SwerveRequest.hpp:988
ctre::phoenix::StatusCode Apply(SwerveRequest::ControlParameters const ¶meters, std::vector< std::unique_ptr< impl::SwerveModuleImpl > > const &modulesToApply) override
Applies this swerve request to the given modules.
Definition: SwerveRequest.hpp:974
units::volt_t VoltsToApply
Voltage to apply to drive wheels.
Definition: SwerveRequest.hpp:972
Status codes reported by APIs, including OK, warnings, and errors.
Definition: StatusCodes.h:27
static constexpr int OK
No Error.
Definition: StatusCodes.h:34
ForwardReferenceValue
The reference for "forward" is sometimes different if you're talking about field relative.
Definition: SwerveRequest.hpp:21
@ RedAlliance
This forward reference makes it so "forward" (positive X) is always towards the red alliance.
@ OperatorPerspective
This forward references makes it so "forward" (positive X) is determined from the operator's perspect...
Represents the state of one swerve module.
Definition: StatusCodes.h:18
Represents the speed of a robot chassis.
Definition: SwerveDriveKinematics.hpp:96
static ChassisSpeeds FromFieldRelativeSpeeds(ChassisSpeeds const &fieldRelativeSpeeds, Rotation2d const &robotAngle)
Converts a user provided field-relative ChassisSpeeds object into a robot-relative ChassisSpeeds obje...
Definition: SwerveDriveKinematics.hpp:160
static ChassisSpeeds Discretize(ChassisSpeeds const &continuousSpeeds, units::second_t dt)
Disretizes a continuous-time chassis speed.
Definition: SwerveDriveKinematics.hpp:139
Contains everything the control requests need to calculate the module state.
Definition: SwerveDrivetrainImpl.hpp:124
units::second_t timestamp
Definition: SwerveDrivetrainImpl.hpp:132
Translation2d const * swervePositions
Definition: SwerveDrivetrainImpl.hpp:126
units::meters_per_second_t kMaxSpeed
Definition: SwerveDrivetrainImpl.hpp:127
units::second_t updatePeriod
Definition: SwerveDrivetrainImpl.hpp:133
impl::SwerveDriveKinematics * kinematics
Definition: SwerveDrivetrainImpl.hpp:125
Pose2d currentPose
Definition: SwerveDrivetrainImpl.hpp:131
Rotation2d operatorForwardDirection
Definition: SwerveDrivetrainImpl.hpp:129
Definition: SwerveDriveKinematics.hpp:23