mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-27 11:00:35 +08:00
feat: spacecraft support (#24734)
* rft: initial merging of controllers for spacecraft vehicles * feat: rate controller nominal * feat: spacecraft tooling for commander and VehicleStatus * feat: spacecraft tooling for commander and VehicleStatus * fix: format * fix: format * fix: remove iostream * fix: remove iostream * feat: spacecraft attitude control and minor refactoring of params * feat: add position controller * fix: format * fix: moved trajectories to new message, removed derivative filters * fix: format * fix: removed extra newline * fix: spacecraft allocation builds * feat: add thrusters to effectivenes, add spacecraft build to cmake, clean comments * feat: required changes for allocation * feat: thruster simulation interface * fix: update maximum and minimums * fix: format * fix: added newline at the end of spacecraft actuator effectiveness * feat: configurable board pwm freq from Kconfig * feat: mavlink compliant spacecraft definition * feat: add orbiter to define * boards: Increase TELEM2 rx buffer size for DDS over serial use-case (ARK Jetson) feat: spacecraft tooling for commander and VehicleStatus fix: format fix: remove iostream feat: mavlink compliant spacecraft definition * feat: add orbiter to define * feat: add orbiter to define * fix: change mav_type to new spacecraft orbiter enum value * fix: build issue * feat: update mavlink * feat: update mavlink to latest master with spacecraft * feat: update mavlink * feat: update mavlink to latest * feat: cleanup and synchronization with new mavlink vehicle definition * fix: get away without specifying spacecraft vehicle * fix: removed unnecessary definition * fix: format * feat: cmake variant for spacecraft * feat: proper mav_type and rc init * fix: removed dart from build system * add: thrusters to actuator type * rft: reordering actuator type * rft: initial merging of controllers for spacecraft vehicles * feat: rate controller nominal * fix: format * feat: spacecraft attitude control and minor refactoring of params * feat: add position controller * fix: format * fix: moved trajectories to new message, removed derivative filters * fix: format * fix: removed extra newline * fix: spacecraft allocation builds * feat: add thrusters to effectivenes, add spacecraft build to cmake, clean comments * feat: required changes for allocation * feat: thruster simulation interface * fix: update maximum and minimums * fix: format * fix: added newline at the end of spacecraft actuator effectiveness * feat: configurable board pwm freq from Kconfig * feat: add orbiter to define * feat: cleanup and synchronization with new mavlink vehicle definition * fix: get away without specifying spacecraft vehicle * fix: conflicts * fix: format * fix: remove duplicate entry * rft: remove Kconfig changes * rft: revert main Kconfig * rft: revert main kcoonfig on platforms * rft: remove changes to board PWm (go on another PR) * rft: revert changes to commander (main is correct) * fix: extra char on commander_helper * rft: removed extra spaces * rft: moved effectiveness to spacecraft * fix: spacecraft effectiveness * fix: extra space * feat: preliminary version, still using thrusters * rft: initial pipeline on PX4 side with rotors instead of thrusters * feat: add atmos model * feat: spacecraft with rotor pipeline tested, working * feat: update gz * rft: removed thruster interfaces * fix: format * fix: remove control allocation * fix: thruster normalization * fix: format * fix: nuttx version * fix: clang tidy error * feat: updated gz to add atmos model * fix: update gz * fix: update mavlink * fix: remove friend class from allocation lib * fix: remove actuator_outputs/motors --------- Co-authored-by: Alexander Lerach <alexander@auterion.com>
This commit is contained in:
Pedro Roque
@@ -56,11 +56,12 @@ if (gz-transport_FOUND)
|
||||
add_subdirectory(template_plugin)
|
||||
add_subdirectory(gstreamer)
|
||||
add_subdirectory(moving_platform_controller)
|
||||
add_subdirectory(spacecraft_thruster)
|
||||
|
||||
# Add an alias target for each plugin
|
||||
if (TARGET GstCameraSystem)
|
||||
add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GstCameraSystem)
|
||||
add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController SpacecraftThrusterModelPlugin TemplatePlugin GstCameraSystem)
|
||||
else()
|
||||
add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin)
|
||||
add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController SpacecraftThrusterModelPlugin TemplatePlugin)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
@@ -0,0 +1,68 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2025 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
# Template for a new plugin project
|
||||
# Replace TemplatePlugin with your plugin name
|
||||
project(SpacecraftThrusterModelPlugin)
|
||||
|
||||
# Add external dependencies if needed
|
||||
# include(${CMAKE_CURRENT_SOURCE_DIR}/dependency.cmake)
|
||||
|
||||
# Find required packages
|
||||
# find_package(PackageName REQUIRED)
|
||||
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
# Add your source files here
|
||||
SpacecraftThrusterModel.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(${PROJECT_NAME}
|
||||
PUBLIC px4_gz_msgs
|
||||
PUBLIC gz-sensors${gz-sensors_VERSION_MAJOR}::gz-sensors${gz-sensors_VERSION_MAJOR}
|
||||
PUBLIC gz-plugin${gz-plugin_VERSION_MAJOR}::gz-plugin${gz-plugin_VERSION_MAJOR}
|
||||
PUBLIC gz-sim${gz-sim_VERSION_MAJOR}::gz-sim${gz-sim_VERSION_MAJOR}
|
||||
PUBLIC gz-transport${gz-transport_VERSION_MAJOR}::gz-transport${gz-transport_VERSION_MAJOR}
|
||||
# Add other dependencies as needed
|
||||
# PUBLIC ${OtherLib_LIBS}
|
||||
)
|
||||
|
||||
target_include_directories(${PROJECT_NAME}
|
||||
PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
|
||||
PUBLIC px4_gz_msgs
|
||||
# Add other include directories as needed
|
||||
# PUBLIC ${OtherLib_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
# Add dependencies if needed
|
||||
# add_dependencies(${PROJECT_NAME} ExternalDependency)
|
||||
@@ -0,0 +1,389 @@
|
||||
/*
|
||||
* Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
|
||||
* Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
|
||||
* Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
|
||||
* Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
|
||||
* Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
|
||||
* Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
|
||||
* Copyright (C) 2024 Open Source Robotics Foundation
|
||||
* Copyright (C) 2024 Benjamin Perseghetti, Rudis Laboratories
|
||||
* Copyright (C) 2024 Pedro Roque, DCS, KTH, Sweden
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "SpacecraftThrusterModel.hpp"
|
||||
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <optional>
|
||||
#include <chrono>
|
||||
|
||||
#include <gz/msgs/actuators.pb.h>
|
||||
|
||||
#include <gz/common/Profiler.hh>
|
||||
|
||||
#include <gz/plugin/Register.hh>
|
||||
#include <gz/transport/Node.hh>
|
||||
|
||||
#include <gz/math/Helpers.hh>
|
||||
#include <gz/math/Pose3.hh>
|
||||
#include <gz/math/Vector3.hh>
|
||||
#include <gz/msgs/Utility.hh>
|
||||
|
||||
#include <sdf/sdf.hh>
|
||||
|
||||
#include "gz/sim/components/Actuators.hh"
|
||||
#include "gz/sim/components/ExternalWorldWrenchCmd.hh"
|
||||
#include "gz/sim/components/Pose.hh"
|
||||
#include "gz/sim/Link.hh"
|
||||
#include "gz/sim/Model.hh"
|
||||
#include "gz/sim/Util.hh"
|
||||
|
||||
using namespace gz;
|
||||
using namespace sim;
|
||||
using namespace systems;
|
||||
|
||||
class gz::sim::systems::SpacecraftThrusterModelPrivate
|
||||
{
|
||||
/// \brief Callback for actuator commands.
|
||||
public: void OnActuatorMsg(const msgs::Actuators &_msg);
|
||||
|
||||
/// \brief Apply link forces and moments based on propeller state.
|
||||
public: void UpdateForcesAndMoments(EntityComponentManager &_ecm);
|
||||
|
||||
/// \brief Link Entity
|
||||
public: Entity linkEntity;
|
||||
|
||||
/// \brief Link name
|
||||
public: std::string linkName;
|
||||
|
||||
/// \brief Model interface
|
||||
public: Model model{kNullEntity};
|
||||
|
||||
/// \brief sub topic for actuator commands.
|
||||
public: std::string subTopic;
|
||||
|
||||
/// \brief Topic namespace.
|
||||
public: std::string topic;
|
||||
|
||||
/// \brief Simulation time tracker
|
||||
public: double simTime = 0.01;
|
||||
|
||||
/// \brief Index of motor in Actuators msg on multirotor_base.
|
||||
public: int actuatorNumber = 0;
|
||||
|
||||
/// \brief Duty cycle frequency
|
||||
public: double dutyCycleFrequency = 10.0;
|
||||
|
||||
/// \brief Cycle start time
|
||||
public: double cycleStartTime = 0.0;
|
||||
|
||||
/// \brief Sampling time with the cycle period.
|
||||
public: double samplingTime = 0.01;
|
||||
|
||||
/// \brief Actuator maximum thrust
|
||||
public: double maxThrust = 0.0;
|
||||
|
||||
/// \brief Maximum duty cycle value. Use the same value as in PX4
|
||||
/// maximum PWM_<channel>_MAXi value. For the thruster, it is assumed
|
||||
/// that the minimum value is 0 (solenoid closed).
|
||||
public: double maxDutyCycle = 10000.0;
|
||||
|
||||
/// \brief Received Actuators message. This is nullopt if no message has been
|
||||
/// received.
|
||||
public: std::optional<msgs::Actuators> recvdActuatorsMsg;
|
||||
|
||||
/// \brief Mutex to protect recvdActuatorsMsg.
|
||||
public: std::mutex recvdActuatorsMsgMutex;
|
||||
|
||||
/// \brief Gazebo communication node.
|
||||
public: transport::Node node;
|
||||
};
|
||||
|
||||
//////////////////////////////////////////////////
|
||||
SpacecraftThrusterModel::SpacecraftThrusterModel()
|
||||
: dataPtr(std::make_unique<SpacecraftThrusterModelPrivate>())
|
||||
{
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////
|
||||
void SpacecraftThrusterModel::Configure(const Entity &_entity,
|
||||
const std::shared_ptr<const sdf::Element> &_sdf,
|
||||
EntityComponentManager &_ecm,
|
||||
EventManager &/*_eventMgr*/)
|
||||
{
|
||||
this->dataPtr->model = Model(_entity);
|
||||
|
||||
if (!this->dataPtr->model.Valid(_ecm)) {
|
||||
gzerr << "SpacecraftThrusterModel plugin should be attached to a model "
|
||||
<< "entity. Failed to initialize." << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
auto sdfClone = _sdf->Clone();
|
||||
|
||||
this->dataPtr->topic.clear();
|
||||
|
||||
if (sdfClone->HasElement("topic")) {
|
||||
this->dataPtr->topic =
|
||||
sdfClone->Get<std::string>("topic");
|
||||
|
||||
} else {
|
||||
gzwarn << "No topic set using entity name.\n";
|
||||
this->dataPtr->topic = this->dataPtr->model.Name(_ecm);
|
||||
}
|
||||
|
||||
if (sdfClone->HasElement("link_name")) {
|
||||
this->dataPtr->linkName = sdfClone->Get<std::string>("link_name");
|
||||
}
|
||||
|
||||
if (this->dataPtr->linkName.empty()) {
|
||||
gzerr << "SpacecraftThrusterModel found an empty link_name parameter. "
|
||||
<< "Failed to initialize.";
|
||||
return;
|
||||
}
|
||||
|
||||
if (sdfClone->HasElement("actuator_number")) {
|
||||
this->dataPtr->actuatorNumber =
|
||||
sdfClone->GetElement("actuator_number")->Get<int>();
|
||||
|
||||
} else {
|
||||
gzerr << "Please specify a actuator_number.\n";
|
||||
}
|
||||
|
||||
if (sdfClone->HasElement("max_thrust")) {
|
||||
this->dataPtr->maxThrust =
|
||||
sdfClone->GetElement("max_thrust")->Get<double>();
|
||||
|
||||
} else {
|
||||
gzerr << "Please specify actuator "
|
||||
<< this->dataPtr->actuatorNumber << " max_thrust.\n";
|
||||
}
|
||||
|
||||
if (sdfClone->HasElement("duty_cycle_frequency")) {
|
||||
this->dataPtr->dutyCycleFrequency =
|
||||
sdfClone->GetElement("duty_cycle_frequency")->Get<double>();
|
||||
|
||||
} else {
|
||||
gzerr << "Please specify actuator "
|
||||
<< this->dataPtr->actuatorNumber << " duty_cycle_frequency.\n";
|
||||
}
|
||||
|
||||
if (sdfClone->HasElement("max_duty_cycle")) {
|
||||
this->dataPtr->maxDutyCycle =
|
||||
sdfClone->GetElement("max_duty_cycle")->Get<double>();
|
||||
|
||||
if (this->dataPtr->maxDutyCycle <= 0.0) {
|
||||
gzerr << "Please specify a positive max_duty_cycle.\n";
|
||||
return;
|
||||
}
|
||||
|
||||
} else {
|
||||
gzerr << "Please specify actuator "
|
||||
<< this->dataPtr->actuatorNumber << " max_duty_cycle.\n";
|
||||
}
|
||||
|
||||
std::string topic;
|
||||
|
||||
if (sdfClone->HasElement("sub_topic")) {
|
||||
this->dataPtr->subTopic =
|
||||
sdfClone->Get<std::string>("sub_topic");
|
||||
topic = transport::TopicUtils::AsValidTopic(
|
||||
this->dataPtr->topic + "/" + this->dataPtr->subTopic);
|
||||
|
||||
} else {
|
||||
topic = transport::TopicUtils::AsValidTopic(
|
||||
this->dataPtr->topic);
|
||||
}
|
||||
|
||||
// Subscribe to actuator command message
|
||||
if (topic.empty()) {
|
||||
gzerr << "Failed to create topic for [" << this->dataPtr->topic
|
||||
<< "]" << std::endl;
|
||||
return;
|
||||
|
||||
} else {
|
||||
gzdbg << "Listening to topic: " << topic << std::endl;
|
||||
}
|
||||
|
||||
this->dataPtr->node.Subscribe(topic,
|
||||
&SpacecraftThrusterModelPrivate::OnActuatorMsg, this->dataPtr.get());
|
||||
|
||||
// Look for components
|
||||
// If the link hasn't been identified yet, look for it
|
||||
if (this->dataPtr->linkEntity == kNullEntity) {
|
||||
this->dataPtr->linkEntity =
|
||||
this->dataPtr->model.LinkByName(_ecm, this->dataPtr->linkName);
|
||||
}
|
||||
|
||||
if (this->dataPtr->linkEntity == kNullEntity) {
|
||||
gzerr << "Failed to find link entity. "
|
||||
<< "Failed to initialize." << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
// skip UpdateForcesAndMoments if needed components are missing
|
||||
bool providedAllComponents = true;
|
||||
|
||||
if (!_ecm.Component<components::WorldPose>(this->dataPtr->linkEntity)) {
|
||||
_ecm.CreateComponent(this->dataPtr->linkEntity, components::WorldPose());
|
||||
}
|
||||
|
||||
if (!providedAllComponents) {
|
||||
gzdbg << "Created necessary components." << std::endl;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////
|
||||
void SpacecraftThrusterModelPrivate::OnActuatorMsg(
|
||||
const msgs::Actuators &_msg)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(this->recvdActuatorsMsgMutex);
|
||||
this->recvdActuatorsMsg = _msg;
|
||||
|
||||
if (this->actuatorNumber == 0) {
|
||||
gzdbg << "Received actuator message!" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////
|
||||
void SpacecraftThrusterModel::PreUpdate(const UpdateInfo &_info,
|
||||
EntityComponentManager &_ecm)
|
||||
{
|
||||
GZ_PROFILE("SpacecraftThrusterModel::PreUpdate");
|
||||
|
||||
// \TODO(anyone) Support rewind
|
||||
if (_info.dt < std::chrono::steady_clock::duration::zero()) {
|
||||
gzwarn << "Detected jump back in time ["
|
||||
<< std::chrono::duration_cast<std::chrono::seconds>(_info.dt).count()
|
||||
<< "s]. System may not work properly." << std::endl;
|
||||
}
|
||||
|
||||
// Nothing left to do if paused.
|
||||
if (_info.paused) {
|
||||
return;
|
||||
}
|
||||
|
||||
this->dataPtr->simTime = std::chrono::duration<double>(_info.simTime).count();
|
||||
this->dataPtr->UpdateForcesAndMoments(_ecm);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////
|
||||
void SpacecraftThrusterModelPrivate::UpdateForcesAndMoments(
|
||||
EntityComponentManager &_ecm)
|
||||
{
|
||||
GZ_PROFILE("SpacecraftThrusterModelPrivate::UpdateForcesAndMoments");
|
||||
std::optional<msgs::Actuators> msg;
|
||||
auto actuatorMsgComp =
|
||||
_ecm.Component<components::Actuators>(this->model.Entity());
|
||||
|
||||
// Actuators messages can come in from transport or via a component. If a
|
||||
// component is available, it takes precedence.
|
||||
if (actuatorMsgComp) {
|
||||
msg = actuatorMsgComp->Data();
|
||||
|
||||
} else {
|
||||
std::lock_guard<std::mutex> lock(this->recvdActuatorsMsgMutex);
|
||||
|
||||
if (this->recvdActuatorsMsg.has_value()) {
|
||||
msg = *this->recvdActuatorsMsg;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg.has_value()) {
|
||||
if (this->actuatorNumber > msg->velocity_size() - 1) {
|
||||
gzerr << "You tried to access index " << this->actuatorNumber
|
||||
<< " of the Actuator array which is of size "
|
||||
<< msg->velocity_size() << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
// Normalize input
|
||||
double normalizedInput =
|
||||
msg->velocity(this->actuatorNumber) / this->maxDutyCycle;
|
||||
|
||||
// METHOD:
|
||||
//
|
||||
// targetDutyCycle starts as a normalized value between 0 and 1, so we
|
||||
// need to convert it to the corresponding time in the duty cycle
|
||||
// period
|
||||
// |________| |________
|
||||
// | ^ | | |
|
||||
// __| | |____| |__
|
||||
// a b c d
|
||||
// a: cycle start time
|
||||
// b: sampling time
|
||||
// c: target duty cycle
|
||||
// d: cycle period
|
||||
double targetDutyCycle = normalizedInput * (1.0 / this->dutyCycleFrequency);
|
||||
|
||||
if (this->actuatorNumber == 0)
|
||||
gzdbg << this->actuatorNumber
|
||||
<< ": target duty cycle: " << targetDutyCycle << std::endl;
|
||||
|
||||
// Calculate cycle start time
|
||||
if (this->samplingTime >= 1.0 / this->dutyCycleFrequency) {
|
||||
if (this->actuatorNumber == 0)
|
||||
gzdbg << this->actuatorNumber
|
||||
<< ": Cycle completed. Resetting cycle start time."
|
||||
<< std::endl;
|
||||
|
||||
this->cycleStartTime = this->simTime;
|
||||
}
|
||||
|
||||
// Calculate sampling time instant within the cycle
|
||||
this->samplingTime = this->simTime - this->cycleStartTime;
|
||||
|
||||
if (this->actuatorNumber == 0)
|
||||
gzdbg << this->actuatorNumber
|
||||
<< ": PWM Period: " << 1.0 / this->dutyCycleFrequency
|
||||
<< " Cycle Start time: " << this->cycleStartTime
|
||||
<< " Sampling time: " << this->samplingTime << std::endl;
|
||||
|
||||
// Apply force if the sampling time is less than the target ON duty cycle
|
||||
double force = this->samplingTime <= targetDutyCycle ? this->maxThrust : 0.0;
|
||||
|
||||
if (targetDutyCycle < 1e-9) { force = 0.0; }
|
||||
|
||||
if (this->actuatorNumber == 0)
|
||||
gzdbg << this->actuatorNumber
|
||||
<< ": Force: " << force
|
||||
<< " Sampling time: " << this->samplingTime
|
||||
<< " Tgt duty cycle: " << targetDutyCycle << std::endl;
|
||||
|
||||
// Apply force to the link
|
||||
Link link(this->linkEntity);
|
||||
const auto worldPose = link.WorldPose(_ecm);
|
||||
link.AddWorldForce(_ecm,
|
||||
worldPose->Rot().RotateVector(math::Vector3d(0, 0, force)));
|
||||
|
||||
if (this->actuatorNumber == 0)
|
||||
gzdbg << this->actuatorNumber
|
||||
<< ": Input Value: " << msg->velocity(this->actuatorNumber)
|
||||
<< " Calc. Force: " << force << std::endl;
|
||||
}
|
||||
|
||||
GZ_ADD_PLUGIN(SpacecraftThrusterModel,
|
||||
System,
|
||||
SpacecraftThrusterModel::ISystemConfigure,
|
||||
SpacecraftThrusterModel::ISystemPreUpdate)
|
||||
|
||||
GZ_ADD_PLUGIN_ALIAS(SpacecraftThrusterModel,
|
||||
"gz::sim::systems::SpacecraftThrusterModel")
|
||||
@@ -0,0 +1,89 @@
|
||||
/*
|
||||
* Copyright (C) 2019 Open Source Robotics Foundation
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*
|
||||
*/
|
||||
#ifndef GZ_SIM_SYSTEMS_SPACECRAFTTHRUSTERMODEL_HH_
|
||||
#define GZ_SIM_SYSTEMS_SPACECRAFTTHRUSTERMODEL_HH_
|
||||
|
||||
#include <gz/sim/System.hh>
|
||||
#include <memory>
|
||||
|
||||
namespace gz
|
||||
{
|
||||
namespace sim
|
||||
{
|
||||
// Inline bracket to help doxygen filtering.
|
||||
inline namespace GZ_SIM_VERSION_NAMESPACE
|
||||
{
|
||||
namespace systems
|
||||
{
|
||||
// Forward declaration
|
||||
class SpacecraftThrusterModelPrivate;
|
||||
|
||||
/// \brief This system applies a thrust force to models with RCS-like
|
||||
/// thrusters. See tutorials/spacecraft_thrusters.md for a tutorial usage.
|
||||
/// Below follow the minimum necessary parameters needed by the plugin:
|
||||
/// \param link_name Name of the link that the thruster is attached to.
|
||||
/// \param actuator_number Index of the element to be used from the
|
||||
/// actuators message for a joint.
|
||||
/// \param duty_cycle_frequency Frequency of the duty cycle signal in Hz.
|
||||
/// \param max_thrust Maximum thrust force in Newtons, applied on the
|
||||
/// «on» phase of the duty cycle.
|
||||
/// \param topic Name of the topic where the commanded normalized
|
||||
/// thrust is published. Unit is <0, 1>, corresponding to the
|
||||
/// percentage of the duty cycle that the thruster is on.
|
||||
/// Default uses the models name.
|
||||
/// \param sub_topic [optional] Name of the sub_topic to listen to actuator
|
||||
/// message on.
|
||||
///
|
||||
/// This plugin replicates the PWM thruster behavior in:
|
||||
/// Nakka, Yashwanth Kumar, et al. "A six degree-of-freedom spacecraft
|
||||
/// dynamics simulator for formation control research." 2018 AAS/AIAA
|
||||
/// Astrodynamics Specialist Conference. 2018. -> 'Thruster Firing Time'
|
||||
/// Phodapol, S. (2023). Predictive Controllers for Load Transportation in
|
||||
/// Microgravity Environments (Dissertation). -> '5.3.4 PWM Controller Node'
|
||||
/// Retrieved from https://urn.kb.se/resolve?urn=urn:nbn:se:kth:diva-344440
|
||||
|
||||
class SpacecraftThrusterModel
|
||||
: public System,
|
||||
public ISystemConfigure,
|
||||
public ISystemPreUpdate
|
||||
{
|
||||
/// \brief Constructor
|
||||
public: SpacecraftThrusterModel();
|
||||
|
||||
/// \brief Destructor
|
||||
public: ~SpacecraftThrusterModel() override = default;
|
||||
|
||||
// Documentation inherited
|
||||
public: void Configure(const Entity &_entity,
|
||||
const std::shared_ptr<const sdf::Element> &_sdf,
|
||||
EntityComponentManager &_ecm,
|
||||
EventManager &_eventMgr) override;
|
||||
|
||||
// Documentation inherited
|
||||
public: void PreUpdate(
|
||||
const gz::sim::UpdateInfo &_info,
|
||||
gz::sim::EntityComponentManager &_ecm) override;
|
||||
|
||||
/// \brief Private data pointer
|
||||
private: std::unique_ptr<SpacecraftThrusterModelPrivate> dataPtr;
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user