Skip to content

Commit

Permalink
Added matrix and wrench health cheks
Browse files Browse the repository at this point in the history
  • Loading branch information
Mokaz committed Oct 9, 2023
1 parent fed95a2 commit a5a1940
Show file tree
Hide file tree
Showing 6 changed files with 78 additions and 15 deletions.
24 changes: 22 additions & 2 deletions motion/thruster_allocator/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,6 +1,11 @@
cmake_minimum_required(VERSION 3.8)
project(thruster_allocator)

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
Expand All @@ -11,9 +16,24 @@ find_package(rclcpp REQUIRED)
find_package(vortex_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)

add_executable(thruster_allocator_node src/main.cpp src/allocator_ros.cpp src/pseudoinverse_allocator.cpp)
include_directories(include)

add_executable(thruster_allocator_node
src/main.cpp
src/allocator_ros.cpp
src/pseudoinverse_allocator.cpp
)

ament_target_dependencies(thruster_allocator_node rclcpp geometry_msgs vortex_msgs)
ament_target_dependencies(thruster_allocator_node
rclcpp
geometry_msgs
vortex_msgs
)

install(
DIRECTORY include/
DESTINATION include
)

install(TARGETS
thruster_allocator_node
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,15 +13,19 @@ using namespace std::chrono_literals;
class Allocator : public rclcpp::Node {
public:
explicit Allocator();
void spinOnce();

private:
Eigen::MatrixXd thrust_configuration =
(Eigen::MatrixXd(3, 4) << 0.70711, 0.70711, 0.70711, 0.70711, -0.70711,
0.70711, -0.70711, 0.70711, 0.27738, 0.27738, -0.27738, -0.27738)
.finished(); // Hardcoded thruster config matrix for T_pinv
(Eigen::MatrixXd(3, 4) <<
0.70711, 0.70711, 0.70711, 0.70711,
-0.70711, 0.70711, -0.70711, 0.70711,
0.27738, 0.27738, -0.27738, -0.27738)
.finished();
// Hardcoded thruster config matrix for T_pinv

void spinOnce();
void wrench_callback(const geometry_msgs::msg::Wrench &msg);
bool healthyWrench(const Eigen::VectorXd &v) const;
Eigen::VectorXd wrenchMsgToEigen(const geometry_msgs::msg::Wrench &msg) const;
Eigen::VectorXd wrenchMsgToEigen(const float force_x, const float force_y,
const float torque) const;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,12 +21,18 @@ inline void printMatrix(std::string name, const Eigen::MatrixXd &X) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), ss.str());
}

inline void printVector(std::string name, const Eigen::VectorXd &X) {
std::stringstream ss;
ss << std::endl << name << " = " << std::endl << X;
RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), ss.str());
}

// Calculate the pseudoinverse matrix of the matrix X.
// Return false if the calculations fails.
inline bool calculatePseudoinverse(const Eigen::MatrixXd &X,
Eigen::MatrixXd *X_pinv) {
Eigen::MatrixXd pseudoinverse = X.transpose() * (X * X.transpose()).inverse();

if (isInvalidMatrix(pseudoinverse)) {
return false;
}
Expand Down
42 changes: 37 additions & 5 deletions motion/thruster_allocator/src/allocator_ros.cpp
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
#include "../include/thruster_allocator/allocator_ros.hpp"
#include "../include/thruster_allocator/allocator_utils.hpp"
#include "../include/thruster_allocator/pseudoinverse_allocator.hpp"
#include "thruster_allocator/allocator_ros.hpp"
#include "thruster_allocator/allocator_utils.hpp"
#include "thruster_allocator/pseudoinverse_allocator.hpp"
#include "vortex_msgs/msg/thruster_forces.hpp"

Allocator::Allocator() : Node("thrust_allocator_node") {
Expand Down Expand Up @@ -28,26 +28,44 @@ void Allocator::spinOnce() {
const Eigen::VectorXd body_frame_forces = wrenchMsgToEigen(
body_frame_force_x, body_frame_force_y, body_frame_torque);


// printMatrix("T_pinv", m_pseudoinverse_allocator->T_pinv);

// u vector
Eigen::VectorXd thruster_forces =
m_pseudoinverse_allocator->calculateAllocatedThrust(body_frame_forces);

// TODO: Legg til isValicMatrix sjekk og clampVectorValues (saturateVector)

if(isInvalidMatrix(thruster_forces)){
RCLCPP_ERROR(get_logger(), "ThrusterForces vector invalid");
return;
}

vortex_msgs::msg::ThrusterForces msg_out;
//printVector("thruster_forces", thruster_forces);
arrayEigenToMsg(thruster_forces, &msg_out);
for (int i = 0; i < 4; i++) // 4 thrusters
msg_out.thrust[i] *= m_direction[i];
//for (double d : msg_out.thrust){
// RCLCPP_INFO(this->get_logger(), "msg_out-thrust: '%f'", d);
//}

publisher_->publish(msg_out);
}

void Allocator::wrench_callback(const geometry_msgs::msg::Wrench &msg) {
// const Eigen::VectorXd body_frame_forces = wrenchMsgToEigen(msg); // Wrench
const Eigen::VectorXd body_frame_forces = wrenchMsgToEigen(msg); // Wrench
// health check
if (!healthyWrench(body_frame_forces)) {
RCLCPP_ERROR(get_logger(), "ASV wrench vector invalid, ignoring.");
return;
}

body_frame_force_x = msg.force.x;
body_frame_force_y = msg.force.y;
body_frame_torque = msg.torque.z;
RCLCPP_INFO(this->get_logger(), "I heard: '%f'", msg.force.x);
//RCLCPP_INFO(this->get_logger(), "I heard: '%f'", msg.force.x);
}

Eigen::VectorXd
Expand All @@ -68,6 +86,20 @@ Eigen::VectorXd Allocator::wrenchMsgToEigen(const float force_x,
body_frame_forces(2) = torque; // yaw
return body_frame_forces;
}

bool Allocator::healthyWrench(const Eigen::VectorXd &v) const {
// Check for NaN/Inf
if (isInvalidMatrix(v))
return false;

// Check reasonableness
for (unsigned i = 0; i < v.size(); ++i)
if (std::abs(v[i]) > 100) //c_force_range_limit
return false;

return true;
}

// void Allocator::timer_callback() {
// auto message = geometry_msgs::msg::Wrench();
// message.force.x = 1;
Expand Down
5 changes: 3 additions & 2 deletions motion/thruster_allocator/src/main.cpp
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
#include "../include/thruster_allocator/allocator_ros.hpp"
#include "../include/thruster_allocator/allocator_utils.hpp"
#include "thruster_allocator/allocator_ros.hpp"
#include "thruster_allocator/allocator_utils.hpp"

int main(int argc, char **argv) {

Expand All @@ -8,6 +8,7 @@ int main(int argc, char **argv) {
rclcpp::Rate loop_rate(10);

while (rclcpp::ok()) {
allocator->spinOnce();
rclcpp::spin_some(allocator);
loop_rate.sleep();
}
Expand Down
2 changes: 1 addition & 1 deletion motion/thruster_allocator/src/pseudoinverse_allocator.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
#include "../include/thruster_allocator/pseudoinverse_allocator.hpp"
#include "thruster_allocator/pseudoinverse_allocator.hpp"

PseudoinverseAllocator::PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv)
: T_pinv(T_pinv) {}
Expand Down

0 comments on commit a5a1940

Please sign in to comment.