forked from Archive/PX4-Autopilot
Added mecanum module
This commit is contained in:
PerFrivik
parent
892087e336
commit
c0162c4701
|
|
@ -0,0 +1,41 @@
|
|||
#!/bin/sh
|
||||
# @name Aion Robotics R1 Rover
|
||||
# @type Rover
|
||||
# @class Rover
|
||||
|
||||
. ${R}etc/init.d/rc.rover_differential_defaults
|
||||
|
||||
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
|
||||
PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
|
||||
PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover_mecanum}
|
||||
|
||||
param set-default SIM_GZ_EN 1 # Gazebo bridge
|
||||
|
||||
# Simulated sensors
|
||||
param set-default SENS_EN_GPSSIM 1
|
||||
param set-default SENS_EN_BAROSIM 0
|
||||
param set-default SENS_EN_MAGSIM 1
|
||||
param set-default SENS_EN_ARSPDSIM 1
|
||||
|
||||
# Actuator mapping
|
||||
param set-default SIM_GZ_WH_FUNC1 101 # right wheel front
|
||||
param set-default SIM_GZ_WH_MIN1 0
|
||||
param set-default SIM_GZ_WH_MAX1 200
|
||||
param set-default SIM_GZ_WH_DIS1 100
|
||||
|
||||
param set-default SIM_GZ_WH_FUNC2 102 # left wheel front
|
||||
param set-default SIM_GZ_WH_MIN2 0
|
||||
param set-default SIM_GZ_WH_MAX2 200
|
||||
param set-default SIM_GZ_WH_DIS2 100
|
||||
|
||||
param set-default SIM_GZ_WH_FUNC3 103 # right wheel back
|
||||
param set-default SIM_GZ_WH_MIN3 0
|
||||
param set-default SIM_GZ_WH_MAX3 200
|
||||
param set-default SIM_GZ_WH_DIS3 100
|
||||
|
||||
param set-default SIM_GZ_WH_FUNC4 104 # left wheel back
|
||||
param set-default SIM_GZ_WH_MIN4 0
|
||||
param set-default SIM_GZ_WH_MAX4 200
|
||||
param set-default SIM_GZ_WH_DIS4 100
|
||||
|
||||
# param set-default SIM_GZ_WH_REV 1 # reverse right wheel
|
||||
|
|
@ -82,6 +82,7 @@ px4_add_romfs_files(
|
|||
4008_gz_advanced_plane
|
||||
4009_gz_r1_rover
|
||||
4010_gz_x500_mono_cam
|
||||
4011_gz_r1_rover_mecanum
|
||||
|
||||
6011_gazebo-classic_typhoon_h480
|
||||
6011_gazebo-classic_typhoon_h480.post
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@
|
|||
ekf2 start &
|
||||
|
||||
# Start rover differential drive controller.
|
||||
differential_drive start
|
||||
mecanum_drive start
|
||||
|
||||
# Start Land Detector.
|
||||
land_detector start rover
|
||||
|
|
|
|||
|
|
@ -6,6 +6,6 @@ set VEHICLE_TYPE rover_differential
|
|||
param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER
|
||||
|
||||
param set-default CA_AIRFRAME 6 # Rover (Differential)
|
||||
param set-default CA_R_REV 3 # Right and left motors reversible
|
||||
param set-default CA_R_REV 15 # Right and left motors reversible
|
||||
|
||||
param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying
|
||||
|
|
|
|||
|
|
@ -40,6 +40,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
|
|||
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
|
||||
CONFIG_MODULES_MC_POS_CONTROL=y
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=y
|
||||
CONFIG_MODULES_MECANUM_DRIVE=y
|
||||
CONFIG_MODULES_NAVIGATOR=y
|
||||
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
|
||||
CONFIG_MODULES_PAYLOAD_DELIVERER=y
|
||||
|
|
|
|||
|
|
@ -137,6 +137,7 @@ set(msg_files
|
|||
ManualControlSwitches.msg
|
||||
MavlinkLog.msg
|
||||
MavlinkTunnel.msg
|
||||
MecanumDriveSetpoint.msg
|
||||
MessageFormatRequest.msg
|
||||
MessageFormatResponse.msg
|
||||
Mission.msg
|
||||
|
|
|
|||
|
|
@ -0,0 +1,8 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
float32[2] speed # [m/s] collective roll-off speed in body x- and y-axis
|
||||
bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward
|
||||
float32 yaw_rate # [rad/s] yaw rate
|
||||
bool closed_loop_yaw_rate_control # true if yaw rate is controlled using gyroscope feedback, false if direct feed-forward
|
||||
|
||||
# TOPICS mecanum_drive_setpoint mecanum_drive_control_output
|
||||
|
|
@ -437,12 +437,25 @@ bool MixingOutput::update()
|
|||
bool all_disabled = true;
|
||||
_reversible_mask = 0;
|
||||
|
||||
// printf("max num outputs: %d\n", _max_num_outputs);
|
||||
|
||||
for (int i = 0; i < _max_num_outputs; ++i) {
|
||||
|
||||
// printf(" _functions[i]: %p\n", (void *)_functions[i]);
|
||||
// printf("_function_assignment: %d\n", (int)_function_assignment[i]);
|
||||
|
||||
if (_functions[i]) {
|
||||
|
||||
// printf("valid function\n");
|
||||
// printf("armed or prearmed allowprearmcontrol %d\n", _functions[i]->allowPrearmControl());
|
||||
// printf("prearmed %d\n", _armed.prearmed);
|
||||
// printf("armed %d\n", _armed.armed);
|
||||
// printf("nan\n");
|
||||
all_disabled = false;
|
||||
|
||||
if (_armed.armed || (_armed.prearmed && _functions[i]->allowPrearmControl())) {
|
||||
outputs[i] = _functions[i]->value(_function_assignment[i]);
|
||||
printf("outputs[i]: %f\n", (double)outputs[i]);
|
||||
|
||||
} else {
|
||||
outputs[i] = NAN;
|
||||
|
|
@ -471,6 +484,9 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat
|
|||
{
|
||||
bool stop_motors = !_throttle_armed && !_actuator_test.inTestMode();
|
||||
|
||||
// printf("outputs from limitAndUpdateOutputs: %f, %f, %f, %f\n", (double)outputs[0], (double)outputs[1],
|
||||
// (double)outputs[2], (double)outputs[3]);
|
||||
|
||||
if (_armed.lockdown || _armed.manual_lockdown) {
|
||||
// overwrite outputs in case of lockdown with disarmed values
|
||||
for (size_t i = 0; i < _max_num_outputs; i++) {
|
||||
|
|
|
|||
|
|
@ -961,10 +961,14 @@ mixer:
|
|||
actuators:
|
||||
- actuator_type: 'motor'
|
||||
instances:
|
||||
- name: 'Right Motor'
|
||||
position: [ 0, 1., 0 ]
|
||||
- name: 'Left Motor'
|
||||
position: [ 0, -1., 0 ]
|
||||
- name: 'Right Motor Front'
|
||||
position: [ 1, 1., 0 ]
|
||||
- name: 'Left Motor Front'
|
||||
position: [ 1, -1., 0 ]
|
||||
- name: 'Right Motor Back'
|
||||
position: [ -1, 1., 0 ]
|
||||
- name: 'Left Motor Back'
|
||||
position: [ -1, -1., 0 ]
|
||||
|
||||
7: # Motors (6DOF)
|
||||
actuators:
|
||||
|
|
|
|||
|
|
@ -0,0 +1,20 @@
|
|||
{
|
||||
"configurations": [
|
||||
{
|
||||
"browse": {
|
||||
"databaseFilename": "${default}",
|
||||
"limitSymbolsToIncludedHeaders": false
|
||||
},
|
||||
"includePath": [
|
||||
"/opt/ros/humble/include/**",
|
||||
"/usr/include/**"
|
||||
],
|
||||
"name": "ROS",
|
||||
"intelliSenseMode": "gcc-x64",
|
||||
"compilerPath": "/usr/bin/gcc",
|
||||
"cStandard": "gnu11",
|
||||
"cppStandard": "c++14"
|
||||
}
|
||||
],
|
||||
"version": 4
|
||||
}
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
{
|
||||
"python.autoComplete.extraPaths": [
|
||||
"/opt/ros/humble/lib/python3.10/site-packages",
|
||||
"/opt/ros/humble/local/lib/python3.10/dist-packages"
|
||||
],
|
||||
"python.analysis.extraPaths": [
|
||||
"/opt/ros/humble/lib/python3.10/site-packages",
|
||||
"/opt/ros/humble/local/lib/python3.10/dist-packages"
|
||||
]
|
||||
}
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2023-2024 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(MecanumDriveControl)
|
||||
# add_subdirectory(MecanumDriveGuidance)
|
||||
add_subdirectory(MecanumDriveKinematics)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__mecanum_drive
|
||||
MAIN mecanum_drive
|
||||
SRCS
|
||||
MecanumDrive.cpp
|
||||
MecanumDrive.hpp
|
||||
DEPENDS
|
||||
MecanumDriveControl
|
||||
# MecanumDriveGuidance
|
||||
MecanumDriveKinematics
|
||||
px4_work_queue
|
||||
modules__control_allocator # for parameter CA_R_REV
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
)
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
menuconfig MODULES_MECANUM_DRIVE
|
||||
bool "mecanum_drive"
|
||||
default n
|
||||
depends on MODULES_CONTROL_ALLOCATOR
|
||||
---help---
|
||||
Enable support for control of mecanum drive rovers
|
||||
|
|
@ -0,0 +1,195 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023-2024 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 "MecanumDrive.hpp"
|
||||
|
||||
MecanumDrive::MecanumDrive() :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
|
||||
{
|
||||
updateParams();
|
||||
}
|
||||
|
||||
bool MecanumDrive::init()
|
||||
{
|
||||
ScheduleOnInterval(10_ms); // 100 Hz
|
||||
return true;
|
||||
}
|
||||
|
||||
void MecanumDrive::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
|
||||
_mecanum_drive_kinematics.setWheelBase(_param_md_wheel_base.get(), _param_md_wheel_base.get());
|
||||
_mecanum_drive_kinematics.setWheelRadius(_param_md_wheel_radius.get());
|
||||
|
||||
_max_speed = _param_md_wheel_speed.get() * _param_md_wheel_radius.get();
|
||||
// _mecanum_drive_guidance.setMaxSpeed(_max_speed);
|
||||
_mecanum_drive_kinematics.setMaxSpeed(_max_speed, _max_speed);
|
||||
|
||||
_max_angular_velocity = _max_speed / (_param_md_wheel_base.get() / 2.f);
|
||||
// _mecanum_drive_guidance.setMaxAngularVelocity(_max_angular_velocity);
|
||||
_mecanum_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
|
||||
}
|
||||
|
||||
void MecanumDrive::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
}
|
||||
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f;
|
||||
_time_stamp_last = now;
|
||||
|
||||
if (_parameter_update_sub.updated()) {
|
||||
parameter_update_s parameter_update;
|
||||
_parameter_update_sub.copy(¶meter_update);
|
||||
updateParams();
|
||||
}
|
||||
|
||||
if (_vehicle_control_mode_sub.updated()) {
|
||||
vehicle_control_mode_s vehicle_control_mode{};
|
||||
|
||||
if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
|
||||
_mecanum_drive_kinematics.setArmed(vehicle_control_mode.flag_armed);
|
||||
_manual_driving = vehicle_control_mode.flag_control_manual_enabled;
|
||||
_mission_driving = vehicle_control_mode.flag_control_auto_enabled;
|
||||
}
|
||||
}
|
||||
|
||||
if (_vehicle_status_sub.updated()) {
|
||||
vehicle_status_s vehicle_status{};
|
||||
|
||||
if (_vehicle_status_sub.copy(&vehicle_status)) {
|
||||
_acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO);
|
||||
}
|
||||
}
|
||||
|
||||
if (_manual_driving) {
|
||||
// Manual mode
|
||||
// directly produce setpoints from the manual control setpoint (joystick)
|
||||
if (_manual_control_setpoint_sub.updated()) {
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
|
||||
if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
|
||||
mecanum_drive_setpoint_s setpoint{};
|
||||
setpoint.yaw_rate = manual_control_setpoint.throttle * math::max(0.f, _param_md_speed_scale.get()) * _max_speed;
|
||||
setpoint.speed[1] = manual_control_setpoint.yaw * math::max(0.f, _param_md_speed_scale.get()) * _max_speed;
|
||||
setpoint.speed[0] = manual_control_setpoint.roll * _param_md_ang_velocity_scale.get() * _max_angular_velocity;
|
||||
// setpoint.speed[0] = 0;
|
||||
// setpoint.speed[1] = 0.0;
|
||||
// setpoint.yaw_rate = 0;
|
||||
|
||||
// if acro mode, we activate the yaw rate control
|
||||
if (_acro_driving) {
|
||||
setpoint.closed_loop_speed_control = false;
|
||||
setpoint.closed_loop_yaw_rate_control = true;
|
||||
|
||||
} else {
|
||||
setpoint.closed_loop_speed_control = false;
|
||||
setpoint.closed_loop_yaw_rate_control = false;
|
||||
}
|
||||
|
||||
setpoint.timestamp = now;
|
||||
_mecanum_drive_setpoint_pub.publish(setpoint);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// else if (_mission_driving) {
|
||||
// Mission mode
|
||||
// directly receive setpoints from the guidance library
|
||||
// _mecanum_drive_guidance.computeGuidance(
|
||||
// _mecanum_drive_control.getVehicleYaw(),
|
||||
// _mecanum_drive_control.getVehicleBodyYawRate(),
|
||||
// dt
|
||||
// );
|
||||
// }
|
||||
|
||||
_mecanum_drive_control.control(dt);
|
||||
_mecanum_drive_kinematics.allocate();
|
||||
}
|
||||
|
||||
int MecanumDrive::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
MecanumDrive *instance = new MecanumDrive();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int MecanumDrive::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int MecanumDrive::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_ERR("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
Rover Mecanum Drive controller.
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("mecanum_drive", "controller");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int mecanum_drive_main(int argc, char *argv[])
|
||||
{
|
||||
return MecanumDrive::main(argc, argv);
|
||||
}
|
||||
|
|
@ -0,0 +1,104 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023-2024 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 <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/mecanum_drive_setpoint.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
#include "MecanumDriveControl/MecanumDriveControl.hpp"
|
||||
// #include "MecanumDriveGuidance/MecanumDriveGuidance.hpp"
|
||||
#include "MecanumDriveKinematics/MecanumDriveKinematics.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class MecanumDrive : public ModuleBase<MecanumDrive>, public ModuleParams,
|
||||
public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
MecanumDrive();
|
||||
~MecanumDrive() override = default;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
bool init();
|
||||
|
||||
protected:
|
||||
void updateParams() override;
|
||||
|
||||
private:
|
||||
void Run() override;
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Publication<mecanum_drive_setpoint_s> _mecanum_drive_setpoint_pub{ORB_ID(mecanum_drive_setpoint)};
|
||||
|
||||
bool _manual_driving = false;
|
||||
bool _mission_driving = false;
|
||||
bool _acro_driving = false;
|
||||
hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */
|
||||
|
||||
// MecanumDriveGuidance _mecanum_drive_guidance{this};
|
||||
MecanumDriveControl _mecanum_drive_control{this};
|
||||
MecanumDriveKinematics _mecanum_drive_kinematics{this};
|
||||
|
||||
float _max_speed{0.f};
|
||||
float _max_angular_velocity{0.f};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::MD_ANG_SCALE>) _param_md_ang_velocity_scale,
|
||||
(ParamFloat<px4::params::MD_SPEED_SCALE>) _param_md_speed_scale,
|
||||
(ParamFloat<px4::params::MD_WHEEL_BASE>) _param_md_wheel_base,
|
||||
(ParamFloat<px4::params::MD_WHEEL_SPEED>) _param_md_wheel_speed,
|
||||
(ParamFloat<px4::params::MD_WHEEL_RADIUS>) _param_md_wheel_radius
|
||||
)
|
||||
};
|
||||
|
|
@ -0,0 +1,39 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2024 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(MecanumDriveControl
|
||||
MecanumDriveControl.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(MecanumDriveControl PUBLIC pid)
|
||||
target_include_directories(MecanumDriveControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
|
@ -0,0 +1,129 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023-2024 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 "MecanumDriveControl.hpp"
|
||||
|
||||
using namespace matrix;
|
||||
using namespace time_literals;
|
||||
|
||||
MecanumDriveControl::MecanumDriveControl(ModuleParams *parent) : ModuleParams(parent)
|
||||
{
|
||||
pid_init(&_pid_angular_velocity, PID_MODE_DERIVATIV_NONE, 0.001f);
|
||||
pid_init(&_pid_speed, PID_MODE_DERIVATIV_NONE, 0.001f);
|
||||
}
|
||||
|
||||
void MecanumDriveControl::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
|
||||
pid_set_parameters(&_pid_angular_velocity,
|
||||
_param_rdd_p_gain_angular_velocity.get(), // Proportional gain
|
||||
_param_rdd_i_gain_angular_velocity.get(), // Integral gain
|
||||
0, // Derivative gain
|
||||
20, // Integral limit
|
||||
200); // Output limit
|
||||
|
||||
pid_set_parameters(&_pid_speed,
|
||||
_param_rdd_p_gain_speed.get(), // Proportional gain
|
||||
_param_rdd_i_gain_speed.get(), // Integral gain
|
||||
0, // Derivative gain
|
||||
2, // Integral limit
|
||||
200); // Output limit
|
||||
}
|
||||
|
||||
void MecanumDriveControl::control(float dt)
|
||||
{
|
||||
if (_vehicle_angular_velocity_sub.updated()) {
|
||||
vehicle_angular_velocity_s vehicle_angular_velocity{};
|
||||
|
||||
if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) {
|
||||
_vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2];
|
||||
}
|
||||
}
|
||||
|
||||
if (_vehicle_attitude_sub.updated()) {
|
||||
vehicle_attitude_s vehicle_attitude{};
|
||||
|
||||
if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
|
||||
_vehicle_attitude_quaternion = Quatf(vehicle_attitude.q);
|
||||
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
|
||||
}
|
||||
}
|
||||
|
||||
if (_vehicle_local_position_sub.updated()) {
|
||||
vehicle_local_position_s vehicle_local_position{};
|
||||
|
||||
if (_vehicle_local_position_sub.copy(&vehicle_local_position)) {
|
||||
Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
|
||||
Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
|
||||
_vehicle_forward_speed = velocity_in_body_frame(0);
|
||||
}
|
||||
}
|
||||
|
||||
if (_vehicle_status_sub.updated()) {
|
||||
vehicle_status_s vehicle_status;
|
||||
|
||||
if (_vehicle_status_sub.copy(&vehicle_status)) {
|
||||
|
||||
const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
|
||||
_spooled_up = armed && hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s;
|
||||
}
|
||||
}
|
||||
|
||||
_mecanum_drive_setpoint_sub.update(&_mecanum_drive_setpoint);
|
||||
|
||||
// PID to reach setpoint using control_output
|
||||
mecanum_drive_setpoint_s mecanum_drive_control_output = _mecanum_drive_setpoint;
|
||||
|
||||
if (_mecanum_drive_setpoint.closed_loop_speed_control) {
|
||||
mecanum_drive_control_output.speed[0] +=
|
||||
pid_calculate(&_pid_speed, _mecanum_drive_setpoint.speed[0], _vehicle_forward_speed, 0, dt);
|
||||
mecanum_drive_control_output.speed[1] +=
|
||||
pid_calculate(&_pid_speed, _mecanum_drive_setpoint.speed[1], _vehicle_forward_speed, 0, dt);
|
||||
}
|
||||
|
||||
if (_mecanum_drive_setpoint.closed_loop_yaw_rate_control) {
|
||||
mecanum_drive_control_output.yaw_rate +=
|
||||
pid_calculate(&_pid_angular_velocity, _mecanum_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt);
|
||||
}
|
||||
|
||||
|
||||
if (!_spooled_up) {
|
||||
mecanum_drive_control_output.speed[0] = 0.0f;
|
||||
mecanum_drive_control_output.speed[1] = 0.0f;
|
||||
mecanum_drive_control_output.yaw_rate = 0.0f;
|
||||
}
|
||||
|
||||
mecanum_drive_control_output.timestamp = hrt_absolute_time();
|
||||
_mecanum_drive_control_output_pub.publish(mecanum_drive_control_output);
|
||||
}
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023-2024 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file MecanumDriveControl.hpp
|
||||
*
|
||||
* Controller for heading rate and forward speed.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <lib/pid/pid.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/mecanum_drive_setpoint.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
class MecanumDriveControl : public ModuleParams
|
||||
{
|
||||
public:
|
||||
MecanumDriveControl(ModuleParams *parent);
|
||||
~MecanumDriveControl() = default;
|
||||
|
||||
void control(float dt);
|
||||
float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; }
|
||||
float getVehicleYaw() const { return _vehicle_yaw; }
|
||||
|
||||
protected:
|
||||
void updateParams() override;
|
||||
|
||||
private:
|
||||
uORB::Subscription _mecanum_drive_setpoint_sub{ORB_ID(mecanum_drive_setpoint)};
|
||||
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
||||
uORB::Publication<mecanum_drive_setpoint_s> _mecanum_drive_control_output_pub{ORB_ID(mecanum_drive_control_output)};
|
||||
|
||||
mecanum_drive_setpoint_s _mecanum_drive_setpoint{};
|
||||
|
||||
matrix::Quatf _vehicle_attitude_quaternion{};
|
||||
float _vehicle_yaw{0.f};
|
||||
|
||||
// States
|
||||
float _vehicle_body_yaw_rate{0.f};
|
||||
float _vehicle_forward_speed{0.f};
|
||||
|
||||
PID_t _pid_angular_velocity; ///< The PID controller for yaw rate.
|
||||
PID_t _pid_speed; ///< The PID controller for velocity.
|
||||
|
||||
bool _spooled_up{false};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::RDD_P_SPEED>) _param_rdd_p_gain_speed,
|
||||
(ParamFloat<px4::params::RDD_I_SPEED>) _param_rdd_i_gain_speed,
|
||||
(ParamFloat<px4::params::RDD_P_ANG_VEL>) _param_rdd_p_gain_angular_velocity,
|
||||
(ParamFloat<px4::params::RDD_I_ANG_VEL>) _param_rdd_i_gain_angular_velocity,
|
||||
|
||||
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
|
||||
)
|
||||
};
|
||||
|
|
@ -0,0 +1,38 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2023-2024 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(MecanumDriveGuidance
|
||||
MecanumDriveGuidance.cpp
|
||||
)
|
||||
|
||||
target_include_directories(MecanumDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
|
@ -0,0 +1,138 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023-2024 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 "MecanumDriveGuidance.hpp"
|
||||
|
||||
#include <mathlib/math/Limits.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
MecanumDriveGuidance::MecanumDriveGuidance(ModuleParams *parent) : ModuleParams(parent)
|
||||
{
|
||||
updateParams();
|
||||
|
||||
pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f);
|
||||
}
|
||||
|
||||
void MecanumDriveGuidance::computeGuidance(float yaw, float angular_velocity, float dt)
|
||||
{
|
||||
if (_position_setpoint_triplet_sub.updated()) {
|
||||
_position_setpoint_triplet_sub.copy(&_position_setpoint_triplet);
|
||||
}
|
||||
|
||||
if (_vehicle_global_position_sub.updated()) {
|
||||
_vehicle_global_position_sub.copy(&_vehicle_global_position);
|
||||
}
|
||||
|
||||
matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon);
|
||||
matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon);
|
||||
matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon);
|
||||
|
||||
const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1),
|
||||
current_waypoint(0),
|
||||
current_waypoint(1));
|
||||
|
||||
float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0),
|
||||
current_waypoint(1));
|
||||
float heading_error = matrix::wrap_pi(desired_heading - yaw);
|
||||
|
||||
if (_current_waypoint != current_waypoint) {
|
||||
_currentState = GuidanceState::TURNING;
|
||||
}
|
||||
|
||||
// Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly.
|
||||
if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) {
|
||||
_currentState = GuidanceState::GOAL_REACHED;
|
||||
}
|
||||
|
||||
float desired_speed = 0.f;
|
||||
|
||||
switch (_currentState) {
|
||||
case GuidanceState::TURNING:
|
||||
desired_speed = 0.f;
|
||||
|
||||
if (fabsf(heading_error) < 0.05f) {
|
||||
_currentState = GuidanceState::DRIVING;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case GuidanceState::DRIVING: {
|
||||
const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_md_max_jerk.get(),
|
||||
_param_md_max_accel.get(), distance_to_next_wp, 0.0f);
|
||||
_forwards_velocity_smoothing.updateDurations(max_velocity);
|
||||
_forwards_velocity_smoothing.updateTraj(dt);
|
||||
desired_speed = math::interpolate<float>(abs(heading_error), 0.1f, 0.8f,
|
||||
_forwards_velocity_smoothing.getCurrentVelocity(), 0.0f);
|
||||
break;
|
||||
}
|
||||
|
||||
case GuidanceState::GOAL_REACHED:
|
||||
// temporary till I find a better way to stop the vehicle
|
||||
desired_speed = 0.f;
|
||||
heading_error = 0.f;
|
||||
angular_velocity = 0.f;
|
||||
_desired_angular_velocity = 0.f;
|
||||
break;
|
||||
}
|
||||
|
||||
// Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint.
|
||||
float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0,
|
||||
dt) + heading_error;
|
||||
|
||||
mecanum_drive_setpoint_s output{};
|
||||
output.speed = math::constrain(desired_speed, -_max_speed, _max_speed);
|
||||
output.yaw_rate = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity);
|
||||
output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true;
|
||||
output.timestamp = hrt_absolute_time();
|
||||
|
||||
_mecanum_drive_setpoint_pub.publish(output);
|
||||
|
||||
_current_waypoint = current_waypoint;
|
||||
}
|
||||
|
||||
void MecanumDriveGuidance::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
|
||||
pid_set_parameters(&_heading_p_controller,
|
||||
_param_md_p_gain_heading.get(), // Proportional gain
|
||||
0, // Integral gain
|
||||
0, // Derivative gain
|
||||
0, // Integral limit
|
||||
200); // Output limit
|
||||
|
||||
_forwards_velocity_smoothing.setMaxJerk(_param_md_max_jerk.get());
|
||||
_forwards_velocity_smoothing.setMaxAccel(_param_md_max_accel.get());
|
||||
_forwards_velocity_smoothing.setMaxVel(_max_speed);
|
||||
}
|
||||
|
|
@ -0,0 +1,140 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023-2024 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 <px4_platform_common/module_params.h>
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <lib/motion_planning/PositionSmoothing.hpp>
|
||||
#include <lib/motion_planning/VelocitySmoothing.hpp>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/mecanum_drive_setpoint.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
|
||||
#include <lib/pid/pid.h>
|
||||
|
||||
|
||||
/**
|
||||
* @brief Enum class for the different states of guidance.
|
||||
*/
|
||||
enum class GuidanceState {
|
||||
TURNING, ///< The vehicle is currently turning.
|
||||
DRIVING, ///< The vehicle is currently driving straight.
|
||||
GOAL_REACHED ///< The vehicle has reached its goal.
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Class for mecanum drive guidance.
|
||||
*/
|
||||
class MecanumDriveGuidance : public ModuleParams
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for MecanumDriveGuidance.
|
||||
* @param parent The parent ModuleParams object.
|
||||
*/
|
||||
MecanumDriveGuidance(ModuleParams *parent);
|
||||
~MecanumDriveGuidance() = default;
|
||||
|
||||
/**
|
||||
* @brief Compute guidance for the vehicle.
|
||||
* @param global_pos The global position of the vehicle in degrees.
|
||||
* @param current_waypoint The current waypoint the vehicle is heading towards in degrees.
|
||||
* @param next_waypoint The next waypoint the vehicle will head towards after reaching the current waypoint in degrees.
|
||||
* @param vehicle_yaw The yaw orientation of the vehicle in radians.
|
||||
* @param body_velocity The velocity of the vehicle in m/s.
|
||||
* @param angular_velocity The angular velocity of the vehicle in rad/s.
|
||||
* @param dt The time step in seconds.
|
||||
*/
|
||||
void computeGuidance(float yaw, float angular_velocity, float dt);
|
||||
|
||||
/**
|
||||
* @brief Set the maximum speed for the vehicle.
|
||||
* @param max_speed The maximum speed in m/s.
|
||||
* @return The set maximum speed in m/s.
|
||||
*/
|
||||
float setMaxSpeed(float max_speed) { return _max_speed = max_speed; }
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the maximum angular velocity for the vehicle.
|
||||
* @param max_angular_velocity The maximum angular velocity in rad/s.
|
||||
* @return The set maximum angular velocity in rad/s.
|
||||
*/
|
||||
float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; }
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Update the parameters of the module.
|
||||
*/
|
||||
void updateParams() override;
|
||||
|
||||
private:
|
||||
uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
|
||||
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
|
||||
|
||||
uORB::Publication<mecanum_drive_setpoint_s> _mecanum_drive_setpoint_pub{ORB_ID(mecanum_drive_setpoint)};
|
||||
|
||||
position_setpoint_triplet_s _position_setpoint_triplet{};
|
||||
vehicle_global_position_s _vehicle_global_position{};
|
||||
|
||||
GuidanceState _currentState; ///< The current state of guidance.
|
||||
|
||||
float _desired_angular_velocity; ///< The desired angular velocity.
|
||||
|
||||
float _max_speed; ///< The maximum speed.
|
||||
float _max_angular_velocity; ///< The maximum angular velocity.
|
||||
|
||||
matrix::Vector2d _current_waypoint; ///< The current waypoint.
|
||||
|
||||
VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion.
|
||||
PositionSmoothing _position_smoothing; ///< The position smoothing.
|
||||
|
||||
PID_t _heading_p_controller; ///< The PID controller for yaw rate.
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::MD_P_HEADING>) _param_md_p_gain_heading,
|
||||
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
|
||||
(ParamFloat<px4::params::MD_MAX_JERK>) _param_md_max_jerk,
|
||||
(ParamFloat<px4::params::MD_MAX_ACCEL>) _param_md_max_accel
|
||||
)
|
||||
};
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2023-2024 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(MecanumDriveKinematics
|
||||
MecanumDriveKinematics.cpp
|
||||
)
|
||||
|
||||
target_include_directories(MecanumDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
px4_add_functional_gtest(SRC MecanumDriveKinematicsTest.cpp LINKLIBS MecanumDriveKinematics)
|
||||
|
|
@ -0,0 +1,115 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023-2024 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 "MecanumDriveKinematics.hpp"
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
using namespace matrix;
|
||||
using namespace time_literals;
|
||||
|
||||
MecanumDriveKinematics::MecanumDriveKinematics(ModuleParams *parent) : ModuleParams(parent)
|
||||
{}
|
||||
|
||||
void MecanumDriveKinematics::allocate()
|
||||
{
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
if (_mecanum_drive_control_output_sub.updated()) {
|
||||
_mecanum_drive_control_output_sub.copy(&_mecanum_drive_control_output);
|
||||
}
|
||||
|
||||
const bool setpoint_timeout = (_mecanum_drive_control_output.timestamp + 100_ms) < now;
|
||||
|
||||
Vector4f wheel_speeds =
|
||||
computeInverseKinematics(_mecanum_drive_control_output.speed[0], _mecanum_drive_control_output.speed[1],
|
||||
_mecanum_drive_control_output.yaw_rate);
|
||||
|
||||
// printf("wheel_speeds: %f, %f, %f, %f\n", (double)wheel_speeds(0), (double)wheel_speeds(1), (double)wheel_speeds(2),
|
||||
// (double)wheel_speeds(3));
|
||||
|
||||
if (!_armed || setpoint_timeout) {
|
||||
wheel_speeds = {}; // stop
|
||||
}
|
||||
|
||||
wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f);
|
||||
|
||||
// printf("wheel_speeds after all checks: %f, %f, %f, %f\n", (double)wheel_speeds(0), (double)wheel_speeds(1),
|
||||
// (double)wheel_speeds(2),
|
||||
// (double)wheel_speeds(3));
|
||||
|
||||
actuator_motors_s actuator_motors{};
|
||||
actuator_motors.reversible_flags = _param_r_rev.get(); // should be 15 see rc.rover_mecanum_defaults
|
||||
wheel_speeds.copyTo(actuator_motors.control);
|
||||
actuator_motors.timestamp = now;
|
||||
_actuator_motors_pub.publish(actuator_motors);
|
||||
}
|
||||
|
||||
matrix::Vector4f MecanumDriveKinematics::computeInverseKinematics(float linear_velocity_x, float linear_velocity_y,
|
||||
float yaw_rate) const
|
||||
{
|
||||
// if (_max_speed < FLT_EPSILON) {
|
||||
// return Vector4f();
|
||||
// }
|
||||
|
||||
linear_velocity_x = math::constrain(linear_velocity_x, -_vx_max, _vx_max);
|
||||
linear_velocity_y = math::constrain(linear_velocity_y, -_vy_max, _vy_max);
|
||||
yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity);
|
||||
|
||||
// Define input vector and matrix data
|
||||
float input_data[3] = {linear_velocity_x, linear_velocity_y, yaw_rate};
|
||||
Matrix<float, 3, 1> input(input_data);
|
||||
|
||||
float m_data[12] = {1, -1, -(_lx + _ly), 1, 1, (_lx + _ly), 1, 1, -(_lx + _ly), 1, -1, (_lx + _ly)};
|
||||
Matrix<float, 4, 3> m(m_data);
|
||||
|
||||
// Perform matrix-vector multiplication
|
||||
auto result = m * input; // result is Matrix<float, 4, 1>
|
||||
|
||||
// Precompute scale factor
|
||||
const float scale = 1 / _r;
|
||||
|
||||
// Scale the result by 1/_r
|
||||
for (size_t i = 0; i < 4; ++i) {
|
||||
result(i, 0) *= scale; // Efficiently use precomputed scale factor
|
||||
}
|
||||
|
||||
// Initialize Vector4f with the scaled results
|
||||
Vector4f output(result(0, 0), result(1, 0), result(2, 0), result(3, 0));
|
||||
|
||||
// output = {0.1f, 0.5f, -0.5f, -0.1f};
|
||||
|
||||
// printf("output: %f, %f, %f, %f\n", (double)output(0), (double)output(1), (double)output(2), (double)output(3));
|
||||
|
||||
return output;
|
||||
}
|
||||
|
|
@ -0,0 +1,108 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023-2024 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>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/actuator_motors.h>
|
||||
#include <uORB/topics/mecanum_drive_setpoint.h>
|
||||
|
||||
/**
|
||||
* @brief Mecanum Drive Kinematics class for computing the kinematics of a mecanum 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 MecanumDriveKinematics : public ModuleParams
|
||||
{
|
||||
public:
|
||||
MecanumDriveKinematics(ModuleParams *parent);
|
||||
~MecanumDriveKinematics() = default;
|
||||
|
||||
/**
|
||||
* @brief Sets the wheel base of the robot.
|
||||
*
|
||||
* @param wheel_base The distance between the centers of the wheels.
|
||||
*/
|
||||
void setWheelBase(const float lx, const float ly) { _lx = lx; _ly = ly; };
|
||||
|
||||
/**
|
||||
* @brief Sets the maximum speed of the robot.
|
||||
*
|
||||
* @param max_speed The maximum speed of the robot.
|
||||
*/
|
||||
void setMaxSpeed(const float vx_max, const float vy_max) { _vx_max = vx_max; _vy_max = vy_max; };
|
||||
|
||||
/**
|
||||
* @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; };
|
||||
|
||||
void setWheelRadius(const float wheel_radius) {_r = wheel_radius;};
|
||||
|
||||
void setArmed(const bool armed) { _armed = armed; };
|
||||
|
||||
void allocate();
|
||||
|
||||
/**
|
||||
* @brief Computes the inverse kinematics for mecanum 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::Vector4f computeInverseKinematics(float linear_velocity_x, float linear_velocity_y, float yaw_rate) const;
|
||||
|
||||
private:
|
||||
uORB::Subscription _mecanum_drive_control_output_sub{ORB_ID(mecanum_drive_control_output)};
|
||||
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
|
||||
|
||||
mecanum_drive_setpoint_s _mecanum_drive_control_output{};
|
||||
bool _armed = false;
|
||||
|
||||
float _lx{0.f};
|
||||
float _ly{0.f};
|
||||
float _vx_max{0.f};
|
||||
float _vy_max{0.f};
|
||||
float _r{1.f};
|
||||
float _max_angular_velocity{0.f};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
|
||||
)
|
||||
};
|
||||
|
|
@ -0,0 +1,168 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023-2024 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 "MecanumDriveKinematics.hpp"
|
||||
#include <mathlib/math/Functions.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
class MecanumDriveKinematicsTest : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
MecanumDriveKinematics kinematics{nullptr};
|
||||
};
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, AllZeroInputCase)
|
||||
{
|
||||
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_F(MecanumDriveKinematicsTest, InvalidParameterCase)
|
||||
{
|
||||
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_F(MecanumDriveKinematicsTest, UnitCase)
|
||||
{
|
||||
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_F(MecanumDriveKinematicsTest, UnitSaturationCase)
|
||||
{
|
||||
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_F(MecanumDriveKinematicsTest, OppositeUnitSaturationCase)
|
||||
{
|
||||
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_F(MecanumDriveKinematicsTest, RandomCase)
|
||||
{
|
||||
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_F(MecanumDriveKinematicsTest, RotateInPlaceCase)
|
||||
{
|
||||
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_F(MecanumDriveKinematicsTest, StraightMovementCase)
|
||||
{
|
||||
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_F(MecanumDriveKinematicsTest, MinInputValuesCase)
|
||||
{
|
||||
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_F(MecanumDriveKinematicsTest, MaxSpeedLimitCase)
|
||||
{
|
||||
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_F(MecanumDriveKinematicsTest, MaxSpeedForwardsCase)
|
||||
{
|
||||
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_F(MecanumDriveKinematicsTest, MaxAngularCase)
|
||||
{
|
||||
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));
|
||||
}
|
||||
|
|
@ -0,0 +1,122 @@
|
|||
module_name: Mecanum Drive
|
||||
|
||||
parameters:
|
||||
- group: Rover Mecanum Drive
|
||||
definitions:
|
||||
MD_WHEEL_BASE:
|
||||
description:
|
||||
short: Wheel base
|
||||
long: Distance from the center of the right wheel to the center of the left wheel
|
||||
type: float
|
||||
unit: m
|
||||
min: 0.001
|
||||
max: 100
|
||||
increment: 0.001
|
||||
decimal: 3
|
||||
default: 0.5
|
||||
MD_WHEEL_RADIUS:
|
||||
description:
|
||||
short: Wheel radius
|
||||
long: Size of the wheel, half the diameter of the wheel
|
||||
type: float
|
||||
unit: m
|
||||
min: 0.001
|
||||
max: 100
|
||||
increment: 0.001
|
||||
decimal: 3
|
||||
default: 0.1
|
||||
MD_SPEED_SCALE:
|
||||
description:
|
||||
short: Manual speed scale
|
||||
type: float
|
||||
min: 0
|
||||
max: 1
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 1
|
||||
MD_ANG_SCALE:
|
||||
description:
|
||||
short: Manual angular velocity scale
|
||||
type: float
|
||||
min: 0
|
||||
max: 1
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 1
|
||||
MD_WHEEL_SPEED:
|
||||
description:
|
||||
short: Maximum wheel speed
|
||||
type: float
|
||||
unit: rad/s
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 0.3
|
||||
MD_P_HEADING:
|
||||
description:
|
||||
short: Proportional gain for heading controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 1
|
||||
MD_P_SPEED:
|
||||
description:
|
||||
short: Proportional gain for speed controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 1
|
||||
MD_I_SPEED:
|
||||
description:
|
||||
short: Integral gain for ground speed controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 0
|
||||
MD_P_ANG_VEL:
|
||||
description:
|
||||
short: Proportional gain for angular velocity controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 1
|
||||
MD_I_ANG_VEL:
|
||||
description:
|
||||
short: Integral gain for angular velocity controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 0
|
||||
MD_MAX_JERK:
|
||||
description:
|
||||
short: Maximum jerk
|
||||
long: Limit for forwards acc/deceleration change.
|
||||
type: float
|
||||
unit: m/s^3
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 0.5
|
||||
MD_MAX_ACCEL:
|
||||
description:
|
||||
short: Maximum acceleration
|
||||
long: Maximum acceleration is used to limit the acceleration of the rover
|
||||
type: float
|
||||
unit: m/s^2
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 0.5
|
||||
|
|
@ -63,6 +63,8 @@ bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MA
|
|||
{
|
||||
unsigned active_output_count = 0;
|
||||
|
||||
// printf("num_outputs: %d\n", num_outputs);
|
||||
|
||||
for (unsigned i = 0; i < num_outputs; i++) {
|
||||
if (_mixing_output.isFunctionSet(i)) {
|
||||
active_output_count++;
|
||||
|
|
@ -72,6 +74,10 @@ bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MA
|
|||
}
|
||||
}
|
||||
|
||||
// printf("active_output_count: %d\n", active_output_count);
|
||||
|
||||
// active_output_count = 4;
|
||||
|
||||
if (active_output_count > 0) {
|
||||
gz::msgs::Actuators wheel_velocity_message;
|
||||
wheel_velocity_message.mutable_velocity()->Resize(active_output_count, 0);
|
||||
|
|
@ -79,10 +85,14 @@ bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MA
|
|||
for (unsigned i = 0; i < active_output_count; i++) {
|
||||
// Offsetting the output allows for negative values despite unsigned integer to reverse the wheels
|
||||
static constexpr float output_offset = 100.0f;
|
||||
// printf("outputs[%d]: %d\n", i, outputs[i]);
|
||||
float scaled_output = (float)outputs[i] - output_offset;
|
||||
// printf("scaled_output: %f\n", (double)scaled_output);
|
||||
wheel_velocity_message.set_velocity(i, scaled_output);
|
||||
}
|
||||
|
||||
// printf("\n");
|
||||
|
||||
|
||||
if (_actuators_pub.Valid()) {
|
||||
return _actuators_pub.Publish(wheel_velocity_message);
|
||||
|
|
|
|||
|
|
@ -50,6 +50,7 @@ class GZMixingInterfaceWheel : public OutputModuleInterface
|
|||
{
|
||||
public:
|
||||
static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
|
||||
// static constexpr int MAX_ACTUATORS = 4;
|
||||
|
||||
GZMixingInterfaceWheel(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
|
||||
OutputModuleInterface(MODULE_NAME "-actuators-wheel", px4::wq_configurations::rate_ctrl),
|
||||
|
|
|
|||
|
|
@ -33,4 +33,4 @@ actuator_output:
|
|||
min: { min: 0, max: 200, default: 0 }
|
||||
max: { min: 0, max: 200, default: 200 }
|
||||
failsafe: { min: 0, max: 200 }
|
||||
num_channels: 2
|
||||
num_channels: 4
|
||||
|
|
|
|||
Loading…
Reference in New Issue