forked from Archive/PX4-Autopilot
folder restructure
This commit is contained in:
parent
6efca3a196
commit
1f332d561e
|
@ -59,7 +59,7 @@
|
||||||
// Local includes
|
// Local includes
|
||||||
// #include <DifferentialDriveKinematics.hpp>
|
// #include <DifferentialDriveKinematics.hpp>
|
||||||
// #include "DifferentialDriveControl.hpp"
|
// #include "DifferentialDriveControl.hpp"
|
||||||
#include "AckermannDriveControl.hpp"
|
#include "AckermannDriveControl/AckermannDriveControl.hpp"
|
||||||
|
|
||||||
#define DIFFERENTIAL_DRIVE 6
|
#define DIFFERENTIAL_DRIVE 6
|
||||||
#define ACKERMANN_DRIVE 5
|
#define ACKERMANN_DRIVE 5
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
############################################################################
|
############################################################################
|
||||||
#
|
#
|
||||||
# Copyright (c) 2023 PX4 Development Team. All rights reserved.
|
# Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||||
#
|
#
|
||||||
# Redistribution and use in source and binary forms, with or without
|
# Redistribution and use in source and binary forms, with or without
|
||||||
# modification, are permitted provided that the following conditions
|
# modification, are permitted provided that the following conditions
|
||||||
|
@ -31,10 +31,8 @@
|
||||||
#
|
#
|
||||||
############################################################################
|
############################################################################
|
||||||
|
|
||||||
px4_add_library(DifferentialDriveKinematics
|
px4_add_library(AckermannDriveControl
|
||||||
DifferentialDriveKinematics.cpp
|
AckermannDriveControl.cpp
|
||||||
)
|
)
|
||||||
|
|
||||||
target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
target_include_directories(AckermannDriveControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||||
|
|
||||||
px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)
|
|
|
@ -33,16 +33,16 @@
|
||||||
|
|
||||||
# add_subdirectory(DifferentialDriveKinematics)
|
# add_subdirectory(DifferentialDriveKinematics)
|
||||||
|
|
||||||
|
add_subdirectory(AckermannDriveControl)
|
||||||
|
|
||||||
px4_add_module(
|
px4_add_module(
|
||||||
MODULE modules__ackermann_drive
|
MODULE modules__ackermann_drive
|
||||||
MAIN ackermann_drive
|
MAIN ackermann_drive
|
||||||
SRCS
|
SRCS
|
||||||
AckermannDriveControl.cpp
|
|
||||||
AckermannDriveControl.hpp
|
|
||||||
AckermannDrive.cpp
|
AckermannDrive.cpp
|
||||||
AckermannDrive.hpp
|
AckermannDrive.hpp
|
||||||
DEPENDS
|
DEPENDS
|
||||||
# DifferentialDriveKinematics
|
AckermannDriveControl
|
||||||
px4_work_queue
|
px4_work_queue
|
||||||
# MODULE_CONFIG
|
# MODULE_CONFIG
|
||||||
# module.yaml
|
# module.yaml
|
||||||
|
|
|
@ -1,67 +0,0 @@
|
||||||
/****************************************************************************
|
|
||||||
*
|
|
||||||
* Copyright (C) 2023 PX4 Development Team. All rights reserved.
|
|
||||||
*
|
|
||||||
* Redistribution and use in source and binary forms, with or without
|
|
||||||
* modification, are permitted provided that the following conditions
|
|
||||||
* are met:
|
|
||||||
*
|
|
||||||
* 1. Redistributions of source code must retain the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer.
|
|
||||||
* 2. Redistributions in binary form must reproduce the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer in
|
|
||||||
* the documentation and/or other materials provided with the
|
|
||||||
* distribution.
|
|
||||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
||||||
* used to endorse or promote products derived from this software
|
|
||||||
* without specific prior written permission.
|
|
||||||
*
|
|
||||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
||||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
||||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
||||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
||||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
||||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
||||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
||||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
*
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
#include "DifferentialDriveKinematics.hpp"
|
|
||||||
|
|
||||||
#include <mathlib/mathlib.h>
|
|
||||||
|
|
||||||
using namespace matrix;
|
|
||||||
|
|
||||||
matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate)
|
|
||||||
{
|
|
||||||
if (_max_speed < FLT_EPSILON) {
|
|
||||||
return Vector2f();
|
|
||||||
}
|
|
||||||
|
|
||||||
linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed);
|
|
||||||
yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity);
|
|
||||||
|
|
||||||
const float rotational_velocity = (_wheel_base / 2.f) * yaw_rate;
|
|
||||||
float combined_velocity = fabsf(linear_velocity_x) + fabsf(rotational_velocity);
|
|
||||||
|
|
||||||
// Compute an initial gain
|
|
||||||
float gain = 1.0f;
|
|
||||||
|
|
||||||
if (combined_velocity > _max_speed) {
|
|
||||||
float excess_velocity = fabsf(combined_velocity - _max_speed);
|
|
||||||
float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
|
|
||||||
gain = adjusted_linear_velocity / fabsf(linear_velocity_x);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Apply the gain
|
|
||||||
linear_velocity_x *= gain;
|
|
||||||
|
|
||||||
// Calculate the left and right wheel speeds
|
|
||||||
return Vector2f(linear_velocity_x - rotational_velocity,
|
|
||||||
linear_velocity_x + rotational_velocity) / _max_speed;
|
|
||||||
}
|
|
|
@ -1,84 +0,0 @@
|
||||||
/****************************************************************************
|
|
||||||
*
|
|
||||||
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
|
|
||||||
*
|
|
||||||
* Redistribution and use in source and binary forms, with or without
|
|
||||||
* modification, are permitted provided that the following conditions
|
|
||||||
* are met:
|
|
||||||
*
|
|
||||||
* 1. Redistributions of source code must retain the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer.
|
|
||||||
* 2. Redistributions in binary form must reproduce the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer in
|
|
||||||
* the documentation and/or other materials provided with the
|
|
||||||
* distribution.
|
|
||||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
||||||
* used to endorse or promote products derived from this software
|
|
||||||
* without specific prior written permission.
|
|
||||||
*
|
|
||||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
||||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
||||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
||||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
||||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
||||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
||||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
||||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
*
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <matrix/matrix/math.hpp>
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot.
|
|
||||||
*
|
|
||||||
* This class provides functions to set the wheel base and radius, and to compute the inverse kinematics
|
|
||||||
* given linear velocity and yaw rate.
|
|
||||||
*/
|
|
||||||
class DifferentialDriveKinematics
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
DifferentialDriveKinematics() = default;
|
|
||||||
~DifferentialDriveKinematics() = default;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Sets the wheel base of the robot.
|
|
||||||
*
|
|
||||||
* @param wheel_base The distance between the centers of the wheels.
|
|
||||||
*/
|
|
||||||
void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; };
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Sets the maximum speed of the robot.
|
|
||||||
*
|
|
||||||
* @param max_speed The maximum speed of the robot.
|
|
||||||
*/
|
|
||||||
void setMaxSpeed(const float max_speed) { _max_speed = max_speed; };
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Sets the maximum angular speed of the robot.
|
|
||||||
*
|
|
||||||
* @param max_angular_speed The maximum angular speed of the robot.
|
|
||||||
*/
|
|
||||||
void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; };
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Computes the inverse kinematics for differential drive.
|
|
||||||
*
|
|
||||||
* @param linear_velocity_x Linear velocity along the x-axis.
|
|
||||||
* @param yaw_rate Yaw rate of the robot.
|
|
||||||
* @return matrix::Vector2f Motor velocities for the right and left motors.
|
|
||||||
*/
|
|
||||||
matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate);
|
|
||||||
|
|
||||||
private:
|
|
||||||
float _wheel_base{0.f};
|
|
||||||
float _max_speed{0.f};
|
|
||||||
float _max_angular_velocity{0.f};
|
|
||||||
};
|
|
|
@ -1,174 +0,0 @@
|
||||||
/****************************************************************************
|
|
||||||
*
|
|
||||||
* Copyright (C) 2023 PX4 Development Team. All rights reserved.
|
|
||||||
*
|
|
||||||
* Redistribution and use in source and binary forms, with or without
|
|
||||||
* modification, are permitted provided that the following conditions
|
|
||||||
* are met:
|
|
||||||
*
|
|
||||||
* 1. Redistributions of source code must retain the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer.
|
|
||||||
* 2. Redistributions in binary form must reproduce the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer in
|
|
||||||
* the documentation and/or other materials provided with the
|
|
||||||
* distribution.
|
|
||||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
||||||
* used to endorse or promote products derived from this software
|
|
||||||
* without specific prior written permission.
|
|
||||||
*
|
|
||||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
||||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
||||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
||||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
||||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
||||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
||||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
||||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
*
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
#include <gtest/gtest.h>
|
|
||||||
#include "DifferentialDriveKinematics.hpp"
|
|
||||||
#include <mathlib/math/Functions.hpp>
|
|
||||||
|
|
||||||
using namespace matrix;
|
|
||||||
|
|
||||||
TEST(DifferentialDriveKinematicsTest, AllZeroInputCase)
|
|
||||||
{
|
|
||||||
DifferentialDriveKinematics kinematics;
|
|
||||||
kinematics.setWheelBase(1.f);
|
|
||||||
kinematics.setMaxSpeed(10.f);
|
|
||||||
kinematics.setMaxAngularVelocity(10.f);
|
|
||||||
|
|
||||||
// Test with zero linear velocity and zero yaw rate (stationary vehicle)
|
|
||||||
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 0.f), Vector2f());
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
TEST(DifferentialDriveKinematicsTest, InvalidParameterCase)
|
|
||||||
{
|
|
||||||
DifferentialDriveKinematics kinematics;
|
|
||||||
kinematics.setWheelBase(0.f);
|
|
||||||
kinematics.setMaxSpeed(10.f);
|
|
||||||
kinematics.setMaxAngularVelocity(10.f);
|
|
||||||
|
|
||||||
// Test with invalid parameters (zero wheel base and wheel radius)
|
|
||||||
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, .1f), Vector2f());
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
TEST(DifferentialDriveKinematicsTest, UnitCase)
|
|
||||||
{
|
|
||||||
DifferentialDriveKinematics kinematics;
|
|
||||||
kinematics.setWheelBase(1.f);
|
|
||||||
kinematics.setMaxSpeed(10.f);
|
|
||||||
kinematics.setMaxAngularVelocity(10.f);
|
|
||||||
|
|
||||||
// Test with unit values for linear velocity and yaw rate
|
|
||||||
EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0.05f, 0.15f));
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
|
|
||||||
{
|
|
||||||
DifferentialDriveKinematics kinematics;
|
|
||||||
kinematics.setWheelBase(1.f);
|
|
||||||
kinematics.setMaxSpeed(1.f);
|
|
||||||
kinematics.setMaxAngularVelocity(1.f);
|
|
||||||
|
|
||||||
// Test with unit values for linear velocity and yaw rate, but with max speed that requires saturation
|
|
||||||
EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0, 1));
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
|
|
||||||
{
|
|
||||||
DifferentialDriveKinematics kinematics;
|
|
||||||
kinematics.setWheelBase(1.f);
|
|
||||||
kinematics.setMaxSpeed(1.f);
|
|
||||||
kinematics.setMaxAngularVelocity(1.f);
|
|
||||||
|
|
||||||
// Negative linear velocity for backward motion and positive yaw rate for turning right
|
|
||||||
EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0));
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(DifferentialDriveKinematicsTest, RandomCase)
|
|
||||||
{
|
|
||||||
DifferentialDriveKinematics kinematics;
|
|
||||||
kinematics.setWheelBase(2.f);
|
|
||||||
kinematics.setMaxSpeed(1.f);
|
|
||||||
kinematics.setMaxAngularVelocity(1.f);
|
|
||||||
|
|
||||||
// Negative linear velocity for backward motion and positive yaw rate for turning right
|
|
||||||
EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f));
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
|
|
||||||
{
|
|
||||||
DifferentialDriveKinematics kinematics;
|
|
||||||
kinematics.setWheelBase(1.f);
|
|
||||||
kinematics.setMaxSpeed(1.f);
|
|
||||||
kinematics.setMaxAngularVelocity(1.f);
|
|
||||||
|
|
||||||
// Test rotating in place (zero linear velocity, non-zero yaw rate)
|
|
||||||
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f));
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(DifferentialDriveKinematicsTest, StraightMovementCase)
|
|
||||||
{
|
|
||||||
DifferentialDriveKinematics kinematics;
|
|
||||||
kinematics.setWheelBase(1.f);
|
|
||||||
kinematics.setMaxSpeed(1.f);
|
|
||||||
kinematics.setMaxAngularVelocity(1.f);
|
|
||||||
|
|
||||||
// Test moving straight (non-zero linear velocity, zero yaw rate)
|
|
||||||
EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f));
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(DifferentialDriveKinematicsTest, MinInputValuesCase)
|
|
||||||
{
|
|
||||||
DifferentialDriveKinematics kinematics;
|
|
||||||
kinematics.setWheelBase(FLT_MIN);
|
|
||||||
kinematics.setMaxSpeed(FLT_MIN);
|
|
||||||
kinematics.setMaxAngularVelocity(FLT_MIN);
|
|
||||||
|
|
||||||
// Test with minimum possible input values
|
|
||||||
EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f));
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
|
|
||||||
{
|
|
||||||
DifferentialDriveKinematics kinematics;
|
|
||||||
kinematics.setWheelBase(1.f);
|
|
||||||
kinematics.setMaxSpeed(1.f);
|
|
||||||
kinematics.setMaxAngularVelocity(1.f);
|
|
||||||
|
|
||||||
// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
|
|
||||||
EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f));
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
|
|
||||||
{
|
|
||||||
DifferentialDriveKinematics kinematics;
|
|
||||||
kinematics.setWheelBase(1.f);
|
|
||||||
kinematics.setMaxSpeed(1.f);
|
|
||||||
kinematics.setMaxAngularVelocity(1.f);
|
|
||||||
|
|
||||||
// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
|
|
||||||
EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f));
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(DifferentialDriveKinematicsTest, MaxAngularCase)
|
|
||||||
{
|
|
||||||
DifferentialDriveKinematics kinematics;
|
|
||||||
kinematics.setWheelBase(2.f);
|
|
||||||
kinematics.setMaxSpeed(1.f);
|
|
||||||
kinematics.setMaxAngularVelocity(1.f);
|
|
||||||
|
|
||||||
// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
|
|
||||||
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 10.f), Vector2f(-1.f, 1.f));
|
|
||||||
}
|
|
Loading…
Reference in New Issue