diff --git a/.github/workflows/catkin_lint.yml b/.github/workflows/catkin_lint.yml index 971df95f9..6c6046751 100644 --- a/.github/workflows/catkin_lint.yml +++ b/.github/workflows/catkin_lint.yml @@ -28,4 +28,4 @@ jobs: wstool init wstool merge aerial_robot_noetic.rosinstall wstool update - ROS_DISTRO=noetic catkin_lint --resolve-env --strict $PWD --skip-path kalman_filter --skip-path ublox_gps --skip-path aerial_robot_3rdparty --skip-path rosserial + ROS_DISTRO=noetic catkin_lint --resolve-env --strict $PWD --skip-path kalman_filter --skip-path ublox_gps --skip-path aerial_robot_3rdparty --skip-path rosserial --skip-path livox_ros_driver2 --skip-path fast_lio diff --git a/.travis.sh b/.travis.sh index 89eb8bd9a..df8f561a0 100644 --- a/.travis.sh +++ b/.travis.sh @@ -59,6 +59,16 @@ if [ ${ROS_DISTRO} = 'kinetic' ]; then wget https://raw.githubusercontent.com/osrf/gazebo_models/master/ground_plane/model.config -P ${path} fi +if [[ "$ROS_DISTRO" = "kinetic" ]]; then + # to use c++17 + sudo apt-get install -y software-properties-common + sudo add-apt-repository ppa:ubuntu-toolchain-r/test + sudo apt update + sudo apt install -y g++-7 + export CXX='g++-7' + export CC='gcc-7' +fi + # Build catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON catkin build -p1 -j1 --no-status diff --git a/aerial_robot_base/CMakeLists.txt b/aerial_robot_base/CMakeLists.txt index 72a5a4522..22418aa88 100644 --- a/aerial_robot_base/CMakeLists.txt +++ b/aerial_robot_base/CMakeLists.txt @@ -14,7 +14,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_python_setup() -add_compile_options(-std=c++14) +add_compile_options(-std=c++17) catkin_package( INCLUDE_DIRS include diff --git a/aerial_robot_base/bin/rosbag_control_data.sh b/aerial_robot_base/bin/rosbag_control_data.sh index 00486cea9..4a99aa277 100755 --- a/aerial_robot_base/bin/rosbag_control_data.sh +++ b/aerial_robot_base/bin/rosbag_control_data.sh @@ -1,3 +1,3 @@ #!/bin/bash -rosrun aerial_robot_base rosbag_raw_sensors.sh $1 rosout rosout_agg $1/four_axes/command $1/rpy/pid $1/rpy/gain $1/debug/pose/pid $1/desire_coordinate $1/flight_config_cmd $1/kf/imu1/data $1/kf/alt1/data $1/kf/gps1/data $1/kf/mocap1/data $1/kf/vo1/data $1/ground_truth $1/debug/four_axes/gain $1/motor_info $1/p_matrix_pseudo_inverse_inertia $1/servo/torque_enable $1/servo/target_states /tf_static /tf $1/uav/baselink/odom $1/uav/cog/odom $1/uav/full_state $1/uav_info $1/uav_power $1/uav/nav $1/joints_ctrl $1/joint_states ${@:2} +rosrun aerial_robot_base rosbag_raw_sensors.sh $1 rosout rosout_agg $1/four_axes/command $1/rpy/pid $1/rpy/gain $1/debug/pose/pid $1/desire_coordinate $1/flight_config_cmd $1/kf/imu1/data $1/kf/alt1/data $1/kf/gps1/data $1/kf/mocap1/data $1/kf/vo1/data $1/ground_truth $1/debug/four_axes/gain $1/motor_info $1/p_matrix_pseudo_inverse_inertia $1/servo/torque_enable $1/servo/target_states /tf_static /tf $1/uav/baselink/odom $1/uav/cog/odom $1/uav/full_state $1/uav_info $1/uav_power $1/uav/nav $1/joints_ctrl $1/joint_states $1/joy ${@:2} diff --git a/aerial_robot_base/config/sensors/lio/livox_mid360.yaml b/aerial_robot_base/config/sensors/lio/livox_mid360.yaml new file mode 100644 index 000000000..22b1a3456 --- /dev/null +++ b/aerial_robot_base/config/sensors/lio/livox_mid360.yaml @@ -0,0 +1,16 @@ +sensor_plugin: + vo: + vo_sub_topic_name: Odometry_precede + level_pos_noise_sigma: 0.01 # 0.01 is better that 0.001 + z_pos_noise_sigma: 0.01 # 0.01 is better than 0.001 + z_vel_mode: false + vel_noise_sigma: 0.01 # TODO: modification + sensor_frame: lidar_imu + reference_frame: fc + vio_mode: true + local_vel_mode: false # the twist/twist/velocity is described in global frame + time_sync: false + reset_duration: 0.5 + + fusion_mode: 2 # ONLY_POS_MODE = 0, ONLY_VEL_MODE = 1, POS_VEL_MODE = 2 + #param_verbose: true diff --git a/aerial_robot_base/launch/external_module/mocap.launch b/aerial_robot_base/launch/external_module/mocap.launch index 8a083dac1..728a66bdd 100644 --- a/aerial_robot_base/launch/external_module/mocap.launch +++ b/aerial_robot_base/launch/external_module/mocap.launch @@ -6,8 +6,7 @@ type="mocap_node" name="mocap_node" respawn="false" - launch-prefix="" - required="true"> + launch-prefix=""> rigid_bodies: '$(arg robot_id)': diff --git a/aerial_robot_control/CMakeLists.txt b/aerial_robot_control/CMakeLists.txt index 001c62619..edfd207a1 100644 --- a/aerial_robot_control/CMakeLists.txt +++ b/aerial_robot_control/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.0.2) project(aerial_robot_control) -add_compile_options(-std=c++11) +add_compile_options(-std=c++17) find_package(catkin REQUIRED COMPONENTS aerial_robot_estimation @@ -38,7 +38,7 @@ set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O3 -g -DNDEBUG") catkin_package( INCLUDE_DIRS include - LIBRARIES control_utils flight_control_pluginlib flight_navigation + LIBRARIES control_utils flight_control_pluginlib flight_navigation trajectory_generation CATKIN_DEPENDS aerial_robot_estimation aerial_robot_model aerial_robot_msgs dynamic_reconfigure pluginlib roscpp spinal tf ) @@ -69,13 +69,38 @@ add_dependencies(flight_control_pluginlib ${PROJECT_NAME}_gencfg) add_library (flight_navigation src/flight_navigation.cpp) target_link_libraries (flight_navigation ${catkin_LIBRARIES}) +### trajectory generate based on dodgelib +add_library (trajectory_generation + src/trajectory/reference_base.cpp + src/trajectory/trajectory_reference/polynomial.cpp + src/trajectory/trajectory_reference/polynomial_trajectory.cpp + src/trajectory/trajectory_reference/sampled_trajectory.cpp + src/trajectory/base/parameter_base.cpp + src/trajectory/math/math.cpp + src/trajectory/types/command.cpp + src/trajectory/types/quad_state.cpp + src/trajectory/types/quadrotor.cpp + src/trajectory/utils/logger.cpp + src/trajectory/utils/timer.cpp + ) +target_link_libraries (trajectory_generation ${catkin_LIBRARIES}) + +add_executable(trajectory_generation_node src/trajectory/test.cpp) +target_link_libraries (trajectory_generation_node ${catkin_LIBRARIES} trajectory_generation) + + + install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(TARGETS control_utils flight_control_pluginlib flight_navigation +install(TARGETS control_utils flight_control_pluginlib flight_navigation trajectory_generation DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) +install(TARGETS trajectory_generation_node + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + install(DIRECTORY scripts plugins DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS diff --git a/aerial_robot_control/include/aerial_robot_control/control/base/pose_linear_controller.h b/aerial_robot_control/include/aerial_robot_control/control/base/pose_linear_controller.h index fdcb82a00..8dc30d75a 100644 --- a/aerial_robot_control/include/aerial_robot_control/control/base/pose_linear_controller.h +++ b/aerial_robot_control/include/aerial_robot_control/control/base/pose_linear_controller.h @@ -84,7 +84,7 @@ namespace aerial_robot_control tf::Vector3 pos_, target_pos_; tf::Vector3 vel_, target_vel_; - tf::Vector3 target_acc_; + tf::Vector3 target_acc_, target_ang_acc_; tf::Vector3 rpy_, target_rpy_; tf::Vector3 omega_, target_omega_; diff --git a/aerial_robot_control/include/aerial_robot_control/control/under_actuated_lqi_controller.h b/aerial_robot_control/include/aerial_robot_control/control/under_actuated_lqi_controller.h index dff98b775..1a8ab4299 100644 --- a/aerial_robot_control/include/aerial_robot_control/control/under_actuated_lqi_controller.h +++ b/aerial_robot_control/include/aerial_robot_control/control/under_actuated_lqi_controller.h @@ -105,6 +105,7 @@ namespace aerial_robot_control virtual void sendCmd() override; virtual void sendFourAxisCommand(); + Eigen::MatrixXd getQInv(); virtual void allocateYawTerm(); void cfgLQICallback(aerial_robot_control::LQIConfig &config, uint32_t level); //dynamic reconfigure diff --git a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h index 9bf3cec77..6af31d67d 100644 --- a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h +++ b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -13,6 +14,8 @@ #include #include #include +#include +#include namespace aerial_robot_navigation { @@ -75,6 +78,7 @@ namespace aerial_robot_navigation inline tf::Vector3 getTargetAcc() {return target_acc_;} inline tf::Vector3 getTargetRPY() {return target_rpy_;} inline tf::Vector3 getTargetOmega() {return target_omega_;} + inline tf::Vector3 getTargetAngAcc() {return target_ang_acc_;} inline void setTargetPos(tf::Vector3 pos) { target_pos_ = pos; } inline void setTargetPos(double x, double y, double z) { setTargetPos(tf::Vector3(x, y, z)); } @@ -88,12 +92,22 @@ namespace aerial_robot_navigation inline void setTargetZeroAcc() { setTargetAcc(tf::Vector3(0,0,0)); } inline void setTargetRoll(float value) { target_rpy_.setX(value); } + inline void setTargetOmega(tf::Vector3 omega) { target_omega_ = omega; } + inline void setTargetOmega(double x, double y, double z) { setTargetOmega(tf::Vector3(x, y, z)); } + inline void setTargetZeroOmega() { setTargetOmega(0,0,0); } inline void setTargetOmegaX(float value) { target_omega_.setX(value); } inline void setTargetPitch(float value) { target_rpy_.setY(value); } inline void setTargetOmegaY(float value) { target_omega_.setY(value); } inline void setTargetYaw(float value) { target_rpy_.setZ(value); } inline void addTargetYaw(float value) { setTargetYaw(angles::normalize_angle(target_rpy_.z() + value)); } inline void setTargetOmegaZ(float value) { target_omega_.setZ(value); } + inline void setTargetAngAcc(tf::Vector3 acc) { target_ang_acc_ = acc; } + inline void setTargetAngAcc(double x, double y, double z) { setTargetAngAcc(tf::Vector3(x, y, z)); } + inline void setTargetZeroAngAcc() { setTargetAngAcc(tf::Vector3(0,0,0)); } + inline void setTargetAngAccX(double value) { target_ang_acc_.setX(value); } + inline void setTargetAngAccY(double value) { target_ang_acc_.setY(value); } + inline void setTargetAngAccZ(double value) { target_ang_acc_.setZ(value); } + inline void setTargetPosX( float value){ target_pos_.setX(value);} inline void setTargetVelX( float value){ target_vel_.setX(value);} inline void setTargetAccX( float value){ target_acc_.setX(value);} @@ -116,6 +130,8 @@ namespace aerial_robot_navigation uint8_t getEstimateMode(){ return estimate_mode_;} void setEstimateMode(uint8_t estimate_mode){ estimate_mode_ = estimate_mode;} + void generateNewTrajectory(geometry_msgs::PoseStamped pose); + static constexpr uint8_t POS_CONTROL_COMMAND = 0; static constexpr uint8_t VEL_CONTROL_COMMAND = 1; @@ -219,7 +235,10 @@ namespace aerial_robot_navigation ros::Publisher flight_config_pub_; ros::Publisher power_info_pub_; ros::Publisher flight_state_pub_; + ros::Publisher path_pub_; ros::Subscriber navi_sub_; + ros::Subscriber pose_sub_; + ros::Subscriber simple_move_base_goal_sub_; ros::Subscriber battery_sub_; ros::Subscriber flight_status_ack_sub_; ros::Subscriber takeoff_sub_; @@ -266,7 +285,7 @@ namespace aerial_robot_navigation /* target value */ tf::Vector3 target_pos_, target_vel_, target_acc_; - tf::Vector3 target_rpy_, target_omega_; + tf::Vector3 target_rpy_, target_omega_, target_ang_acc_; double takeoff_height_; double init_height_; @@ -307,17 +326,28 @@ namespace aerial_robot_navigation std::string teleop_local_frame_; + /* trajectory */ + double trajectory_mean_vel_; + double trajectory_mean_yaw_rate_; + double trajectory_min_du_; + bool enable_latch_yaw_trajectory_; + std::shared_ptr traj_generator_ptr_; + /* battery info */ double low_voltage_thre_; bool low_voltage_flag_; + double high_voltage_cell_thre_; + bool high_voltage_flag_; int bat_cell_; double bat_resistance_; double bat_resistance_voltage_rate_; double hovering_current_; virtual void rosParamInit(); - void naviCallback(const aerial_robot_msgs::FlightNavConstPtr & msg); - void joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg); + void poseCallback(const geometry_msgs::PoseStampedConstPtr & msg); + void simpleMoveBaseGoalCallback(const geometry_msgs::PoseStampedConstPtr & msg); + virtual void naviCallback(const aerial_robot_msgs::FlightNavConstPtr & msg); + virtual void joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg); void batteryCheckCallback(const std_msgs::Float32ConstPtr &msg); virtual void halt() {} @@ -433,9 +463,6 @@ namespace aerial_robot_navigation if(!teleop_flag_) return; setNaviState(LAND_STATE); - - setTargetXyFromCurrentState(); - setTargetYawFromCurrentState(); ROS_INFO("Land state"); } diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/base/module.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/base/module.hpp new file mode 100644 index 000000000..5d961e3f8 --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/base/module.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include + +#include "aerial_robot_control/trajectory/utils/logger.hpp" + +namespace agi { + +template +class Module { + public: + Module(const std::string& name) : logger_(name) {} + virtual ~Module() {} + + inline const std::string& name() const { return logger_.name(); } + virtual void logTiming() const {}; + + protected: + Logger logger_; +}; + +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/base/parameter_base.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/base/parameter_base.hpp new file mode 100644 index 000000000..97475e344 --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/base/parameter_base.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include +#include +#include + +#include "aerial_robot_control/trajectory/math/types.hpp" +#include "aerial_robot_control/trajectory/utils/filesystem.hpp" +#include "aerial_robot_control/trajectory/utils/yaml.hpp" + +namespace agi { + + +struct ParameterException : public std::exception { + ParameterException() = default; + ParameterException(const std::string& msg) + : msg(std::string("Dodgelib Parameter Exception: ") + msg) {} + const char* what() const throw() { return msg.c_str(); } + + const std::string msg{"Dodgelib Parameter Exception"}; +}; + +struct ParameterBase { + virtual ~ParameterBase() = default; + virtual bool load(const fs::path& filename); + virtual bool load(const Yaml& yaml); + + virtual bool valid() const; +}; + +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/math/gravity.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/math/gravity.hpp new file mode 100644 index 000000000..489aa236e --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/math/gravity.hpp @@ -0,0 +1,22 @@ +#pragma once +#include "aerial_robot_control/trajectory/math/types.hpp" + +namespace agi { + +/** + * Gravity Value [m/s^2] + * + * This is the gravity value used in the whole project. + */ +static constexpr Scalar G = 9.8066; + + +/** + * Gravity Vector [m/s^2] + * + * This is the gravity vector pointing in negative z-direction. + * It uses the value of #G. + */ +const Vector<3> GVEC{0, 0, -G}; + +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/math/math.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/math/math.hpp new file mode 100644 index 000000000..13587c8cd --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/math/math.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include + +namespace agi { + +Matrix<3, 3> skew(const Vector<3>& v); + +Matrix<4, 4> Q_left(const Quaternion& q); + +Matrix<4, 4> Q_right(const Quaternion& q); + +Matrix<4, 3> qFromQeJacobian(const Quaternion& q); + +Matrix<4, 4> qConjugateJacobian(); + +Matrix<3, 3> qeRotJacobian(const Quaternion& q, const Matrix<3, 1>& t); + +Matrix<3, 3> qeInvRotJacobian(const Quaternion& q, const Matrix<3, 1>& t); + +void matrixToTripletList(const SparseMatrix& matrix, + std::vector* const list, + const int row_offset = 0, const int col_offset = 0); + +void matrixToTripletList(const Matrix<>& matrix, + std::vector* const list, + const int row_offset = 0, const int col_offset = 0); + +void insert(const SparseMatrix& from, SparseMatrix* const into, + const int row_offset = 0, const int col_offset = 0); + +void insert(const Matrix<>& from, SparseMatrix* const into, + const int row_offset = 0, const int col_offset = 0); + +void insert(const Matrix<>& from, Matrix<>* const into, + const int row_offset = 0, const int col_offset = 0); + +Vector<> clip(const Vector<>& v, const Vector<>& bound); + +inline constexpr Scalar toRad(const Scalar angle_deg) { + return angle_deg * M_PI / 180.0; +} +inline constexpr Scalar toDeg(const Scalar angle_deg) { + return angle_deg / M_PI * 180.0; +} + +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/math/types.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/math/types.hpp new file mode 100644 index 000000000..431f5297a --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/math/types.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include +#include + +namespace agi { + +// Define the scalar type used. +using Scalar = double; +static constexpr Scalar INF = std::numeric_limits::infinity(); + +// Define `Dynamic` matrix size. +static constexpr int Dynamic = Eigen::Dynamic; + +// Using shorthand for `Matrix` with scalar type. +template +using Matrix = Eigen::Matrix; + +// Using shorthand for `Vector` with scalar type. +template +using Vector = Matrix; + +// Using shorthand for `Array` with scalar type. +template +using Array = Eigen::Array; + +template +using ArrayVector = Array; + +// Using `SparseMatrix` with type. +using SparseMatrix = Eigen::SparseMatrix; + +// Using SparseTriplet with type. +using SparseTriplet = Eigen::Triplet; + +// Using `Quaternion` with type. +using Quaternion = Eigen::Quaternion; + +// Using `Ref` for modifier references. +template +using Ref = Eigen::Ref; + +// Using `ConstRef` for constant references. +template +using ConstRef = const Eigen::Ref; + +// Using `Map`. +template +using Map = Eigen::Map; + +using DynamicsFunction = + std::function>, Ref>)>; +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/reference_base.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/reference_base.hpp new file mode 100644 index 000000000..775946f93 --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/reference_base.hpp @@ -0,0 +1,51 @@ +#pragma once + +#include +#include +#include + +#include "aerial_robot_control/trajectory/base/module.hpp" +#include "aerial_robot_control/trajectory/math/types.hpp" +#include "aerial_robot_control/trajectory/types/setpoint.hpp" +#include "aerial_robot_control/trajectory/utils/logger.hpp" + +namespace agi { + +class ReferenceBase { + public: + ReferenceBase(const std::string& name = "ReferenceBase"); + ReferenceBase(const QuadState& state, const Scalar duration, + const std::string& name = "ReferenceBase"); + virtual ~ReferenceBase(); + + virtual Setpoint getSetpoint(const QuadState& state, const Scalar t) = 0; + virtual Setpoint getSetpoint(const QuadState& state) final; + virtual Setpoint getSetpoint(const Scalar t, + const Scalar heading = NAN) final; + virtual Setpoint getStartSetpoint(); + virtual Setpoint getEndSetpoint(); + + inline Scalar getStartTime() const { return start_state_.t; } + inline Scalar getEndTime() const { return start_state_.t + duration_; } + inline Scalar getDuration() const { return duration_; } + bool isTimeInRange(const Scalar time) const; + std::string time_in_HH_MM_SS_MMM(const Scalar time) const; + + virtual bool valid() const { return start_state_.valid(); } + const std::string& name() const { return name_; } + virtual bool isHover() const { return false; } + virtual bool isVelocityRefernce() const { return false; } + virtual bool isAbsolute() const { return true; } + + bool truncate(const Scalar& t); + friend std::ostream& operator<<(std::ostream& os, const ReferenceBase& ref); + + protected: + QuadState start_state_; + Scalar duration_; + const std::string name_; +}; + +using ReferenceVector = std::vector>; + +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial.hpp new file mode 100644 index 000000000..7b48f0155 --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial.hpp @@ -0,0 +1,74 @@ +#pragma once + +#include "aerial_robot_control/trajectory/math/types.hpp" + +namespace agi { + +template>> +class Polynomial { + public: + Polynomial(); + Polynomial(const int order, const Vector<>& weights = Vector<4>(0, 0, 0, 1), + const int continuity = -1); + Polynomial(const Polynomial& rhs) = default; + ~Polynomial() = default; + + bool scale(const Scalar start_time = NAN, const Scalar duration = NAN); + int addConstraint(const Scalar time, const Vector<>& constraint); + bool solve(); + bool eval(const Scalar time, Ref> state) const; + Scalar operator()(const Scalar time, const int order = 0) const; + + inline int order() const noexcept { return order_; } + inline int size() const noexcept { return order_ + 1; } + bool solved() const { return c_.allFinite(); } + + Matrix<> H() const { return H_; } + Matrix<> A() const { return A_; } + Vector<> b() const { return b_; } + Matrix<> exponents() const { return exponents_.transpose(); } + Matrix<> alpha() const { return alpha_.transpose(); } + Vector<> coeffs() const { return c_; } + + Scalar tauFromTime(const Scalar t) const; + Vector<> tauVector(const Scalar tau, const int order = 0) const; + Vector<> tauVectorFromTime(const Scalar time, const int order = 0) const; + Matrix<> polyMatrix(const Scalar tau, const int order) const; + Matrix<> polyMatrixFromTime(const Scalar time, const int order) const; + + void reset(); + + private: + Matrix<> createH(const Vector<>& weights) const; + bool solve(const Matrix<>& H, const Matrix<>& A, const Vector<>& b); + + const int order_; + const int continuity_; + Vector<> c_; + const Array<> exponents_; + const Array<> alpha_; + + Vector<> weights_; // Weights on derivatives starting from velocity, + Matrix<> H_ = Matrix<>::Constant(1, order_, NAN); + Matrix<> A_ = Matrix<>::Constant(1, order_, NAN); + Vector<> b_ = Vector<>::Constant(1, NAN); + + Scalar t_scale_{1.0}; + Scalar t_offset_{0.0}; +}; + +// Closed-Form Minimum Jerk Specialization +template<> +Polynomial::Polynomial(const int order, const Vector<>& weights, + const int continuity) = delete; + +template<> +bool Polynomial::solve(const Matrix<>& H, const Matrix<>& A, + const Vector<>& b) = delete; + +class ClosedFormMinJerkAxis : public Polynomial { + public: + using Polynomial::Polynomial; +}; + +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp new file mode 100644 index 000000000..4f198071d --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp @@ -0,0 +1,118 @@ + +#pragma once + +#include "aerial_robot_control/trajectory/reference_base.hpp" +#include "aerial_robot_control/trajectory/trajectory_reference/polynomial.hpp" +#include "aerial_robot_control/trajectory/types/quadrotor.hpp" + +namespace agi { + +template> +class PolynomialTrajectory : public ReferenceBase { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PolynomialTrajectory(const QuadState& start_state, const QuadState& end_state, + const Vector<>& weights = Vector<4>(0, 0, 0, 1), + const int order = 11, const int continuity = -1, + const std::string& name = "Polynomial Trajectory"); + PolynomialTrajectory(const std::vector& states, + const Vector<>& weights = Vector<4>(0, 0, 0, 1), + const int order = 11, const int continuity = -1, + const std::string& name = "Polynomial Trajectory"); + virtual ~PolynomialTrajectory() = default; + + using ReferenceBase::getSetpoint; + Setpoint getSetpoint(const QuadState& state, const Scalar time) override; + QuadState getState(const Scalar time) const; + virtual Setpoint getStartSetpoint() override final; + virtual Setpoint getEndSetpoint() override final; + + bool addStateConstraint(const QuadState& state, int ord = -1); + bool solved() const; + bool valid() const override; + + // utilities + Vector<> evalTranslation(const Scalar time, const int order = 0) const; + Scalar findTimeMaxAcc(const Scalar precision = 1e-3) const; + Scalar findTimeMaxOmega(const Scalar precision = 1e-3) const; + Scalar findTimeMaxAcc(const Scalar t_start, const Scalar t_end, + const Scalar precision = 1e-3) const; + Scalar findTimeMaxOmega(const Scalar t_start, const Scalar t_end, + const Scalar precision = 1e-3) const; + void scale(const Scalar start_time = NAN, const Scalar duration = NAN); + Scalar scaleToLimits(const Quadrotor& quad, const int iterations = 10, + const Scalar tolerance = 1e-4); + Scalar scaleToLimits(const Scalar acc_limit, const int iterations = 10, + const Scalar tolerance = 1e-4); + + void setForwardHeading(const bool forward); + + protected: + template + Scalar findTime(const Scalar dt, const Scalar dt_min, const Scalar t_start, + const Scalar t_end, EvalFunc eval, CompFunc comp) const; + + QuadState end_state_; + PolyType x_; + PolyType y_; + PolyType z_; + PolyType yaw_; + + std::vector states_; + + mutable Quaternion q_pitch_roll_last_{1, 0, 0, 0}; + mutable Scalar yaw_last_{0.0}; + bool forward_heading_{false}; +}; + +// Child classes +class MinSnapTrajectory : public PolynomialTrajectory> { + public: + MinSnapTrajectory(const QuadState& start_state, const QuadState& end_state, + const int order = 11, const int continuity = 3, + const std::string& name = "Minimum Snap Trajectory") + : PolynomialTrajectory(start_state, end_state, Vector<4>(0, 0, 0, 1), order, + continuity, name) {} + MinSnapTrajectory(const std::vector& states, + const int order = 11, const int continuity = 3, + const std::string& name = "Minimum Snap Trajectory") + : PolynomialTrajectory(states, Vector<4>(0, 0, 0, 1), order, + continuity, name) {} +}; + +class MinJerkTrajectory : public PolynomialTrajectory> { + public: + MinJerkTrajectory(const QuadState& start_state, const QuadState& end_state, + const int order = 11, const int continuity = 2, + const std::string& name = "Minimum Jerk Trajectory") + : PolynomialTrajectory(start_state, end_state, Vector<3>(0, 0, 1), order, + continuity, name) {} + MinJerkTrajectory(const std::vector& states, + const int order = 11, const int continuity = 2, + const std::string& name = "Minimum Jerk Trajectory") + : PolynomialTrajectory(states, Vector<3>(0, 0, 1), order, + continuity, name) {} +}; + +// Closed-Form Minimum-Jerk Specialization +template<> +PolynomialTrajectory::PolynomialTrajectory( + const QuadState& start_state, const QuadState& end_state, + const Vector<>& weights, const int order, const int continuity, + const std::string& name); + +template<> +PolynomialTrajectory::PolynomialTrajectory( + const std::vector& states, const Vector<>& weights, + const int order, const int continuity,const std::string& name) = delete; + +class ClosedFormMinJerkTrajectory + : public PolynomialTrajectory { + public: + ClosedFormMinJerkTrajectory(const QuadState& start_state, + const QuadState& end_state) + : PolynomialTrajectory(start_state, end_state, Vector<3>(0, 0, 1), 5, 3, + "Closed-Form Mininum Jerk Trajectory") {} +}; + +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/sampled_trajectory.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/sampled_trajectory.hpp new file mode 100644 index 000000000..01f249c33 --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/sampled_trajectory.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include "aerial_robot_control/trajectory/reference_base.hpp" + +namespace agi { + +class SampledTrajectory : public ReferenceBase { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + SampledTrajectory(const SetpointVector& setpoints); + virtual ~SampledTrajectory() = default; + + virtual Setpoint getSetpoint(const QuadState& state, const Scalar t) override; + virtual Setpoint getStartSetpoint() override; + virtual Setpoint getEndSetpoint() override; + + private: + Setpoint interpolateSetpoints(const Setpoint& setpoint_1, + const Setpoint& setpoint_2, + const Scalar x) const; + SetpointVector setpoints_; +}; + +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/types/command.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/types/command.hpp new file mode 100644 index 000000000..b64e1bb04 --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/types/command.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include "aerial_robot_control/trajectory/math/types.hpp" + +namespace agi { + +struct Command { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Command(); + + Command(const Scalar t, const Scalar thrust, const Vector<3>& omega); + + Command(const Scalar t, const Vector<4>& thrusts); + + Command(const Scalar t); + + bool valid() const; + bool isSingleRotorThrusts() const; + bool isRatesThrust() const; + + /// time in [s] + Scalar t{NAN}; + + /// Collective mass-normalized thrust in [m/s^2] + Scalar collective_thrust{NAN}; + + /// Bodyrates in [rad/s] + Vector<3> omega{NAN, NAN, NAN}; + + /// Single rotor thrusts in [N] + Vector<4> thrusts{NAN, NAN, NAN, NAN}; + + friend std::ostream& operator<<(std::ostream& os, const Command& command); + + bool operator==(const Command& rhs) const; +}; + +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/types/pose.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/types/pose.hpp new file mode 100644 index 000000000..097c50fa0 --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/types/pose.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include "aerial_robot_control/trajectory/math/types.hpp" + +namespace agi { + +struct Pose { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Pose() = default; + Pose(const Pose&) = default; + ~Pose() = default; + + static constexpr int SIZE = 7; + + bool valid() const { + return std::isfinite(t) && position.allFinite() && + attitude.coeffs().allFinite(); + } + + Scalar t{NAN}; + Vector<3> position{NAN, NAN, NAN}; + Quaternion attitude{NAN, NAN, NAN, NAN}; +}; + + +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/types/quad_state.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/types/quad_state.hpp new file mode 100644 index 000000000..00e501d31 --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/types/quad_state.hpp @@ -0,0 +1,130 @@ +#pragma once + +#include + +#include "aerial_robot_control/trajectory/math/types.hpp" + +namespace agi { + +struct QuadState { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum IDX : int { + POS = 0, + POSX = 0, + POSY = 1, + POSZ = 2, + NPOS = 3, + ATT = 3, + ATTW = 3, + ATTX = 4, + ATTY = 5, + ATTZ = 6, + NATT = 4, + VEL = 7, + VELX = 7, + VELY = 8, + VELZ = 9, + NVEL = 3, + OME = 10, + OMEX = 10, + OMEY = 11, + OMEZ = 12, + NOME = 3, + ACC = 13, + ACCX = 13, + ACCY = 14, + ACCZ = 15, + NACC = 3, + TAU = 16, + TAUX = 16, + TAUY = 17, + TAUZ = 18, + NTAU = 3, + JERK = 19, + JERKX = 19, + JERKY = 20, + JERKZ = 21, + NJERK = 3, + SNAP = 22, + SNAPX = 22, + SNAPY = 23, + SNAPZ = 24, + NSNAP = 3, + BOME = 25, + BOMEX = 25, + BOMEY = 26, + BOMEZ = 27, + NBOME = 3, + BACC = 28, + BACCX = 28, + BACCY = 29, + BACCZ = 30, + NBACC = 3, + MOT = 31, + MOT1 = 31, + MOT2 = 32, + MOT3 = 33, + MOT4 = 34, + NMOT = 4, + MOTDES = 35, + MOTDES1 = 35, + MOTDES2 = 36, + MOTDES3 = 37, + MOTDES4 = 38, + NMOTDES = 4, + SIZE = 39, + DYN = 19 + }; + + QuadState(); + QuadState(const Scalar t, const Vector& x); + QuadState(const Scalar t, const Vector<3>& position, const Scalar yaw); + QuadState(const QuadState& state); + ~QuadState(); + + inline static int size() { return SIZE; } + + Quaternion q() const; + void q(const Quaternion quaternion); + void q(const Scalar angle, const Vector<3>& axis = Vector<3>::UnitZ()); + Matrix<3, 3> R() const; + + void setZero(const bool& reset_time = true); + void linearize(); + inline bool valid() const { return x.allFinite() && std::isfinite(t); } + + Vector x = Vector::Constant(NAN); + Scalar t{NAN}; + + Ref> p{x.segment(IDX::POS)}; + Ref> qx{x.segment(IDX::ATT)}; + Ref> v{x.segment(IDX::VEL)}; + Ref> w{x.segment(IDX::OME)}; + Ref> a{x.segment(IDX::ACC)}; + Ref> tau{x.segment(IDX::TAU)}; + Ref> j{x.segment(IDX::JERK)}; + Ref> s{x.segment(IDX::SNAP)}; + Ref> bw{x.segment(IDX::BOME)}; + Ref> ba{x.segment(IDX::BACC)}; + Ref> mot{x.segment(IDX::MOT)}; + Ref> motdes{x.segment(IDX::MOTDES)}; + + Scalar getYaw(const Scalar yaw = NAN) const; + void setYaw(const Scalar angle); + QuadState getHoverState() const; + + bool operator==(const QuadState& rhs) const; + bool isApprox(const QuadState& rhs, const Scalar tol = 1e-6) const; + + inline bool operator<(const Scalar time) const { return t < time; } + inline bool operator<=(const Scalar time) const { return t <= time; } + inline bool operator>(const Scalar time) const { return t > time; } + inline bool operator>=(const Scalar time) const { return t >= time; } + + friend std::ostream& operator<<(std::ostream& os, const QuadState& state); +}; + +using QS = QuadState; + +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/types/quadrotor.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/types/quadrotor.hpp new file mode 100644 index 000000000..cdee5d3ac --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/types/quadrotor.hpp @@ -0,0 +1,86 @@ +#pragma once + +#include + +#include "aerial_robot_control/trajectory/base/parameter_base.hpp" +#include "aerial_robot_control/trajectory/math/types.hpp" +#include "aerial_robot_control/trajectory/types/quad_state.hpp" + +namespace agi { + +struct Quadrotor : public ParameterBase { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum class RotorConfig { plus, cross }; + + Quadrotor(const Scalar m, const Scalar l); + Quadrotor(); + + bool dynamics(const QuadState& state, QuadState* const derivative) const; + + bool dynamics(const Ref> state, + Ref> derivative) const; + + bool jacobian(const Ref> state, + Ref> jac) const; + + bool jacobian(const Ref> state, + SparseMatrix& jac) const; + + DynamicsFunction getDynamicsFunction() const; + + using ParameterBase::load; + bool load(const Yaml& node) override; + bool valid() const override; + + // Helpers to apply limits. + Vector<4> clampThrust(const Vector<4> thrusts) const; + Scalar clampThrust(const Scalar thrust) const; + Scalar clampCollectiveThrust(const Scalar thrust) const; + Vector<4> clampMotorOmega(const Vector<4>& omega) const; + Vector<3> clampBodyrates(const Vector<3>& omega) const; + + inline Scalar collective_thrust_min() const { return 4.0 * thrust_min_ / m_; } + inline Scalar collective_thrust_max() const { return 4.0 * thrust_max_ / m_; } + + // Helpers for conversion + Vector<4> motorOmegaToThrust(const Vector<4>& omega) const; + Vector<4> motorOmegaToTorque(const Vector<4>& omega) const; + Vector<4> motorThrustToOmega(const Vector<4>& thrusts) const; + + // Getter Functions for member variables + Matrix<4, 4> getAllocationMatrix() const; + + friend std::ostream& operator<<(std::ostream& os, const Quadrotor& quad); + + // Quadrotor physics + Scalar m_; + Matrix<3, 4> t_BM_; + Matrix<3, 3> J_; + Matrix<3, 3> J_inv_; + + // Motor + Scalar motor_omega_min_; + Scalar motor_omega_max_; + Scalar motor_tau_inv_; + + // Propellers + Vector<3> thrust_map_; + Vector<3> torque_map_; + Scalar kappa_; + Scalar thrust_min_; + Scalar thrust_max_; + RotorConfig rotors_config_; + + // Quadrotor limits + Vector<3> omega_max_; + + // Simple cubic aerodynamic model f= c_1 * v +_ c_3 * v^3. + Vector<3> aero_coeff_1_; + Vector<3> aero_coeff_3_; + + // Forward-flight induced thrust variation fz -= c_h * (vx^2 + vy^2) + Scalar aero_coeff_h_; +}; + +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/types/setpoint.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/types/setpoint.hpp new file mode 100644 index 000000000..6f20ac931 --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/types/setpoint.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include + +#include "aerial_robot_control/trajectory/types/command.hpp" +#include "aerial_robot_control/trajectory/types/quad_state.hpp" + +namespace agi { + +struct Setpoint { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Setpoint() = default; + Setpoint(const QuadState& state, const Command& input) + : state(state), input(input) {} + + QuadState state; + Command input; +}; + +using SetpointVector = std::vector; + +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/utils/filesystem.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/utils/filesystem.hpp new file mode 100644 index 000000000..6f11d6e02 --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/utils/filesystem.hpp @@ -0,0 +1,9 @@ +#if __has_include() +#include +namespace fs = std::filesystem; +#elif __has_include() +#include +namespace fs = std::experimental::filesystem; +#else +#error "no filesystem support found" +#endif \ No newline at end of file diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/utils/logger.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/utils/logger.hpp new file mode 100644 index 000000000..8999ce6a8 --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/utils/logger.hpp @@ -0,0 +1,121 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "aerial_robot_control/trajectory/math/types.hpp" + +namespace agi { + +#ifndef DEBUG_LOG +namespace { +struct NoPrint { + template + constexpr NoPrint operator<<(const T&) const noexcept { + return NoPrint(); + } + constexpr NoPrint operator<<(std::ostream& (*)(std::ostream&)) const // + noexcept { + return NoPrint(); + } +}; +} // namespace +#endif + +struct PublishLogContainer { + Vector<> data; + bool advertise; +}; + + +class Logger { + public: + Logger(const std::string& name, const bool color = true); + ~Logger(); + + inline std::streamsize precision(const std::streamsize n); + inline void scientific(const bool on = true); + + void info(const char* msg, ...) const; + void warn(const char* msg, ...) const; + void error(const char* msg, ...) const; + void fatal(const char* msg, ...) const; + +#ifdef DEBUG_LOG + void debug(const char* msg, ...) const; + std::ostream& debug() const; + void debug(const std::function&& lambda) const; +#else + inline constexpr void debug(const char*, ...) const noexcept {} + inline constexpr NoPrint debug() const { return NoPrint(); } + inline constexpr void debug(const std::function&&) const // + noexcept {} +#endif + + inline void addPublishingVariable(const std::string& var_name, + const Vector<>& vec) const { + publishing_variables_[name_ + var_name].advertise = + false; // Not only advertising + publishing_variables_[name_ + var_name].data = vec; + } + + inline void addPublishingVariable(const std::string& var_name, + Scalar val) const { + addPublishingVariable(var_name, Vector<1>(val)); + } + + inline void advertisePublishingVariable(const std::string& var_name) const { + publishing_variables_[name_ + var_name].advertise = + true; // Only advertising + } + + inline void erasePublishingVariable(const std::string var_name) { + publishing_variables_.erase(name_ + var_name); + } + + template + std::ostream& operator<<(const T& printable) const; + + inline const std::string& name() const { return formatted_name_; } + + static constexpr int MAX_CHARS = 256; + + static void for_each_instance( + std::function + function) { + for (auto& pub_var : publishing_variables_) { + function(pub_var.first, pub_var.second); + } + } + + private: + static constexpr int DEFAULT_PRECISION = 3; + static constexpr int NAME_PADDING = 15; + static constexpr char RESET[] = "\033[0m"; + static constexpr char RED[] = "\033[31m"; + static constexpr char YELLOW[] = "\033[33m"; + static constexpr char INFO[] = "Info: "; + static constexpr char WARN[] = "Warning: "; + static constexpr char ERROR[] = "Error: "; + static constexpr char FATAL[] = "Fatal: "; + static constexpr char DEBUGPREFIX[] = "Debug: "; + + const std::string name_; + std::string formatted_name_; + const bool colored_; + static std::unordered_map + publishing_variables_; // Map from variable name to a struct containing + // info about logging data +}; + +template +std::ostream& Logger::operator<<(const T& printable) const { + return std::cout << formatted_name_ << printable; +} + +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/utils/timer.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/utils/timer.hpp new file mode 100644 index 000000000..45066eabc --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/utils/timer.hpp @@ -0,0 +1,129 @@ +#pragma once + +#include +#include +#include + +#include "aerial_robot_control/trajectory/math/types.hpp" + +namespace agi { + +/* + * Timer class to perform runtime analytics. + * + * This timer class provides a simple solution to time code. + * Simply construct a timer and call it's `tic()` and `toc()` functions to time + * code. It is intended to be used to time multiple calls of a function and not + * only reports the `last()` timing, but also statistics such as the `mean()`, + * `min()`, `max()` time, the `count()` of calls to the timer , and even + * standard deviation `std()`. + * + * The constructor can take a name for the timer (like "update") and a name for + * the module (like "Filter"). + * After construction it can be `reset()` if needed. + * + * A simple way to get the timing and stats is `std::cout << timer;` which can + * output to arbitrary streams, overloading the stream operator, + * or `print()` which always prints to console. + * + */ + +using TimeFunction = std::function; +static constexpr auto ChronoTime = []() -> agi::Scalar { + const std::chrono::high_resolution_clock::time_point now = + std::chrono::high_resolution_clock::now(); + return 1e-9 * std::chrono::duration_cast( + now.time_since_epoch()) + .count(); +}; + +class Timer { + public: + Timer(const std::string name = "", const std::string module = ""); + Timer(const Timer& other); + ~Timer() {} + + /// Start the timer. + void tic(); + + /// Stops timer, calculates timing, also tics again. + Scalar toc(); + + /// Reset saved timings and calls; + void reset(); + + // Accessors + Scalar operator()() const; + Scalar mean() const; + Scalar last() const; + Scalar min() const; + Scalar max() const; + Scalar std() const; + int count() const; + Scalar startTime() const; + + /// Custom stream operator for outputs. + friend std::ostream& operator<<(std::ostream& os, const Timer& timer); + + /// Print timing information to console. + void print() const; + + private: + std::string name_, module_; + using TimePoint = std::chrono::high_resolution_clock::time_point; + TimePoint t_start_; + + // Initialize timing to impossible values. + Scalar timing_mean_; + Scalar timing_last_; + Scalar timing_S_; + Scalar timing_min_; + Scalar timing_max_; + + int n_samples_; +}; + +/* + * Simple class to time scopes. + * + * This effectively instantiates a timer and calls `tic()` in its constructor + * and `toc()` and ` print()` in its destructor. + */ +class ScopedTimer : public Timer { + public: + ScopedTimer(const std::string name = "", const std::string module = ""); + ~ScopedTimer(); +}; + +/* + * * Helper Timer class to instantiate a static Timer that prints in + * descructor. + * * + * * Debugging slow code? Simply create this as a static object somewhere and + * * tic-toc it. Once the program ends, the destructor of StaticTimer will + * print + * * its stats. + * */ +class StaticTimer : public Timer { + public: + using Timer::Timer; + + ~StaticTimer() { this->print(); } +}; + +/* + * Simple class to tic and toc a timer within a scope. + * + * This takes a timer as argument, tics in the constructor, and tocs on + * destruction. + */ +class ScopedTicToc { + public: + ScopedTicToc(Timer& timer); + ~ScopedTicToc(); + + private: + Timer& timer_; +}; + +} // namespace agi diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/utils/yaml.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/utils/yaml.hpp new file mode 100644 index 000000000..f58110ad5 --- /dev/null +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/utils/yaml.hpp @@ -0,0 +1,494 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "aerial_robot_control/trajectory/math/types.hpp" +#include "aerial_robot_control/trajectory/utils/filesystem.hpp" + +namespace agi { +namespace { + +struct YamlException : public std::exception { + YamlException() = default; + YamlException(const std::string& msg) : msg(msg) {} + YamlException(const std::string& key, const std::string& msg) + : msg("Key \'" + key + "\':\n" + msg + '\n') {} + + YamlException(const std::string& key, const std::string& msg, + const std::string_view& doc, const size_t pos) + : msg("Key \'" + key + "\' in line " + + std::to_string(getLineNumber(doc.substr(0, pos))) + ":\n" + msg + + '\n') {} + + YamlException(const std::string& msg, const std::string_view& doc, + const size_t pos) + : msg([&] { + const size_t ln = getLineNumber(doc.substr(0, pos)); + return std::string("In line " + std::to_string(ln) + ":\n" + msg + + "\nLine: \'" + getLine(doc, ln) + "\'\n"); + }()) {} + + [[nodiscard]] const char* what() const noexcept override { + return msg.c_str(); + } + + static size_t getLineNumber(const std::string_view& doc) { + return std::count(doc.begin(), doc.end(), '\n'); + } + + static std::string getLine(const std::string_view& doc, const size_t line) { + size_t start = 0; + size_t end = doc.find_first_of('\n'); + if (line) { + for (size_t i = 0; i < line; ++i) { + start = end + 1; + end = doc.find_first_of('\n', start); + } + } + return std::string(doc.substr(start, end - start)); + } + + const std::string msg{"YAML Exception"}; +}; +} // namespace + +class Yaml { + public: + Yaml() = default; + Yaml(const Yaml&) = default; + ~Yaml() = default; + Yaml(const std::string& keyspace, const std::string& key) + : keyspace_(keyspace), key_(key) {} + + // Delegating Constructors + explicit Yaml(const fs::path& filename) : Yaml(loadFile(filename)) {} + explicit Yaml(const std::string& doc) : Yaml(std::string_view(doc)) {} + explicit Yaml(const std::string_view& doc) + : Yaml([](const std::string_view& doc) { + size_t indent = 0; + size_t pos = 0; + return Yaml("", "", doc, pos, doc.size(), indent); + }(doc)) {} + + template + T as() const { + if constexpr (std::is_same_v) + return as_int(); + else if constexpr (std::is_same_v) + return as_uint(); + else if constexpr (std::is_same_v) + return as_float(); + else if constexpr (std::is_same_v) + return as_double(); + else if constexpr (std::is_same_v>) + return as_complex(); + else if constexpr (std::is_same_v>) + return as_complex(); + else if constexpr (std::is_same_v) + return as_bool(); + else + return as_string(); + } + + template, T>::value && + !std::is_base_of, T>::value, + bool>::type = false> + inline friend void operator>>(const Yaml& yaml, T& value) { + value = yaml.as(); + } + + template, Derived>::value, + bool>::type = true> + inline friend void operator>>(const Yaml& yaml, Derived& matrix) { + yaml.as_matrix(matrix); + } + + template, Derived>::value, + bool>::type = true> + inline friend void operator>>(const Yaml& yaml, Derived& matrix) { + yaml.as_quaternion(matrix); + } + + const Yaml operator[](const std::string& s) const { + try { + return map_.at(s); + } catch (std::exception& e) { + return Yaml(getFullKeyName(), s); + } + } + + const Yaml operator[](const int i) const { + const size_t idx = i >= 0 ? (size_t)i : (size_t)(i % (int)seq_.size()); + if (seq_.empty()) return Yaml(); + if (idx < seq_.size()) + return seq_[idx]; + else + return Yaml(getFullKeyName(), std::to_string(i)); + } + + template + bool getIfDefined(T& val) const { + if (isNull()) return false; + if constexpr (std::is_base_of_v, T>) + as_matrix(val); + else if constexpr (std::is_base_of_v, T>) + as_quaternion(val); + else + val = as(); + return true; + } + + [[nodiscard]] inline bool isValid() const { + return !raw_.empty() ^ !map_.empty() ^ !seq_.empty(); + } + [[nodiscard]] inline bool isDefined() const { + return !raw_.empty() || !map_.empty() || !seq_.empty(); + } + [[nodiscard]] inline bool isNull() const { + return raw_.empty() && map_.empty() && seq_.empty(); + } + [[nodiscard]] inline bool isValue() const { return !raw_.empty(); } + [[nodiscard]] inline bool isNode() const { return !map_.empty(); } + [[nodiscard]] inline bool isSequence() const { return !seq_.empty(); } + + [[nodiscard]] inline size_t size() const { + if (isNull()) + return 0; + else if (isSequence()) + return seq_.size(); + else if (isNode()) + return map_.size(); + else + return 1; + } + + friend std::ostream& operator<<(std::ostream& os, const Yaml& yaml) { + yaml.print(os, 0); + return os; + } + + void print(std::ostream& os, const size_t indent) const { + if (isValue()) { + os << ' ' << raw_ << '\n'; + return; + } + os << '\n'; + for (const auto& node : map_) { + os << std::string(indent, ' ') << node.first << ':'; + node.second.print(os, indent + 2); + } + for (const auto& node : seq_) { + os << std::string(indent, ' ') << "- "; + node.print(os, indent + 2); + } + } + + // Parsing Constructor + Yaml(const std::string& keyspace, const std::string& key, + const std::string_view& doc, size_t& pos, const size_t end, + const size_t min_indent = 0, bool list = false) + : keyspace_(keyspace), key_(key) { + while (pos < end) { + const size_t first_non_space = doc.find_first_not_of(" \t", pos); + const size_t first = (list && doc[first_non_space] == '-') + ? doc.find_first_not_of(" \t", first_non_space + 1) + : first_non_space; + list = false; + const size_t line_break = + std::min(doc.find_first_of('\n', first), std::string::npos - 1); + const size_t line_end = std::min(doc.find_first_of("#\n", first), end); + const size_t indent = first - pos; + const size_t next = std::min(line_break + 1, end); + + if (first >= line_end) { + pos = next; + continue; + } + + if (indent < min_indent) break; + + const std::string_view line = doc.substr(first, line_end - first); + + if (line_end > first) { + const size_t indicator = line.find_first_of(":[{("); + if (line[0] == '-' && line[1] == ' ') { + const size_t list_indent = first_non_space - pos; + seq_.emplace_back(getFullKeyName(), std::to_string(seq_.size()), doc, + pos, end, list_indent + 1, true); + } else if (indicator == std::string::npos) { + const size_t value_end = line.find_last_not_of(' '); + if (value_end < std::string::npos) { + raw_ = line.substr(0, 1 + value_end); + } else { + raw_ = line; + } + pos = next; + } else if (line[indicator] == ':') { + const size_t key_end = line.find_first_of(" \t\n:{}[]()"); + const size_t key_remainder = line.find_first_not_of(" \t", key_end); + if (indicator < 1 || key_remainder < indicator) + throw YamlException("Could not parse line! No valid key!", doc, + pos); + const std::string sub_key = + (std::string)line.substr(0, std::min(indicator, key_end)); + const size_t value_start = + line.find_first_not_of(" \t", indicator + 1); + if (value_start < line_end) { + pos = first + value_start; + map_[sub_key] = + Yaml(getFullKeyName(), sub_key, doc, pos, line_end, 0); + } else { + pos = next; + map_[sub_key] = + Yaml(getFullKeyName(), sub_key, doc, pos, end, indent + 1); + } + pos = std::max(pos, next); + } else if (line[indicator] == '[' || line[indicator] == '{' || + line[indicator] == '(') { + pos = doc.find_first_not_of(" \t", first + indicator + 1); + size_t r = pos; + int level = 1; + while (r < end) { + const char c = doc[r]; + if (c == ':') break; + if (c == '[' || c == '{' || c == '(') { + ++level; + ++r; + continue; + } else if (c == ']' || c == '}' || c == ')') { + if (level == 1) { + seq_.emplace_back(getFullKeyName(), std::to_string(seq_.size()), + doc, pos, r, 0); + } + --level; + ++r; + continue; + } else if (c == ',' && level == 1) { + seq_.emplace_back(getFullKeyName(), std::to_string(seq_.size()), + doc, pos, r, 0); + pos = ++r; + continue; + } + ++r; + } + if (level != 0) throw YamlException("List not closed!", doc, pos); + pos = doc.find_first_of('\n', pos); + if (pos < end) ++pos; + } else { + pos = next; + } + } else { + pos = next; + } + } + + // Clean out sequences of size 1. + if (seq_.size() == 1 && map_.empty()) { + raw_ = seq_[0].raw_; + map_ = seq_[0].map_; + seq_ = seq_[0].seq_; + } + } + + private: + // Interpreters + [[nodiscard]] int as_int() const { + try { + return std::stoi(raw_); + } catch (std::exception& e) { + throw YamlException(getFullKeyName(), "Is not a valid int: " + raw_); + }; + } + + [[nodiscard]] int as_uint() const { + try { + return std::stoul(raw_); + } catch (std::exception& e) { + throw YamlException(getFullKeyName(), + "Is not a valid unsigned integer: " + raw_); + }; + } + + [[nodiscard]] float as_float() const { + try { + return std::stof(raw_); + } catch (std::exception& e) { + throw YamlException(getFullKeyName(), "Is not a valid float: " + raw_); + }; + } + + [[nodiscard]] double as_double() const { + try { + return std::stod(raw_); + } catch (std::exception& e) { + throw YamlException(getFullKeyName(), "Is not a valid double: " + raw_); + }; + } + + [[nodiscard]] bool as_bool() const { + // Fast catch for obvious cases such as empty or single char. + if (raw_.empty()) + throw YamlException(getFullKeyName(), "Can't cast empty node to bool!"); + if (raw_.size() == 1) { + if (raw_[0] == '0' || raw_[0] == 'f' || raw_[0] == 'F') return false; + if (raw_[0] == '1' || raw_[0] == 't' || raw_[0] == 'T') return true; + throw YamlException(getFullKeyName(), "Is not a valid bool: " + raw_); + } + + static constexpr std::array strue{'t', 'r', 'u', 'e'}; + static constexpr std::array sfalse{'f', 'a', 'l', 's', 'e'}; + + size_t true_hit = 0; + size_t false_hit = 0; + static constexpr size_t case_dist = 'a' - 'A'; + for (const char c : raw_) { + if (c == strue[true_hit] || (char)(c + case_dist) == strue[true_hit]) { + ++true_hit; + if (true_hit >= strue.size()) return true; + } else { + true_hit = 0; + } + if (c == sfalse[false_hit] || + (char)(c + case_dist) == sfalse[false_hit]) { + ++false_hit; + if (false_hit >= sfalse.size()) return false; + } else { + false_hit = 0; + } + } + throw YamlException(getFullKeyName(), "Is not a valid bool: " + raw_); + } + + [[nodiscard]] std::string as_string() const { + const size_t start = raw_.find_first_not_of(" \'\""); + const size_t end = raw_.find_last_not_of(" \'\""); + if (start < raw_.size() && end < raw_.size()) + return raw_.substr(start, end - start + 1); + else + throw YamlException(getFullKeyName(), + std::string("Is not a valid string: ") + raw_); + return ""; + } + + template + [[nodiscard]] std::complex as_complex() const { + if (seq_.size() != 2) + throw YamlException(getFullKeyName(), "Is not a valid complex number!"); + try { + return std::complex(seq_[0].as(), seq_[1].as()); + } catch (std::exception& e) { + throw YamlException( + getFullKeyName(), + std::string("Can't parse to a complex number:\n") + e.what()); + }; + return std::complex(); + } + + template, Derived>::value, + bool>::type = true> + void as_matrix(Derived& matrix) const { + if (isNull()) + throw YamlException(getFullKeyName(), + "Can't cast empty Yaml node to Eigen object."); + const size_t n = seq_.size(); + try { + if (isValue() && (matrix.rows() == 1 || matrix.cols() == 1)) { + matrix.setConstant(as()); + } else if (matrix.size() == (int)n) { + for (size_t i = 0; i < n; ++i) + matrix((int)i) = seq_[i].as(); + } else { + const int rows = matrix.rows(); + const int cols = matrix.cols(); + if (rows != (int)n) + throw YamlException(getFullKeyName(), + "dimensions wrong\n" + "Eigen object: " + + std::to_string(rows) + 'x' + + std::to_string(cols) + '\n' + + "Yaml object: " + std::to_string(n) + 'x' + + std::to_string(seq_[0].size())); + for (size_t i = 0; i < n; ++i) { + const size_t m = seq_[i].size(); + if (cols != (int)m) + throw YamlException( + getFullKeyName(), + "dimensions wrong\n" + "Eigen object: " + + std::to_string(rows) + 'x' + std::to_string(cols) + '\n' + + "Yaml object: " + std::to_string(n) + 'x' + std::to_string(m)); + for (size_t j = 0; j < m; ++j) + matrix((int)i, (int)j) = seq_[i][j].as(); + } + } + } catch (std::exception& e) { + throw YamlException( + getFullKeyName(), + std::string("Can't parse to Eigen object:\n") + e.what()); + } + } + + template, Derived>::value, + bool>::type = true> + void as_quaternion(Derived& quaternion) const { + if (seq_.size() != 4) + throw YamlException(getFullKeyName(), + "Is not a valid quaternion, because it has " + + std::to_string(seq_.size()) + " entries."); + + try { + quaternion.w() = seq_[0].as(); + quaternion.x() = seq_[1].as(); + quaternion.y() = seq_[2].as(); + quaternion.z() = seq_[3].as(); + } catch (std::exception& e) { + throw YamlException( + getFullKeyName(), + std::string("Can't parse into Eigen quaternion:\n") + e.what()); + } + } + + // File helper + static std::string loadFile(const fs::path& filename) { + std::ifstream fs(filename.c_str()); + if (!fs.is_open()) + throw YamlException("Could not open file \'" + filename.string() + "\'"); + fs.seekg(0, std::ios::end); + const size_t size = fs.tellg(); + if (size < 1) + throw YamlException("Empty file \'" + filename.string() + "\'"); + std::string buffer((size_t)size, ' '); + fs.seekg(0); + fs.read(&buffer[0], size); + return buffer; + } + + [[nodiscard]] std::string getFullKeyName() const { + return keyspace_.empty() ? key_ : keyspace_ + "/" + key_; + } + + // Members + std::string keyspace_; + std::string key_; + std::string raw_; + std::map map_; + std::vector seq_; +}; + + +} // namespace agi diff --git a/aerial_robot_control/src/control/base/pose_linear_controller.cpp b/aerial_robot_control/src/control/base/pose_linear_controller.cpp index a42dbf810..31623e3b5 100644 --- a/aerial_robot_control/src/control/base/pose_linear_controller.cpp +++ b/aerial_robot_control/src/control/base/pose_linear_controller.cpp @@ -139,6 +139,8 @@ namespace aerial_robot_control } /* z */ + getParam(z_nh, "force_landing_descending_rate", force_landing_descending_rate_, -0.1); + if(force_landing_descending_rate_ >= 0) force_landing_descending_rate_ = -0.1; loadParam(z_nh); pid_controllers_.push_back(PID("z", p_gain, i_gain, d_gain, limit_sum, limit_p, limit_i, limit_d, limit_err_p, limit_err_i, limit_err_d)); pid_reconf_servers_.push_back(boost::make_shared(z_nh)); @@ -209,6 +211,7 @@ namespace aerial_robot_control omega_ = estimator_->getAngularVel(Frame::COG, estimate_mode_); target_rpy_ = navigator_->getTargetRPY(); target_omega_ = navigator_->getTargetOmega(); + target_ang_acc_ = navigator_->getTargetAngAcc(); // time diff double du = ros::Time::now().toSec() - control_timestamp_; @@ -276,8 +279,8 @@ namespace aerial_robot_control } du_rp = 0; } - pid_controllers_.at(ROLL).update(target_rpy_.x() - rpy_.x(), du_rp, target_omega_.x() - omega_.x()); - pid_controllers_.at(PITCH).update(target_rpy_.y() - rpy_.y(), du_rp, target_omega_.y() - omega_.y()); + pid_controllers_.at(ROLL).update(target_rpy_.x() - rpy_.x(), du_rp, target_omega_.x() - omega_.x(), target_ang_acc_.x()); + pid_controllers_.at(PITCH).update(target_rpy_.y() - rpy_.y(), du_rp, target_omega_.y() - omega_.y(), target_ang_acc_.y()); // yaw double err_yaw = angles::shortest_angular_distance(rpy_.z(), target_rpy_.z()); @@ -286,7 +289,7 @@ namespace aerial_robot_control { err_omega_z = target_omega_.z(); // part of the control in spinal } - pid_controllers_.at(YAW).update(err_yaw, du, err_omega_z); + pid_controllers_.at(YAW).update(err_yaw, du, err_omega_z, target_ang_acc_.z()); // update control_timestamp_ = ros::Time::now().toSec(); diff --git a/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp b/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp index 347858e5c..73c14304d 100644 --- a/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp +++ b/aerial_robot_control/src/control/under_actuated_lqi_controller.cpp @@ -183,6 +183,11 @@ void UnderActuatedLQIController::controlCore() pid_msg_.z.i_term.at(i) = i_term; pid_msg_.z.d_term.at(i) = d_term; } + // feed-forward term for z + Eigen::MatrixXd q_mat_inv = getQInv(); + double ff_acc_z = navigator_->getTargetAcc().z(); + Eigen::VectorXd ff_term = q_mat_inv.col(0) * ff_acc_z; + target_thrust_z_term += ff_term; // constraint z (also I term) int index; @@ -203,6 +208,24 @@ void UnderActuatedLQIController::controlCore() allocateYawTerm(); } +Eigen::MatrixXd UnderActuatedLQIController::getQInv() +{ + // wrench allocation matrix + const std::vector rotors_origin = robot_model_->getRotorsOriginFromCog(); + const std::vector rotors_normal = robot_model_->getRotorsNormalFromCog(); + const auto& rotor_direction = robot_model_->getRotorDirection(); + const double m_f_rate = robot_model_->getMFRate(); + double uav_mass_inv = 1.0 / robot_model_->getMass(); + Eigen::Matrix3d inertia_inv = robot_model_->getInertia().inverse(); + Eigen::MatrixXd q_mat = Eigen::MatrixXd::Zero(4, motor_num_); + for (unsigned int i = 0; i < motor_num_; ++i) { + q_mat(0, i) = rotors_normal.at(i).z() * uav_mass_inv; + q_mat.block(1, i, 3, 1) = inertia_inv * (rotors_origin.at(i).cross(rotors_normal.at(i)) + m_f_rate * rotor_direction.at(i + 1) * rotors_normal.at(i)); + } + Eigen::MatrixXd q_mat_inv = aerial_robot_model::pseudoinverse(q_mat); + return q_mat_inv; +} + void UnderActuatedLQIController::allocateYawTerm() { Eigen::VectorXd target_thrust_yaw_term = Eigen::VectorXd::Zero(motor_num_); @@ -216,6 +239,13 @@ void UnderActuatedLQIController::allocateYawTerm() pid_msg_.yaw.i_term.at(i) = i_term; pid_msg_.yaw.d_term.at(i) = d_term; } + + // feed-forward term for yaw + Eigen::MatrixXd q_mat_inv = getQInv(); + double ff_ang_yaw = navigator_->getTargetAngAcc().z(); + Eigen::VectorXd ff_term = q_mat_inv.col(3) * ff_ang_yaw; + target_thrust_yaw_term += ff_term; + // constraint yaw (also I term) int index; double max_term = target_thrust_yaw_term.cwiseAbs().maxCoeff(&index); diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index 564bce188..c6cc14f1f 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -9,12 +9,14 @@ BaseNavigator::BaseNavigator(): target_acc_(0, 0, 0), target_rpy_(0, 0, 0), target_omega_(0, 0, 0), + target_ang_acc_(0, 0, 0), init_height_(0), land_height_(0), force_att_control_flag_(false), trajectory_mode_(false), trajectory_reset_time_(0), teleop_reset_time_(0), low_voltage_flag_(false), + high_voltage_flag_(false), prev_xy_control_mode_(ACC_CONTROL_MODE), xy_control_flag_(false), vel_based_waypoint_(false), @@ -42,6 +44,8 @@ void BaseNavigator::initialize(ros::NodeHandle nh, ros::NodeHandle nhp, estimator_ = estimator; loop_du_ = loop_du; + pose_sub_ = nh_.subscribe("target_pose", 1, &BaseNavigator::poseCallback, this, ros::TransportHints().tcpNoDelay()); + simple_move_base_goal_sub_ = nh_.subscribe("/move_base_simple/goal", 1, &BaseNavigator::simpleMoveBaseGoalCallback, this, ros::TransportHints().tcpNoDelay()); navi_sub_ = nh_.subscribe("uav/nav", 1, &BaseNavigator::naviCallback, this, ros::TransportHints().tcpNoDelay()); battery_sub_ = nh_.subscribe("battery_voltage_status", 1, &BaseNavigator::batteryCheckCallback, this); @@ -68,6 +72,7 @@ void BaseNavigator::initialize(ros::NodeHandle nh, ros::NodeHandle nhp, flight_config_pub_ = nh_.advertise("flight_config_cmd", 10); power_info_pub_ = nh_.advertise("uav_power", 10); flight_state_pub_ = nh_.advertise("flight_state", 1); + path_pub_ = nh_.advertise("trajectory", 1); estimate_mode_ = estimator_->getEstimateMode(); force_landing_start_time_ = ros::Time::now(); @@ -92,6 +97,7 @@ void BaseNavigator::batteryCheckCallback(const std_msgs::Float32ConstPtr &msg) voltage += ( (bat_resistance_voltage_rate_ * voltage + bat_resistance_) * hovering_current_); float average_voltage = voltage / bat_cell_; + float input_cell = voltage / VOLTAGE_100P; float percentage = 0; if(average_voltage > VOLTAGE_90P) percentage = (average_voltage - VOLTAGE_90P) / (VOLTAGE_100P - VOLTAGE_90P) * 10 + 90; else if (average_voltage > VOLTAGE_80P) percentage = (average_voltage - VOLTAGE_80P) / (VOLTAGE_90P - VOLTAGE_80P) * 10 + 80; @@ -120,6 +126,13 @@ void BaseNavigator::batteryCheckCallback(const std_msgs::Float32ConstPtr &msg) else low_voltage_flag_ = false; + if(input_cell - bat_cell_ > high_voltage_cell_thre_) + { + high_voltage_flag_ = true; + } + else + high_voltage_flag_ = false; + if(power_info_pub_.getNumSubscribers() == 0) return; geometry_msgs::Vector3Stamped power_info_msgs; @@ -130,6 +143,28 @@ void BaseNavigator::batteryCheckCallback(const std_msgs::Float32ConstPtr &msg) } +void BaseNavigator::poseCallback(const geometry_msgs::PoseStampedConstPtr & msg) +{ + if (traj_generator_ptr_.get() == nullptr) + { + ROS_DEBUG("traj_generator_ptr_ is null"); + } + + generateNewTrajectory(*msg); +} + +void BaseNavigator::simpleMoveBaseGoalCallback(const geometry_msgs::PoseStampedConstPtr & msg) +{ + if (traj_generator_ptr_.get() == nullptr) + { + ROS_DEBUG("traj_generator_ptr_ is null"); + } + geometry_msgs::PoseStamped target_pose = *msg; + target_pose.pose.position.z = getTargetPos().z(); + generateNewTrajectory(target_pose); +} + + void BaseNavigator::naviCallback(const aerial_robot_msgs::FlightNavConstPtr & msg) { if(getNaviState() == TAKEOFF_STATE || getNaviState() == LAND_STATE) return; @@ -412,8 +447,6 @@ void BaseNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg) setNaviState(LAND_STATE); //update - setTargetXyFromCurrentState(); - setTargetYawFromCurrentState(); ROS_INFO("Joy Control: Land state"); return; @@ -647,21 +680,64 @@ void BaseNavigator::update() if(normal_land && !force_att_control_flag_) { setNaviState(LAND_STATE); - setTargetXyFromCurrentState(); - setTargetYawFromCurrentState(); } } /* update the target pos and velocity */ if (trajectory_mode_) { - if (ros::Time::now().toSec() > trajectory_reset_time_) + if (traj_generator_ptr_.get() == nullptr) { - setTargetZeroVel(); + if (ros::Time::now().toSec() > trajectory_reset_time_) + { + setTargetZeroVel(); + setTargetZeroAcc(); - trajectory_mode_ = false; + setTargetZeroOmega(); + setTargetZeroAngAcc(); - ROS_INFO("[Flight nav] stop trajectory mode"); + trajectory_mode_ = false; + + ROS_INFO("[Flight nav] stop trajectory mode in POS-VEL mode"); + } + } + else + { + // trajectory following mode + double t = ros::Time::now().toSec(); + double end_t = traj_generator_ptr_->getEndSetpoint().state.t; + if (t > end_t) + { + ROS_INFO("[Nav] reach the end of trajectory"); + + setTargetZeroVel(); + setTargetZeroAcc(); + + setTargetZeroOmega(); + setTargetZeroAngAcc(); + + trajectory_mode_ = false; + + traj_generator_ptr_.reset(); + } + else + { + agi::QuadState target_state = traj_generator_ptr_->getState(t); + setTargetPos(tf::Vector3(target_state.p(0), target_state.p(1), target_state.p(2))); + setTargetVel(tf::Vector3(target_state.v(0), target_state.v(1), target_state.v(2))); + setTargetAcc(tf::Vector3(target_state.a(0), target_state.a(1), target_state.a(2))); + + double target_yaw = target_state.getYaw(); + double target_omega_z = target_state.w(2); + double target_ang_acc_z = target_state.tau(2); + setTargetYaw(target_yaw); + setTargetOmegaZ(target_omega_z); + setTargetAngAccZ(target_ang_acc_z); + + tf::Vector3 curr_pos = estimator_->getPos(Frame::COG, estimate_mode_); + double yaw_angle = estimator_->getState(State::YAW_COG, estimate_mode_)[0]; + ROS_INFO_THROTTLE(0.5, "[Nav] trajectory mode, target pos&yaw: [%f, %f, %f, %f], curr pos&yaw: [%f, %f, %f, %f]", target_state.p(0), target_state.p(1), target_state.p(2), target_yaw, curr_pos.x(), curr_pos.y(), curr_pos.z(), yaw_angle); + } } } else @@ -698,6 +774,12 @@ void BaseNavigator::update() setNaviState(ARM_OFF_STATE); break; } + if(high_voltage_flag_) + { + setNaviState(ARM_OFF_STATE); + ROS_ERROR("high voltage!"); + break; + } estimator_->setSensorFusionFlag(true); force_landing_flag_ = false; @@ -746,7 +828,7 @@ void BaseNavigator::update() ROS_INFO("expected land height: %f (current height: %f), velocity: %f ", land_height_, curr_pos.z(), vel); if (fabs(delta) < land_pos_convergent_thresh_ && - fabs(vel) < land_vel_convergent_thresh_) + vel > -land_vel_convergent_thresh_) { ROS_INFO("\n \n ====================== \n Land !!! \n ====================== \n"); ROS_INFO("Start disarming motors"); @@ -877,6 +959,90 @@ void BaseNavigator::updateLandCommand() setTargetVelZ(land_descend_vel_); } + +void BaseNavigator::generateNewTrajectory(geometry_msgs::PoseStamped pose) +{ + if (traj_generator_ptr_.get() != nullptr) + { + ROS_WARN("[Nav] force to finish the last trajectory following"); + traj_generator_ptr_.reset(); + } + + agi::QuadState start_state; + start_state.setZero(); + tf::Vector3 last_target_pos = getTargetPos(); + start_state.p = agi::Vector<3>(last_target_pos.x(), last_target_pos.y(), last_target_pos.z()); + tf::Vector3 last_target_vel = getTargetVel(); + start_state.v = agi::Vector<3>(last_target_vel.x(), last_target_vel.y(), last_target_vel.z()); + tf::Vector3 last_target_acc = getTargetAcc(); + start_state.a = agi::Vector<3>(last_target_acc.x(), last_target_acc.y(), last_target_acc.z()); + + double last_target_yaw_angle = getTargetRPY().z(); + start_state.setYaw(last_target_yaw_angle); + double last_target_omega_z = getTargetOmega().z(); + start_state.w(2) = last_target_omega_z; + start_state.t = ros::Time::now().toSec(); + + agi::QuadState end_state; + end_state.setZero(); + Eigen::Vector3d p; + tf::pointMsgToEigen(pose.pose.position, p); + end_state.p = p; + agi::Quaternion q; + tf::quaternionMsgToEigen(pose.pose.orientation, q); + if (std::fabs(1 - q.squaredNorm()) < 1e-6) + { + end_state.q(q); + } + else + { + ROS_WARN("[Nav] the target quaternion is invalid [%f, %f, %f, %f], reset as the start state", q.x(), q.y(), q.z(), q.w()); + end_state.q(start_state.q()); + + } + + double du_tran = (end_state.p - start_state.p).norm() / trajectory_mean_vel_; + double du_rot = fabs(end_state.getYaw() - start_state.getYaw()) / trajectory_mean_yaw_rate_; + double du = std::max(du_tran, trajectory_min_du_); + if (!enable_latch_yaw_trajectory_) + { + du = std::max(du_rot, du); + } + + end_state.t = start_state.t + du; + + ROS_INFO_STREAM("[Nav] revceive the new target pose of " << end_state.p.transpose() + << " (yaw: " << end_state.getYaw() << ")" + << " which starts with the last target pose: " << start_state.p.transpose() + << " (yaw: " << start_state.getYaw() << ")" + << " and target vel: " << start_state.v.transpose() + << " (omega z: " << start_state.w(2) << ")" + << " and target acc: " << start_state.a.transpose()); + + traj_generator_ptr_ = std::make_shared(start_state, end_state); + + trajectory_mode_ = true; + + double dt = 0.02; + nav_msgs::Path msg; + msg.header.stamp.fromSec(start_state.t); + msg.header.frame_id = "world"; + + for (double t = 0; t <= du; t += dt) { + geometry_msgs::PoseStamped pose_stamp; + pose_stamp.header.stamp = msg.header.stamp + ros::Duration(t); + pose_stamp.header.frame_id = msg.header.frame_id; + + agi::QuadState state = traj_generator_ptr_->getState(start_state.t + t); + + tf::pointEigenToMsg(state.p, pose_stamp.pose.position); + tf::quaternionEigenToMsg(state.q(), pose_stamp.pose.orientation); + msg.poses.push_back(pose_stamp); + } + path_pub_.publish(msg); + +} + void BaseNavigator::rosParamInit() { getParam(nhp_, "param_verbose", param_verbose_, false); @@ -884,15 +1050,32 @@ void BaseNavigator::rosParamInit() ros::NodeHandle nh(nh_, "navigation"); getParam(nh, "xy_control_mode", xy_control_mode_, 0); getParam(nh, "takeoff_height", takeoff_height_, 0.0); + getParam(nh, "land_descend_vel",land_descend_vel_, -0.3); + if (land_descend_vel_ >= 0) { + ROS_WARN("land_descend_vel_ (current value: %f) should be negative", land_descend_vel_); + land_descend_vel_ == -0.3; + } + getParam(nh, "hover_convergent_duration", hover_convergent_duration_, 1.0); - getParam(nh, "land_check_duration", land_check_duration_, 1.0); + getParam(nh, "land_check_duration", land_check_duration_, 0.5); + if (land_check_duration_ < 0.5) { + ROS_WARN("land_check_duration_ (current value: %f) should be not smaller than 0.5", land_check_duration_); + land_check_duration_ = 0.5; + } + getParam(nh, "trajectory_reset_duration", trajectory_reset_duration_, 0.5); getParam(nh, "teleop_reset_duration", teleop_reset_duration_, 0.5); getParam(nh, "z_convergent_thresh", z_convergent_thresh_, 0.05); getParam(nh, "xy_convergent_thresh", xy_convergent_thresh_, 0.15); getParam(nh, "land_pos_convergent_thresh", land_pos_convergent_thresh_, 0.02); - getParam(nh, "land_vel_convergent_thresh", land_vel_convergent_thresh_, 0.01); + getParam(nh, "land_vel_convergent_thresh", land_vel_convergent_thresh_, 0.05); + + //*** trajectory + getParam(nh, "trajectory_mean_vel", trajectory_mean_vel_, 0.5); + getParam(nh, "trajectory_mean_yaw_rate", trajectory_mean_yaw_rate_, 0.3); + getParam(nh, "trajectory_min_du", trajectory_min_du_, 2.0); + getParam(nh, "enable_latch_yaw_trajectory", enable_latch_yaw_trajectory_, false); //*** auto vel nav getParam(nh, "nav_vel_limit", nav_vel_limit_, 0.2); @@ -919,6 +1102,7 @@ void BaseNavigator::rosParamInit() ros::NodeHandle bat_nh(nh_, "bat_info"); getParam(bat_nh, "bat_cell", bat_cell_, 0); // Lipo battery cell getParam(bat_nh, "low_voltage_thre", low_voltage_thre_, 0.1); // Lipo battery cell + getParam(bat_nh, "high_voltage_cell_thre", high_voltage_cell_thre_, 1.0); getParam(bat_nh, "bat_resistance", bat_resistance_, 0.0); //Battery internal resistance getParam(bat_nh, "bat_resistance_voltage_rate", bat_resistance_voltage_rate_, 0.0); //Battery internal resistance_voltage_rate getParam(bat_nh, "hovering_current", hovering_current_, 0.0); // current at hovering state diff --git a/aerial_robot_control/src/trajectory/base/parameter_base.cpp b/aerial_robot_control/src/trajectory/base/parameter_base.cpp new file mode 100644 index 000000000..0fba53aff --- /dev/null +++ b/aerial_robot_control/src/trajectory/base/parameter_base.cpp @@ -0,0 +1,18 @@ +#include "aerial_robot_control/trajectory/base/parameter_base.hpp" + +#include + +namespace agi { + +bool ParameterBase::load(const fs::path& filename) { + return load(Yaml(filename)); +} + +bool ParameterBase::load(const Yaml& node) { + throw ParameterException("ParameterBase load should not be called!"); + return false; +} + +bool ParameterBase::valid() const { return false; } + +} // namespace agi diff --git a/aerial_robot_control/src/trajectory/math/math.cpp b/aerial_robot_control/src/trajectory/math/math.cpp new file mode 100644 index 000000000..aaadcd226 --- /dev/null +++ b/aerial_robot_control/src/trajectory/math/math.cpp @@ -0,0 +1,138 @@ +#include "aerial_robot_control/trajectory/math/math.hpp" + +namespace agi { + +Matrix<3, 3> skew(const Vector<3>& v) { + return (Matrix<3, 3>() << 0, -v.z(), v.y(), v.z(), 0, -v.x(), -v.y(), v.x(), + 0) + .finished(); +} + +Matrix<4, 4> Q_left(const Quaternion& q) { + return (Matrix<4, 4>() << q.w(), -q.x(), -q.y(), -q.z(), q.x(), q.w(), -q.z(), + q.y(), q.y(), q.z(), q.w(), -q.x(), q.z(), -q.y(), q.x(), q.w()) + .finished(); +} + +Matrix<4, 4> Q_right(const Quaternion& q) { + return (Matrix<4, 4>() << q.w(), -q.x(), -q.y(), -q.z(), q.x(), q.w(), q.z(), + -q.y(), q.y(), -q.z(), q.w(), q.x(), q.z(), q.y(), -q.x(), q.w()) + .finished(); +} + +Matrix<4, 3> qFromQeJacobian(const Quaternion& q) { + return (Matrix<4, 3>() << -1.0 / q.w() * q.vec().transpose(), + Matrix<3, 3>::Identity()) + .finished(); +} + +Matrix<4, 4> qConjugateJacobian() { + return Matrix<4, 1>(1, -1, -1, -1).asDiagonal(); +} + +Matrix<3, 3> qeRotJacobian(const Quaternion& q, const Matrix<3, 1>& t) { + return 2.0 * + (Matrix<3, 3>() << (q.y() + q.z() * q.x() / q.w()) * t.y() + + (q.z() - q.y() * q.x() / q.w()) * + t.z(), // entry 0,0 + -2.0 * q.y() * t.x() + (q.x() + q.z() * q.y() / q.w()) * t.y() + + (q.w() - q.y() * q.y() / q.w()) * t.z(), // entry 0,1 + -2.0 * q.z() * t.x() + (-q.w() + q.z() * q.z() / q.w()) * t.y() + + (q.x() - q.y() * q.z() / q.w()) * t.z(), // entry 0,2 + + (q.y() - q.z() * q.x() / q.w()) * t.x() + (-2.0 * q.x()) * t.y() + + (-q.w() + q.x() * q.x() / q.w()) * t.z(), // entry 1,0 + (q.x() - q.z() * q.y() / q.w()) * t.x() + + (q.z() + q.x() * q.y() / q.w()) * t.z(), // entry 1,1 + (q.w() - q.z() * q.z() / q.w()) * t.x() + (-2.0 * q.z()) * t.y() + + (q.y() + q.x() * q.z() / q.w()) * t.z(), // entry 1,2 + + (q.z() + q.y() * q.x() / q.w()) * t.x() + + (q.w() - q.x() * q.x() / q.w()) * t.y() + + (-2.0 * q.x()) * t.z(), // entry 2,0 + (-q.w() + q.y() * q.y() / q.w()) * t.x() + + (q.z() - q.x() * q.y() / q.w()) * t.y() + + (-2.0 * q.y()) * t.z(), // entry 2,1 + (q.x() + q.y() * q.z() / q.w()) * t.x() + + (q.y() - q.x() * q.z() / q.w()) * t.y() // entry 2,2 + ) + .finished(); +} + +Matrix<3, 3> qeInvRotJacobian(const Quaternion& q, const Matrix<3, 1>& t) { + return 2.0 * (Matrix<3, 3>() + << (q.y() - q.z() * q.x() / q.w()) * t.y() + + (q.z() + q.y() * q.x() / q.w()) * t.z(), // entry 0,0 + -2.0 * q.y() * t.x() + (q.x() - q.z() * q.y() / q.w()) * t.y() - + (q.w() - q.y() * q.y() / q.w()) * t.z(), // entry 0,1 + -2.0 * q.z() * t.x() + (q.w() - q.z() * q.z() / q.w()) * t.y() + + (q.x() + q.y() * q.z() / q.w()) * t.z(), // entry 0,2 + + (q.y() + q.z() * q.x() / q.w()) * t.x() - 2.0 * q.x() * t.y() + + (q.w() - q.x() * q.x() / q.w()) * t.z(), // entry 1,0 + (q.x() + q.z() * q.y() / q.w()) * t.x() + + (q.z() - q.x() * q.y() / q.w()) * t.z(), // entry 1,1 + -(q.w() - q.z() * q.z() / q.w()) * t.x() - 2.0 * q.z() * t.y() + + (q.y() - q.x() * q.z() / q.w()) * t.z(), // entry 1,2 + + (q.z() - q.y() * q.x() / q.w()) * t.x() - + (q.w() - q.x() * q.x() / q.w()) * t.y() - + 2.0 * q.x() * t.z(), // entry 2,0 + (q.w() - q.y() * q.y() / q.w()) * t.x() + + (q.z() + q.x() * q.y() / q.w()) * t.y() - + 2.0 * q.y() * t.z(), // entry 2,1 + (q.x() - q.y() * q.z() / q.w()) * t.x() + + (q.y() + q.x() * q.z() / q.w()) * t.y() // entry 2,2 + ) + .finished(); +} + +void matrixToTripletList(const SparseMatrix& matrix, + std::vector* const list, + const int row_offset, const int col_offset) { + list->reserve((size_t)matrix.nonZeros() + list->size()); + + for (int i = 0; i < matrix.outerSize(); i++) { + for (typename SparseMatrix::InnerIterator it(matrix, i); it; ++it) { + list->emplace_back(it.row() + row_offset, it.col() + col_offset, + it.value()); + } + } +} + +void matrixToTripletList(const Matrix& matrix, + std::vector* const list, + const int row_offset, const int col_offset) { + const SparseMatrix sparse = matrix.sparseView(); + matrixToTripletList(sparse, list, row_offset, col_offset); +} + +void insert(const SparseMatrix& from, SparseMatrix* const into, + const int row_offset, const int col_offset) { + if (into == nullptr) return; // TODO: Handle exception + std::vector v; + matrixToTripletList(*into, &v); + matrixToTripletList(from, &v, row_offset, col_offset); + + into->setFromTriplets( + v.begin(), v.end(), + [](const Scalar& older, const Scalar& newer) { return newer; }); +} + +void insert(const Matrix<>& from, SparseMatrix* const into, + const int row_offset, const int col_offset) { + if (into == nullptr) return; // TODO: Handle exception + const SparseMatrix from_sparse = from.sparseView(); + insert(from_sparse, into, row_offset, col_offset); +} + +inline void insert(const Matrix<>& from, Matrix<>* const into, + const int row_offset, const int col_offset) { + into->block(row_offset, col_offset, from.rows(), from.cols()) = from; +} + +Vector<> clip(const Vector<>& v, const Vector<>& bound) { + return v.cwiseMin(bound).cwiseMax(-bound); +} + +} // namespace agi diff --git a/aerial_robot_control/src/trajectory/reference_base.cpp b/aerial_robot_control/src/trajectory/reference_base.cpp new file mode 100644 index 000000000..5778e90da --- /dev/null +++ b/aerial_robot_control/src/trajectory/reference_base.cpp @@ -0,0 +1,82 @@ +#include "aerial_robot_control/trajectory/reference_base.hpp" + +#include +#include + +#include "aerial_robot_control/trajectory/math/gravity.hpp" + +namespace agi { + +ReferenceBase::ReferenceBase(const std::string& name) : name_(name) {} + +ReferenceBase::ReferenceBase(const QuadState& state, const Scalar duration, + const std::string& name) + : start_state_(state), + duration_(duration), + name_(name) {} // TODO: , logger_(name) {} + +ReferenceBase::~ReferenceBase() {} + +Setpoint ReferenceBase::getSetpoint(const Scalar t, const Scalar heading) { + QuadState query_state; + query_state.t = t; + if (std::isfinite(heading)) query_state.q(heading); + return getSetpoint(query_state, t); +} + +Setpoint ReferenceBase::getSetpoint(const QuadState& state) { + return getSetpoint(state, state.t); +} + +Setpoint ReferenceBase::getStartSetpoint() { + Setpoint setpoint; + setpoint.state = start_state_; + setpoint.input = Command(start_state_.t, G, Vector<3>::Zero()); + return setpoint; +} + +Setpoint ReferenceBase::getEndSetpoint() { + Setpoint setpoint; + setpoint.state = start_state_; + setpoint.state.t = start_state_.t + duration_; + setpoint.input = Command(start_state_.t + duration_, G, Vector<3>::Zero()); + return setpoint; +} + +bool ReferenceBase::truncate(const Scalar& t) { + if (t <= start_state_.t) return false; + duration_ = t - start_state_.t; + return true; +} + +bool ReferenceBase::isTimeInRange(const Scalar time) const { + return time >= getStartTime() && time <= getEndTime(); +} + +std::string ReferenceBase::time_in_HH_MM_SS_MMM(const Scalar time) const { + if (!std::isfinite(time)) return "inf"; + + const unsigned int ms = fmod(time, 1000.0); + + // convert to std::time_t in order to convert to std::tm (broken time) + std::time_t timer = (std::time_t)(time); + + // convert to broken time + std::tm bt = *std::localtime(&timer); + + std::ostringstream oss; + + oss << std::put_time(&bt, "%H:%M:%S"); // HH:MM:SS + oss << '.' << std::setfill('0') << std::setw(3) << ms; + + return oss.str(); +} + +std::ostream& operator<<(std::ostream& os, const ReferenceBase& ref) { + os << std::setw(30) << std::left << ref.name() << " | " + << ref.time_in_HH_MM_SS_MMM(ref.getStartTime()) << " --> " + << ref.getDuration() << "s --> " + << ref.time_in_HH_MM_SS_MMM(ref.getEndTime()) << " |" << std::endl; + return os; +} +} // namespace agi diff --git a/aerial_robot_control/src/trajectory/test.cpp b/aerial_robot_control/src/trajectory/test.cpp new file mode 100644 index 000000000..4e47bda63 --- /dev/null +++ b/aerial_robot_control/src/trajectory/test.cpp @@ -0,0 +1,137 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +int main (int argc, char **argv) +{ + ros::init (argc, argv, "aeria_robot_base"); + agi::QuadState start_state; + agi::QuadState end_state; + + double du = 2; // 2 [sec] + start_state.setZero(); + start_state.v = agi::Vector<3>(-1,0,0); + end_state.setZero(); + end_state.p = agi::Vector<3>(1,2,3); + end_state.v = agi::Vector<3>(0,0,0); + end_state.t = du; + + // mediate points + agi::QuadState intermediate1_state; + intermediate1_state.setZero(); + intermediate1_state.p = agi::Vector<3>(0.5,-1,1); + intermediate1_state.t = du/2; + + agi::QuadState intermediate2_state; + intermediate2_state.setZero(); + intermediate2_state.p = agi::Vector<3>(1.0,0,2); + intermediate2_state.t = du/4 * 3; + + std::vector states; + states.push_back(start_state); + states.push_back(intermediate1_state); + states.push_back(intermediate2_state); + states.push_back(end_state); + + ros::NodeHandle nh; + ros::Publisher pub = nh.advertise("trajectory", 1); + ros::Publisher pose_pub = nh.advertise("pose", 1); + ros::Publisher twist_pub = nh.advertise("twist", 1); + ros::Publisher acc_pub = nh.advertise("acc", 1); + ros::Publisher marker_pub = nh.advertise("marker", 1); + ros::Duration(0.5).sleep(); + + std::shared_ptr test_ptr = std::make_shared(states); + + + double dt = 0.02; + + nav_msgs::Path msg; + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = "world"; + + for (double t = 0; t <= du; t += dt) { + geometry_msgs::PoseStamped pose_stamp; + pose_stamp.header.stamp = msg.header.stamp + ros::Duration(t); + pose_stamp.header.frame_id = msg.header.frame_id; + + agi::QuadState state = test_ptr->getState(t); + + tf::pointEigenToMsg(state.p, pose_stamp.pose.position); + tf::quaternionEigenToMsg(state.q(), pose_stamp.pose.orientation); + msg.poses.push_back(pose_stamp); + + // ROS_INFO_STREAM("position: " << state.p.transpose()); + + // debug + pose_pub.publish(pose_stamp); + + geometry_msgs::TwistStamped twist_stamp; + twist_stamp.header.stamp = pose_stamp.header.stamp; + twist_stamp.header.frame_id = pose_stamp.header.frame_id; + tf::vectorEigenToMsg(state.v, twist_stamp.twist.linear); + tf::vectorEigenToMsg(state.w, twist_stamp.twist.angular); + twist_pub.publish(twist_stamp); + + geometry_msgs::Vector3Stamped acc_stamp; + acc_stamp.header.stamp = pose_stamp.header.stamp; + acc_stamp.header.frame_id = pose_stamp.header.frame_id; + tf::vectorEigenToMsg(state.a, acc_stamp.vector); + acc_pub.publish(acc_stamp); + } + + pub.publish(msg); + + visualization_msgs::MarkerArray marker_array_msg; + + visualization_msgs::Marker marker_msg; + marker_msg.header.stamp = msg.header.stamp; + marker_msg.header.frame_id = msg.header.frame_id; + marker_msg.id = 0; + marker_msg.action = visualization_msgs::Marker::ADD; + marker_msg.type = visualization_msgs::Marker::SPHERE; + marker_msg.pose.position.x = start_state.p(0); + marker_msg.pose.position.y = start_state.p(1); + marker_msg.pose.position.z = start_state.p(2); + marker_msg.pose.orientation.w = 1; + double r = 0.2; + marker_msg.scale.x = r; + marker_msg.scale.y = r; + marker_msg.scale.z = r; + marker_msg.color.r = 1.0; + marker_msg.color.a = 1.0; + marker_array_msg.markers.push_back(marker_msg); + + marker_msg.id = 1; + marker_msg.pose.position.x = end_state.p(0); + marker_msg.pose.position.y = end_state.p(1); + marker_msg.pose.position.z = end_state.p(2); + marker_array_msg.markers.push_back(marker_msg); + + marker_msg.id = 2; + marker_msg.pose.position.x = intermediate1_state.p(0); + marker_msg.pose.position.y = intermediate1_state.p(1); + marker_msg.pose.position.z = intermediate1_state.p(2); + marker_array_msg.markers.push_back(marker_msg); + + marker_msg.id = 3; + marker_msg.pose.position.x = intermediate2_state.p(0); + marker_msg.pose.position.y = intermediate2_state.p(1); + marker_msg.pose.position.z = intermediate2_state.p(2); + marker_array_msg.markers.push_back(marker_msg); + + marker_pub.publish(marker_array_msg); + + ros::waitForShutdown(); + return 0; +} + + + + diff --git a/aerial_robot_control/src/trajectory/test_visualize.rviz b/aerial_robot_control/src/trajectory/test_visualize.rviz new file mode 100644 index 000000000..3cde19e00 --- /dev/null +++ b/aerial_robot_control/src/trajectory/test_visualize.rviz @@ -0,0 +1,153 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Path1 + - /MarkerArray1 + Splitter Ratio: 0.5 + Tree Height: 1088 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: trajectory + Unreliable: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /marker + Name: MarkerArray + Namespaces: + "": true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 9.436322212219238 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.37979578971862793 + Y: 0.44448453187942505 + Z: 2.2628087997436523 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.2953988015651703 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.5053756833076477 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1385 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000004cbfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004cb000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004cbfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004cb000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004df0000003efc0100000002fb0000000800540069006d00650100000000000004df000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000383000004cb00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1247 + X: 1313 + Y: 27 diff --git a/aerial_robot_control/src/trajectory/trajectory_reference/polynomial.cpp b/aerial_robot_control/src/trajectory/trajectory_reference/polynomial.cpp new file mode 100644 index 000000000..50b0334f9 --- /dev/null +++ b/aerial_robot_control/src/trajectory/trajectory_reference/polynomial.cpp @@ -0,0 +1,299 @@ +#include "aerial_robot_control/trajectory/trajectory_reference/polynomial.hpp" + +#include + +namespace agi { + +/* Create exponents as in +/ derivative_order = 0: [ 0 1 2 3 4 ... N] +/ derivative_order = 1: [ 0 0 1 2 3 ... N-1] +/ derivative_order = 2: [ 0 0 0 1 2 ... N-2] +/ ... +*/ +Array<> createExponents(const int order) { + const int N = order + 1; + Array<> P = Array<>::Zero(N, N); + P.col(0) = ArrayVector<>::LinSpaced(N, 0, order); + + for (int i = 1; i < N; ++i) + P.col(i) << ArrayVector<>::Zero(i), P.col(0).head(N - i); + + return P; +} + +/* Create derivative coefficients as in +/ derivative_order = 0: [ 1 1 1 1 1 ...] +/ derivative_order = 1: [ 0 1 2 3 4 ...] +/ derivative_order = 2: [ 0 0 2 6 12 ...] +/ ... +*/ +Array<> createAlpha(const int order) { + const int N = order + 1; + const ArrayVector<> p = ArrayVector<>::LinSpaced(N, 0, order); + + Array<> alpha = Array<>::Ones(N, N); + + for (int i = 1; i < N; ++i) { + alpha.block(i - 1, i, N + 1 - i, N - i) *= + p.head(N + 1 - i).replicate(1, N - i); + } + + return alpha; +} + +template +Polynomial::Polynomial() + : order_(11), + continuity_(-1), + c_(Vector<>::Constant(12, NAN)), + exponents_(createExponents(11)), + alpha_(createAlpha(11)), + weights_(Vector<4>(0, 0, 0, 1)) {} + +template +Polynomial::Polynomial(const int order, const Vector<>& weights, + const int continuity) + : order_(order), + continuity_(continuity), + c_(Vector<>::Constant(order + 1, NAN)), + exponents_(createExponents(order)), + alpha_(createAlpha(order)), + weights_(weights) {} + +template<> +Polynomial::Polynomial() + : order_(5), + continuity_(3), + c_(Vector<>::Constant(6, NAN)), + exponents_(createExponents(5)), + alpha_(createAlpha(5)), + weights_(Vector<3>(0, 0, 1)), + b_(Vector<>::Constant(6, NAN)) {} + +template +bool Polynomial::scale(const Scalar start_time, const Scalar duration) { + bool success = false; + if (std::isfinite(start_time)) { + t_offset_ = start_time; + success = true; + } + + if (std::isfinite(duration)) { + if (duration > 0.0) { + t_scale_ = 1.0 / duration; + } else { + success = false; + } + } + + return success; +} + +template +int Polynomial::addConstraint(const Scalar time, + const Vector<>& constraint) { + const ArrayVector<> constraints = + continuity_ > 0 + ? constraint.head(std::min((int)constraint.size(), continuity_ + 1)) + : constraint; + const int n = constraints.isFinite().cast().sum(); + if (n < 1) return 0; + + const Scalar tau = tauFromTime(time); + + const int n_old = A_.allFinite() ? A_.rows() : 0; + + if (n_old > 0) { + const Matrix<> A_old = A_; + const Vector<> b_old = b_; + + A_.resize(n_old + n, size()); + b_.resize(n_old + n); + + A_.topRows(n_old) = A_old; + b_.head(n_old) = b_old; + } else { + A_.resize(n, size()); + b_.resize(n); + } + + int idx = n_old; + for (int i = 0; i < constraints.size(); ++i) { + if (std::isfinite(constraints(i))) { + A_.row(idx) = + std::pow(t_scale_, i) * alpha_.col(i) * tauVector(tau, i).array(); + b_(idx) = constraints(i); + ++idx; + } + } + + return n; +} + +template<> +int Polynomial::addConstraint(const Scalar time, + const Vector<>& constraint) { + if (constraint.rows() < 3) { + std::cout << "Closed form polynomial constraints must be of size 3!" + << std::endl; + return 0; + } + + if (std::abs(time - t_offset_) < 1e-3) { + b_.head<3>() = constraint.head<3>(); + } else if (std::abs(time - t_offset_ - 1.0 / t_scale_) < 1e-3) { + b_.tail<3>() = constraint.head<3>(); + } else { + std::cout + << "Closed form polynomial constraints must be at start or end time!" + << std::endl; + return 0; + } + + return 3; +} + +template +Scalar Polynomial::tauFromTime(const Scalar t) const { + return t_scale_ * (t - t_offset_); +} + +template +Vector<> Polynomial::tauVector(const Scalar tau, + const int order) const { + return ArrayVector<>::Constant(size(), tau).pow(exponents_.col(order)); +} + +template +Vector<> Polynomial::tauVectorFromTime(const Scalar time, + const int order) const { + return tauVector(tauFromTime(time), order); +} + +template +Matrix<> Polynomial::polyMatrix(const Scalar tau, + const int order) const { + const ArrayVector<> tau_vec = tauVector(tau); + + const int N = size(); + Array<> M = alpha_.block(0, 0, N, order); + + for (int i = 0; i < order; ++i) + M.block(i, i, N - i, 1) *= tau_vec.head(N - i); + + return M.transpose(); +} + +template +Matrix<> Polynomial::polyMatrixFromTime(const Scalar time, + const int order) const { + return polyMatrix(tauFromTime(time), order); +} + +template +void Polynomial::reset() { + H_ = Matrix<>::Constant(1, order_, NAN); + A_ = Matrix<>::Constant(1, order_, NAN); + b_ = Vector<>::Constant(1, NAN); +} + +template +Matrix<> Polynomial::createH(const Vector<>& weights) const { + const int nW = std::min((int)weights.rows(), order_); + const Matrix<> W = weights.head(nW).asDiagonal(); + + const Vector<> time_scales = ArrayVector<>::Constant(nW, t_scale_) + .pow(ArrayVector<>::LinSpaced(nW, 2, 2 * nW)); + + const int n = size(); + Array<> H = Array<>::Zero(n, n); + + for (int i = 0; i < nW; ++i) { + if (weights(i) <= 0.0) continue; + const Vector<> alpha = alpha_.col(i + 1); + const Array<> denom = exponents_.col(i + 1).replicate(1, n); + const Array<> alpha_outer = alpha * alpha.transpose(); + const Array<> denom_outer = (denom + denom.transpose() + 1).max(1.0); + H += time_scales(i) * weights(i) * alpha_outer / denom_outer; + } + return H; +} + +template +bool Polynomial::solve() { + if (A_.rows() < 2) return false; // Cant solve without constraints. + + H_ = createH(weights_); + return solve(H_, A_, b_); +} + +template +bool Polynomial::solve(const Matrix<>& H, const Matrix<>& A, + const Vector<>& b) { + if (H.cols() != size()) return false; + if (A.rows() != b.size() || A.cols() != size()) return false; + + const int n = H.rows(); + const int m = A.rows(); + + const Matrix<> S = + (Matrix<>(n + m, n + m) << 2.0 * H, A.transpose(), A, Matrix<>::Zero(m, m)) + .finished(); + const Vector<> s = (Vector<>(m + n) << Vector<>::Zero(n), b).finished(); + + Solver solver(S); + const Vector<> x = solver.solve(s); + c_ = x.head(n); + + return true; +} + +template<> +bool Polynomial::solve() { + if (!b_.allFinite()) return false; + + // Hardcoded Matrix inversion for the end state constraints. + // This is the inverse of the right half of the coeffcient matrix + // [ 1 1 1 1 1 1 ] + // [ 0 1 1 3 4 5 ] + // [ 0 0 2 6 12 20] + // also known as alpha within this implementation. + static const Matrix<3, 3> A_inv = + (Matrix<3, 3>() << 10, -4, 0.5, -15, 7, -1, 6, -3, 0.5).finished(); + + const ArrayVector<3> t_vec = ArrayVector<3>::Constant(1.0 / t_scale_) + .pow(ArrayVector<3>::LinSpaced(0, 2)); + + c_.head<3>() = t_vec * b_.array().head<3>(); + c_(2) *= 0.5; + c_.tail<3>() = t_vec * b_.array().tail<3>(); + + const Matrix<3, 3> alpha_012 = alpha_.topLeftCorner(3, 3).transpose(); + const Vector<3> b_delta = c_.tail<3>() - alpha_012 * c_.head<3>(); + c_.tail<3>() = A_inv * b_delta; + + return true; +} + + +template +bool Polynomial::eval(const Scalar time, Ref> state) const { + const int n = state.rows(); + const Vector<> time_scales = ArrayVector<>::Constant(n, t_scale_) + .pow(ArrayVector<>::LinSpaced(n, 0, n - 1)); + state = time_scales.asDiagonal() * polyMatrixFromTime(time, n) * c_; + + return true; +} + +template +Scalar Polynomial::operator()(const Scalar time, + const int order) const { + return std::pow(t_scale_, order) * c_.transpose() * + (tauVectorFromTime(time, order).array() * alpha_.col(order)).matrix(); +} + +template class Polynomial>>; +template class Polynomial; + +} // namespace agi diff --git a/aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp b/aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp new file mode 100644 index 000000000..2b1112ef3 --- /dev/null +++ b/aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp @@ -0,0 +1,348 @@ +#include "aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp" + +#include "aerial_robot_control/trajectory/math/gravity.hpp" + + +namespace agi { + +template +PolynomialTrajectory::PolynomialTrajectory( + const QuadState& start_state, const QuadState& end_state, + const Vector<>& weights, const int order, const int continuity, + const std::string& name) + : ReferenceBase(start_state, end_state.t - start_state.t, name), + end_state_(end_state), + x_(order, weights, continuity), + y_(order, weights, continuity), + z_(order, weights, continuity), + yaw_(5, Vector<3>(0, 0, 1)), + states_({start_state, end_state}) { + yaw_last_ = start_state_.getYaw(); + + x_.scale(start_state_.t, duration_); + y_.scale(start_state_.t, duration_); + z_.scale(start_state_.t, duration_); + yaw_.scale(start_state_.t, duration_); + + addStateConstraint(start_state_); + addStateConstraint(end_state_); + + x_.solve(); + y_.solve(); + z_.solve(); + yaw_.solve(); +} + +template<> +PolynomialTrajectory::PolynomialTrajectory( + const QuadState& start_state, const QuadState& end_state, + const Vector<>& weights, const int order, const int continuity, + const std::string& name) + : ReferenceBase(start_state, end_state.t - start_state.t, name), + end_state_(end_state) { + yaw_last_ = start_state_.getYaw(); + + x_.scale(start_state_.t, duration_); + y_.scale(start_state_.t, duration_); + z_.scale(start_state_.t, duration_); + yaw_.scale(start_state_.t, duration_); + + addStateConstraint(start_state_); + addStateConstraint(end_state_); + + x_.solve(); + y_.solve(); + z_.solve(); + yaw_.solve(); +} + +template +PolynomialTrajectory::PolynomialTrajectory( + const std::vector& states, const Vector<>& weights, + const int order, const int continuity, const std::string& name) + : ReferenceBase(states.front(), states.back().t - states.front().t, name), + end_state_(states.back()), + x_(order, weights, continuity), + y_(order, weights, continuity), + z_(order, weights, continuity), + yaw_(5, Vector<3>(0, 0, 1)), + states_(states) { + yaw_last_ = start_state_.getYaw(); + + x_.scale(start_state_.t, duration_); + y_.scale(start_state_.t, duration_); + z_.scale(start_state_.t, duration_); + yaw_.scale(start_state_.t, duration_); + + addStateConstraint(start_state_); + addStateConstraint(end_state_); + + for (size_t i = 1; i < states_.size() - 1; i++) { + addStateConstraint(states_.at(i), 0); // only consider the position constraints for the intermediate points + } + + + if (!x_.solve()) std::cout << "Could not solve x-axis!" << std::endl; + if (!y_.solve()) std::cout << "Could not solve y-axis!" << std::endl; + if (!z_.solve()) std::cout << "Could not solve z-axis!" << std::endl; + if (!yaw_.solve()) std::cout << "Could not solve yaw!" << std::endl; + + start_state_ = getState(start_state_.t); + if (!start_state_.valid()) + std::cout << "Could not fill start state!" << std::endl; + + end_state_ = getState(end_state_.t); + if (!end_state_.valid()) + std::cout << "Could not fill end state!" << std::endl; +} + +template +bool PolynomialTrajectory::addStateConstraint(const QuadState& state, int ord) { + if (!std::isfinite(state.t)) return false; + if (ord > 4) return false; + + if (ord < 0) ord = 4; + + Matrix<> constraints = Matrix<>::Constant(3, ord+1, NAN); + + constraints.col(0) = state.p; + if (ord > 0) constraints.col(1) = state.v; + if (ord > 1) constraints.col(2) = state.a; + if (ord > 2) constraints.col(3) = state.j; + if (ord > 3) constraints.col(4) = state.s; + + x_.addConstraint(state.t, constraints.row(0).transpose()); + y_.addConstraint(state.t, constraints.row(1).transpose()); + z_.addConstraint(state.t, constraints.row(2).transpose()); + + // ToDo: Yaw relative to initial yaw. + const Scalar yaw_angle = state.getYaw(); + if (std::isfinite(yaw_angle)) + yaw_.addConstraint(state.t, Vector<3>(yaw_angle, state.w.z(), 0)); + + return true; +} + +template +Setpoint PolynomialTrajectory::getSetpoint(const QuadState& state, + const Scalar time) { + const Scalar t = std::isfinite(time) ? std::max(state.t, time) : state.t; + Setpoint setpoint; + if (!std::isfinite(t)) return setpoint; + + setpoint.state = getState(t); + setpoint.input.t = time; + setpoint.input.collective_thrust = (setpoint.state.a - GVEC).norm(); + setpoint.input.omega = setpoint.state.w; + + return setpoint; +} + +template +QuadState PolynomialTrajectory::getState(const Scalar time) const { + QuadState state; + if (!valid()) return state; + + const Scalar t = std::clamp(time, start_state_.t, end_state_.t); + + Matrix<> x = Matrix<>::Zero(5, 3); + Vector<> yaw = Vector<>::Zero(3); + + x_.eval(t, x.col(0)); + y_.eval(t, x.col(1)); + z_.eval(t, x.col(2)); + + state.setZero(); + state.t = t; + state.p = x.row(0).transpose(); + state.v = x.row(1).transpose(); + state.a = x.row(2).transpose(); + state.j = x.row(3).transpose(); + state.s = x.row(4).transpose(); + + const Vector<3> thrust_vec = state.a - GVEC; + const Scalar thrust = thrust_vec.norm(); + const Quaternion q_pitch_roll = + thrust > 1e-3 ? Quaternion::FromTwoVectors(Vector<3>::UnitZ(), thrust_vec) + : q_pitch_roll_last_; + q_pitch_roll_last_ = q_pitch_roll; + + if (forward_heading_) { + const Vector<3> v_body = q_pitch_roll.inverse() * state.v; + if ((v_body.x() * v_body.x() + v_body.y() * v_body.y()) < 1e-6) { + yaw(0) = std::atan2(v_body.y(), v_body.x()); + } else { + yaw(0) = yaw_last_; + } + } else { + yaw_.eval(t, yaw); + } + + yaw_last_ = yaw(0); + + const Quaternion q_heading = + Quaternion(Eigen::AngleAxis(yaw(0), Vector<3>::UnitZ())); + const Quaternion q_att = q_pitch_roll * q_heading; + state.q(q_att.normalized()); + + // compute bodyrates + const Vector<3> body_jerk = q_att.inverse() * state.j; + state.w = Vector<3>(-1.0 / thrust * body_jerk[1], 1.0 / thrust * body_jerk[0], + yaw(1)); + + // compute angular acceleration + state.tau = Vector<3>(0, 0, yaw(2)); + return state; +} + +template +Setpoint PolynomialTrajectory::getStartSetpoint() { + return getSetpoint(start_state_); +} + +template +Setpoint PolynomialTrajectory::getEndSetpoint() { + return getSetpoint(end_state_); +} + +template +bool PolynomialTrajectory::solved() const { + if (!x_.solved()) std::cout << "Could not solve x-axis!" << std::endl; + if (!y_.solved()) std::cout << "Could not solve y-axis!" << std::endl; + if (!z_.solved()) std::cout << "Could not solve z-axis!" << std::endl; + if (!yaw_.solved()) std::cout << "Could not solve yaw!" << std::endl; + return x_.solved() && y_.solved() && z_.solved() && yaw_.solved(); +} + +template +bool PolynomialTrajectory::valid() const { + return x_.solved() && y_.solved() && z_.solved() && yaw_.solved(); +} + +template +Vector<> PolynomialTrajectory::evalTranslation( + const Scalar time, const int order) const { + return Vector<3>(x_(time, order), y_(time, order), z_(time, order)); +} + +template +void PolynomialTrajectory::scale(const Scalar start_time, + const Scalar duration) { + if (std::isfinite(start_time)) { + start_state_.t = start_time; + end_state_.t = start_time + duration_; + } + + if (std::isfinite(duration)) { + duration_ = duration; + end_state_.t = start_state_.t + duration_; + } + + x_.scale(start_state_.t, duration_); + y_.scale(start_state_.t, duration_); + z_.scale(start_state_.t, duration_); + yaw_.scale(start_state_.t, duration_); +} + +template +Scalar PolynomialTrajectory::scaleToLimits(const Quadrotor& quad, + const int iterations, + const Scalar tolerance) { + return scaleToLimits(quad.collective_thrust_max() / quad.m_, iterations, + tolerance); +} + +template +Scalar PolynomialTrajectory::scaleToLimits(const Scalar acc_limit, + const int iterations, + const Scalar tolerance) { + if (!std::isfinite(acc_limit) || acc_limit <= 0.0) return duration_; + + for (int k = 0; k < iterations; ++k) { + const Scalar t_max_acc = findTimeMaxAcc(); + const Scalar max_acc = (evalTranslation(t_max_acc, 2) - GVEC).norm(); + const Scalar acc_ratio = max_acc / acc_limit; + const Scalar acc_scale = std::pow(acc_ratio, 0.5); + + // Catch zero acceleration case. + if (max_acc < 1e-3) break; + + duration_ *= acc_scale; + end_state_.t = start_state_.t + duration_; + x_.scale(NAN, duration_); + y_.scale(NAN, duration_); + z_.scale(NAN, duration_); + yaw_.scale(NAN, duration_); + if (std::abs(acc_scale - 1.0) < tolerance) break; + } + + return duration_; +} + +template +Scalar PolynomialTrajectory::findTimeMaxAcc( + const Scalar precision) const { + return findTimeMaxAcc(NAN, NAN, precision); +} + +template +Scalar PolynomialTrajectory::findTimeMaxAcc( + const Scalar t_start, const Scalar t_end, const Scalar precision) const { + return findTime( + getDuration() / 100, precision, t_start, t_end, + [&](const Scalar t) { return (evalTranslation(t, 2) - GVEC).norm(); }, + [](const Scalar x, const Scalar x_max) { return x > x_max; }); +} + +template +Scalar PolynomialTrajectory::findTimeMaxOmega( + const Scalar precision) const { + return findTimeMaxOmega(NAN, NAN, precision); +} + +template +Scalar PolynomialTrajectory::findTimeMaxOmega( + const Scalar t_start, const Scalar t_end, const Scalar precision) const { + return findTime( + getDuration() / 100, precision, t_start, t_end, + [this](const Scalar t) { + const QuadState state = this->getState(t); + return std::max(std::abs(state.w.x()), std::abs(state.w.y())); + }, + [](const Scalar x, const Scalar x_max) { return x > x_max; }); +} + +template +void PolynomialTrajectory::setForwardHeading(const bool forward) { + forward_heading_ = forward; +} + +template +template +Scalar PolynomialTrajectory::findTime( + const Scalar dt, const Scalar dt_min, const Scalar t_start, + const Scalar t_end, EvalFunc eval, CompFunc comp) const { + const Scalar start = std::isfinite(t_start) ? t_start : getStartTime(); + const Scalar end = std::isfinite(t_end) ? t_end : getEndTime(); + Scalar t_max = start; + Scalar x_max = eval(t_max); + + // Sample between bounds [t_start, t_end]. + for (Scalar t = t_max + dt; t <= end; t += dt) { + const Scalar x = eval(t); + if (comp(x, x_max)) { + x_max = x; + t_max = t; + } + } + + // Recursion: search deeper until dt is less or equal dt_min. + return dt <= dt_min ? t_max + : findTime(0.1 * dt, dt_min, std::max(t_max - dt, start), + std::min(t_max + dt, end), eval, comp); +} + +template class PolynomialTrajectory>; +template class PolynomialTrajectory; + +} // namespace agi diff --git a/aerial_robot_control/src/trajectory/trajectory_reference/sampled_trajectory.cpp b/aerial_robot_control/src/trajectory/trajectory_reference/sampled_trajectory.cpp new file mode 100644 index 000000000..93c7fb9ff --- /dev/null +++ b/aerial_robot_control/src/trajectory/trajectory_reference/sampled_trajectory.cpp @@ -0,0 +1,75 @@ +#include "aerial_robot_control/trajectory/trajectory_reference/sampled_trajectory.hpp" + +namespace agi { + +SampledTrajectory::SampledTrajectory(const SetpointVector& setpoints) + : ReferenceBase(setpoints.front().state, + (setpoints.back().state.t - setpoints.front().state.t), + "Sampled Trajectory") { + setpoints_ = setpoints; +} + +Setpoint SampledTrajectory::getSetpoint(const QuadState& state, + const Scalar t) { + // find first point with timestamp larger than query: + SetpointVector::const_iterator upper_setpoint = + std::lower_bound(setpoints_.begin(), setpoints_.end(), t, + [](const Setpoint& setpoint, const Scalar t) -> bool { + return setpoint.state.t <= t; + }); + + // If query time is beyond trajectory, return last available state instead + if (upper_setpoint == setpoints_.end()) return setpoints_.back(); + + // If query time is earlier than reference, return first point + if (upper_setpoint == setpoints_.begin()) return setpoints_.front(); + + SetpointVector::const_iterator lower_setpoint = std::prev(upper_setpoint); + + // interpolate between closest points in SetpointVector + // setpoint = (1 - a) * lower_setpoint + a * upper_setpoint + const double x = (t - lower_setpoint->state.t) / + (upper_setpoint->state.t - lower_setpoint->state.t); + return interpolateSetpoints(*lower_setpoint, *upper_setpoint, x); +} + +Setpoint SampledTrajectory::getStartSetpoint() { return setpoints_.front(); } + +Setpoint SampledTrajectory::getEndSetpoint() { return setpoints_.back(); } + +/// setpoint = (1 - a) * setpoint_1 + a * setpoint_2 +Setpoint SampledTrajectory::interpolateSetpoints(const Setpoint& setpoint_1, + const Setpoint& setpoint_2, + const Scalar x) const { + if (!setpoint_1.state.valid() || !setpoint_2.state.valid() || + !setpoint_1.input.valid() || !setpoint_2.input.valid()) { + return Setpoint{}; + } + + const Scalar x_comp = 1.0 - x; + + // State + Setpoint setpoint; + setpoint.state.t = x_comp * setpoint_1.state.t + x * setpoint_2.state.t; + setpoint.state.x = x_comp * setpoint_1.state.x + x * setpoint_2.state.x; + setpoint.state.q(setpoint_1.state.q().slerp(x, setpoint_2.state.q())); + + // Inputs + setpoint.input.t = x_comp * setpoint_1.input.t + x * setpoint_2.input.t; + if (setpoint_1.input.isRatesThrust()) { + setpoint.input.collective_thrust = + x_comp * setpoint_1.input.collective_thrust + + x * setpoint_2.input.collective_thrust; + setpoint.input.omega = + x_comp * setpoint_1.input.omega + x * setpoint_2.input.omega; + setpoint.input.thrusts.setConstant(NAN); + } else { + setpoint.input.thrusts = + x_comp * setpoint_1.input.thrusts + x * setpoint_2.input.thrusts; + setpoint.input.collective_thrust = NAN; + setpoint.input.omega.setConstant(NAN); + } + + return setpoint; +} +} // namespace agi diff --git a/aerial_robot_control/src/trajectory/types/command.cpp b/aerial_robot_control/src/trajectory/types/command.cpp new file mode 100644 index 000000000..963d7468f --- /dev/null +++ b/aerial_robot_control/src/trajectory/types/command.cpp @@ -0,0 +1,61 @@ +#include "aerial_robot_control/trajectory/types/command.hpp" + +#include + +namespace agi { + +Command::Command() {} + +Command::Command(const Scalar t, const Scalar thrust, const Vector<3>& omega) + : t(t), collective_thrust(thrust), omega(omega) {} + +Command::Command(const Scalar t, const Vector<4>& thrusts) + : t(t), thrusts(thrusts) {} + +Command::Command(const Scalar t) + : t(t), + collective_thrust(0.0), + omega(Vector<3>::Zero()), + thrusts(Vector<4>::Zero()) {} + +bool Command::valid() const { + return std::isfinite(t) && + ((std::isfinite(collective_thrust) && omega.allFinite()) || + thrusts.allFinite()); +} + +bool Command::isSingleRotorThrusts() const { + return std::isfinite(t) && thrusts.allFinite(); +} + +bool Command::isRatesThrust() const { + return std::isfinite(t) && std::isfinite(collective_thrust) && + omega.allFinite(); +} + +std::ostream& operator<<(std::ostream& os, const Command& command) { + os.precision(3); + os << std::scientific; + os << "Command at " << command.t << "s:\n" + << "collective_thrust = [" << command.collective_thrust << "]\n" + << "omega = [" << command.omega.transpose() << "]\n" + << "thrusts = [" << command.thrusts.transpose() << "]" << std::endl; + os.precision(); + os.unsetf(std::ios::scientific); + return os; +} + +bool Command::operator==(const Command& rhs) const { + if (t != rhs.t) { + return false; + } + if (isRatesThrust()) { + return (collective_thrust == rhs.collective_thrust && + omega.isApprox(rhs.omega, 1e-3)); + } else { + return (thrusts.isApprox(rhs.thrusts, 1e-3)); + } +} + + +} // namespace agi diff --git a/aerial_robot_control/src/trajectory/types/quad_state.cpp b/aerial_robot_control/src/trajectory/types/quad_state.cpp new file mode 100644 index 000000000..aafd78970 --- /dev/null +++ b/aerial_robot_control/src/trajectory/types/quad_state.cpp @@ -0,0 +1,111 @@ +#include "aerial_robot_control/trajectory/types/quad_state.hpp" + +namespace agi { + +QuadState::QuadState() {} + +QuadState::QuadState(const Scalar t, const Vector& x) : x(x), t(t) {} + +QuadState::QuadState(const Scalar t, const Vector<3>& position, + const Scalar yaw) + : x(Vector::Zero()), t(t) { + p = position; + q(yaw); +} + +QuadState::QuadState(const QuadState& state) : x(state.x), t(state.t) {} + +QuadState::~QuadState() {} + +Quaternion QuadState::q() const { + return Quaternion(x(ATTW), x(ATTX), x(ATTY), x(ATTZ)); +} + +void QuadState::q(const Quaternion quaternion) { + x(IDX::ATTW) = quaternion.w(); + x(IDX::ATTX) = quaternion.x(); + x(IDX::ATTY) = quaternion.y(); + x(IDX::ATTZ) = quaternion.z(); +} + + +void QuadState::q(const Scalar angle, const Vector<3>& axis) { + const Quaternion q_aa(Eigen::AngleAxis(angle, axis)); + x(IDX::ATTW) = q_aa.w(); + x(IDX::ATTX) = q_aa.x(); + x(IDX::ATTY) = q_aa.y(); + x(IDX::ATTZ) = q_aa.z(); +} + +Matrix<3, 3> QuadState::R() const { + return Quaternion(x(ATTW), x(ATTX), x(ATTY), x(ATTZ)).toRotationMatrix(); +} + +void QuadState::setZero(const bool& reset_time) { + if (reset_time) { + t = 0.0; + } + x.setZero(); + x(ATTW) = 1.0; +} + +void QuadState::linearize() { qx.normalize(); } + +Scalar QuadState::getYaw(const Scalar yaw) const { + const Vector<3> x_b = q() * Vector<3>::UnitX(); + const Vector<3> x_proj = Vector<3>(x_b.x(), x_b.y(), 0); + if (x_proj.norm() < 1e-3) return yaw; + const Vector<3> x_proj_norm = x_proj.normalized(); + const Vector<3> cross = Vector<3>::UnitX().cross(x_proj_norm); + const Scalar angle = asin(cross.z()); + if (x_proj_norm.x() >= 0.0) return angle; + if (x_proj_norm.y() >= 0.0) return M_PI - angle; + return -M_PI - angle; +} + +void QuadState::setYaw(const Scalar angle) { + Quaternion q_yaw; + q_yaw = Eigen::AngleAxis(angle, Vector<3>::UnitZ()); + q(q_yaw); +} + +bool QuadState::operator==(const QuadState& rhs) const { + return t == rhs.t && x.isApprox(rhs.x); +} + +bool QuadState::isApprox(const QuadState& rhs, const Scalar tol) const { + return std::abs(t - rhs.t) < tol && x.isApprox(rhs.x, tol); +} + +QuadState QuadState::getHoverState() const { + QuadState hover_state; + hover_state.setZero(); + hover_state.t = t; + hover_state.p = p; + hover_state.q(getYaw(0.0)); + return hover_state; +} + +std::ostream& operator<<(std::ostream& os, const QuadState& state) { + os.precision(6); + os << std::scientific; + os << "State at " << state.t << "s:\n"; + os.precision(3); + os << "p= [" << state.p.transpose() << "]\n" + << "q= [" << state.qx.transpose() << "]\n" + << "v= [" << state.v.transpose() << "]\n" + << "w= [" << state.w.transpose() << "]\n" + << "a= [" << state.a.transpose() << "]\n" + << "j= [" << state.j.transpose() << "]\n" + << "s= [" << state.s.transpose() << "]\n" + << "tau=[" << state.tau.transpose() << "]\n" + << "bw= [" << state.bw.transpose() << "]\n" + << "ba= [" << state.ba.transpose() << "]\n" + << "mot=[" << state.mot.transpose() << "]\n" + << "dmt=[" << state.motdes.transpose() << "]" << std::endl; + os.precision(); + os.unsetf(std::ios::scientific); + return os; +} + +} // namespace agi diff --git a/aerial_robot_control/src/trajectory/types/quadrotor.cpp b/aerial_robot_control/src/trajectory/types/quadrotor.cpp new file mode 100644 index 000000000..f5f2393b0 --- /dev/null +++ b/aerial_robot_control/src/trajectory/types/quadrotor.cpp @@ -0,0 +1,304 @@ +#include "aerial_robot_control/trajectory/types/quadrotor.hpp" + +#include +#include + +#include "aerial_robot_control/trajectory/math/math.hpp" + +namespace agi { + +Quadrotor::Quadrotor(const Scalar m, const Scalar l) + : m_(m), + t_BM_( + l * sqrt(0.5) * + (Matrix<3, 4>() << 1, -1, -1, 1, -1, 1, -1, 1, 0, 0, 0, 0).finished()), + J_(m_ / 12.0 * l * l * + Vector<3>(2.25, 2.25, 4) + .asDiagonal()), // from cubic volume inertia with height=0.5 l + J_inv_(J_.inverse()), + motor_omega_min_(150.0), + motor_omega_max_(2000.0), + motor_tau_inv_(1.0 / 0.033), + thrust_map_(1.562522e-06, 0.0, 0.0), + torque_map_(1.908873e-08, 0.0, 0.0), + kappa_(torque_map_(0) / thrust_map_(0)), + thrust_min_(0.0), + thrust_max_(motor_omega_max_ * motor_omega_max_ * thrust_map_(0) + + motor_omega_max_ * thrust_map_(1) + thrust_map_(2)), + rotors_config_(RotorConfig::cross), + omega_max_(Vector<3>::Constant(6.0)), + aero_coeff_1_(Vector<3>::Zero()), + aero_coeff_3_(Vector<3>::Zero()), + aero_coeff_h_(0.0) {} + +Quadrotor::Quadrotor() : Quadrotor(1.0, 0.25) {} + +bool Quadrotor::dynamics(const QuadState& state, + QuadState* const derivative) const { + if (derivative == nullptr) return false; + return dynamics(state.x, derivative->x); +} + +bool Quadrotor::dynamics(const Ref> state, + Ref> derivative) const { + if (!state.segment(0).allFinite()) return false; + + derivative.setZero(); + const Vector<3> omega(state(QS::OMEX), state(QS::OMEY), state(QS::OMEZ)); + const Quaternion q_omega(0, omega.x(), omega.y(), omega.z()); + + derivative.segment(QS::POS) = state.segment(QS::VEL); + derivative.segment(QS::ATT) = + 0.5 * Q_right(q_omega) * state.segment(QS::ATT); + derivative.segment(QS::VEL) = state.segment(QS::ACC); + derivative.segment(QS::OME) = + J_inv_ * (state.segment(QS::TAU) - omega.cross(J_ * omega)); + + return true; +} + +bool Quadrotor::jacobian(const Ref> state, + Ref> jac) const { + if (!state.segment(0).allFinite()) return false; + + jac.setZero(); + + const Quaternion q(state(QS::ATTW), state(QS::ATTX), state(QS::ATTY), + state(QS::ATTZ)); + const Vector<3> omega(state(QS::OMEX), state(QS::OMEY), state(QS::OMEZ)); + const Quaternion q_omega(0, omega.x(), omega.y(), omega.z()); + + // Set dp/dv to identity. + jac.block(QS::POS, QS::VEL) = + Matrix::Identity(); + + // Set dq/dq. + jac.block(QS::ATT, QS::ATT) = 0.5 * Q_right(q_omega); + + // Set dq/dw. + jac.block(QS::ATT, QS::OME) = + 0.5 * Q_left(q).block(0, 1); + + // Set dv/da to identity. + jac.block(QS::VEL, QS::ACC) = + Matrix::Identity(); + + // Set dw/dtau to J_inv + jac.block(QS::OME, QS::TAU) = J_inv_; + + // Set dw/dw to J_inv + jac.block(QS::OME, QS::OME) = + -J_inv_ * (skew(omega) * J_ + skew(J_ * omega).transpose()); + + return true; +} + +bool Quadrotor::jacobian(const Ref> state, + SparseMatrix& jac) const { + Matrix jac_dense; + const bool ret = jacobian(state, jac_dense); + jac = jac_dense.sparseView(); + return ret; +} + +DynamicsFunction Quadrotor::getDynamicsFunction() const { + return std::bind( + static_cast>, + Ref>) const>( + &Quadrotor::dynamics), + this, std::placeholders::_1, std::placeholders::_2); +} + +bool Quadrotor::load(const Yaml& node) { + m_ = node["mass"].as(); + + Vector<3> J_diag = J_.diagonal(); + node["inertia"].getIfDefined(J_diag); + J_ = J_diag.asDiagonal(); + J_inv_ = J_.inverse(); + if (node["arm_length"].isDefined()) { + // Sanity check + if (node["rotors_config"].isDefined()) { + if (node["rotors_config"].as() == "plus") { + throw ParameterException( + "Parameter \"arm_length\" cannot be used with \"+\" configuration. " + "Use \"t_BM\" or \"tbm_XX\" instead."); + } + } + + const Scalar l = node["arm_length"].as(); + t_BM_ = + sqrt(0.5) * l * + (Matrix<3, 4>() << 1, -1, -1, 1, -1, 1, -1, 1, 0, 0, 0, 0).finished(); + + } else if (node["t_BM"].isDefined()) { + node["t_BM"] >> t_BM_; + } else if (node["tbm_fr"].isDefined() && node["tbm_bl"].isDefined() && + node["tbm_br"].isDefined() && node["tbm_fl"].isDefined()) { + t_BM_(0, 0) = node["tbm_fr"][0].as(); + t_BM_(1, 0) = node["tbm_fr"][1].as(); + t_BM_(2, 0) = node["tbm_fr"][2].as(); + + t_BM_(0, 1) = node["tbm_bl"][0].as(); + t_BM_(1, 1) = node["tbm_bl"][1].as(); + t_BM_(2, 1) = node["tbm_bl"][2].as(); + + t_BM_(0, 2) = node["tbm_br"][0].as(); + t_BM_(1, 2) = node["tbm_br"][1].as(); + t_BM_(2, 2) = node["tbm_br"][2].as(); + + t_BM_(0, 3) = node["tbm_fl"][0].as(); + t_BM_(1, 3) = node["tbm_fl"][1].as(); + t_BM_(2, 3) = node["tbm_fl"][2].as(); + + } else { + return false; + } + + motor_omega_min_ = node["motor_omega_min"].as(); + motor_omega_max_ = node["motor_omega_max"].as(); + motor_tau_inv_ = node["motor_tau"].as(); + motor_tau_inv_ = 1.0 / motor_tau_inv_; + + node["thrust_map"].getIfDefined(thrust_map_); + node["torque_map"].getIfDefined(torque_map_); + kappa_ = node["kappa"].as(); + thrust_min_ = node["thrust_min"].as(); + thrust_max_ = node["thrust_max"].as(); + + if (node["rotors_config"].isDefined()) { + if (node["rotors_config"].as() == "cross") { + rotors_config_ = RotorConfig::cross; + } else if (node["rotors_config"].as() == "plus") { + rotors_config_ = RotorConfig::plus; + } + } + + node["omega_max"] >> omega_max_; + + node["aero_coeff_1"].getIfDefined(aero_coeff_1_); + node["aero_coeff_3"].getIfDefined(aero_coeff_3_); + node["aero_coeff_h"].getIfDefined(aero_coeff_h_); + + return valid(); +} + +bool Quadrotor::valid() const { + bool check = true; + + // Hardcode a limit of 100 if someone puts the mass in grams. + check &= m_ > 0.0; + check &= m_ < 100.0; + + check &= t_BM_.allFinite(); + check &= J_.allFinite(); + check &= J_inv_.allFinite(); + + check &= motor_omega_min_ >= 0.0; + check &= (motor_omega_max_ > motor_omega_min_); + + check &= thrust_map_.allFinite(); + check &= torque_map_.allFinite(); + check &= kappa_ > 0.0; + check &= thrust_min_ >= 0.0; + check &= (thrust_max_ > thrust_min_); + + check &= (omega_max_.array() > 0).all(); + + check &= (aero_coeff_1_.array() >= 0.0).all(); + check &= (aero_coeff_3_.array() >= 0.0).all(); + check &= aero_coeff_h_ >= 0.0; + + return check; +} + +Vector<4> Quadrotor::clampMotorOmega(const Vector<4>& omega) const { + return omega.cwiseMax(motor_omega_min_).cwiseMin(motor_omega_max_); +} + + +Vector<4> Quadrotor::clampThrust(const Vector<4> thrusts) const { + return thrusts.cwiseMax(thrust_min_).cwiseMin(thrust_max_); +} + + +Scalar Quadrotor::clampThrust(const Scalar thrust) const { + return std::clamp(thrust, thrust_min_, thrust_max_); +} + + +Scalar Quadrotor::clampCollectiveThrust(const Scalar thrust) const { + return std::clamp(thrust, collective_thrust_min(), collective_thrust_max()); +} + + +Vector<3> Quadrotor::clampBodyrates(const Vector<3>& omega) const { + return omega.cwiseMax(-omega_max_).cwiseMin(omega_max_); +} + + +Vector<4> Quadrotor::motorOmegaToThrust(const Vector<4>& omega) const { + const Matrix<4, 3> omega_poly = + (Matrix<4, 3>() << omega.cwiseProduct(omega), omega, Vector<4>::Ones()) + .finished(); + return omega_poly * thrust_map_; +} + +Vector<4> Quadrotor::motorOmegaToTorque(const Vector<4>& omega) const { + const Matrix<4, 3> omega_poly = + (Matrix<4, 3>() << omega.cwiseProduct(omega), omega, Vector<4>::Ones()) + .finished(); + return omega_poly * torque_map_; +} + +Vector<4> Quadrotor::motorThrustToOmega(const Vector<4>& thrusts) const { + // midnight formula + const Scalar scale = 1.0 / (2.0 * thrust_map_(0)); + const Scalar offset = -thrust_map_(1) * scale; + + const Array<4, 1> root = + (std::pow(thrust_map_(1), 2) - + 4.0 * thrust_map_(0) * (thrust_map_(2) - thrusts.array())) + .sqrt(); + + return offset + scale * root; +} + + +Matrix<4, 4> Quadrotor::getAllocationMatrix() const { + // compute column-wise cross product + // tau_i = t_BM_i x F_i + return (Matrix<4, 4>() << Vector<4>::Ones().transpose(), t_BM_.row(1), + -t_BM_.row(0), kappa_ * Vector<4>(-1, -1, 1, 1).transpose()) + .finished(); +} + +std::ostream& operator<<(std::ostream& os, const Quadrotor& quad) { + os.precision(3); + os << std::scientific; + os << "Quadrotor:\n" + << "mass = [" << quad.m_ << "]\n" + << "t_BM = [" << quad.t_BM_.row(0) << "]\n" + << " [" << quad.t_BM_.row(1) << "]\n" + << " [" << quad.t_BM_.row(2) << "]\n" + << "inertia = [" << quad.J_.row(0) << "]\n" + << " [" << quad.J_.row(1) << "]\n" + << " [" << quad.J_.row(2) << "]\n" + << "motor_omega_min = [" << quad.motor_omega_min_ << "]\n" + << "motor_omega_max = [" << quad.motor_omega_max_ << "]\n" + << "motor_tau_inv = [" << quad.motor_tau_inv_ << "]\n" + << "thrust_map = [" << quad.thrust_map_.transpose() << "]\n" + << "torque_map = [" << quad.torque_map_.transpose() << "]\n" + << "kappa = [" << quad.kappa_ << "]\n" + << "thrust_min = [" << quad.thrust_min_ << "]\n" + << "thrust_max = [" << quad.thrust_max_ << "]\n" + << "omega_max = [" << quad.omega_max_.transpose() << "]\n" + << "aero_coeff_1 = [" << quad.aero_coeff_1_.transpose() << "]\n" + << "aero_coeff_3 = [" << quad.aero_coeff_3_.transpose() << "]" + << std::endl; + os.precision(); + os.unsetf(std::ios::scientific); + return os; +} + +} // namespace agi diff --git a/aerial_robot_control/src/trajectory/utils/logger.cpp b/aerial_robot_control/src/trajectory/utils/logger.cpp new file mode 100644 index 000000000..04c64cf87 --- /dev/null +++ b/aerial_robot_control/src/trajectory/utils/logger.cpp @@ -0,0 +1,113 @@ +#include "aerial_robot_control/trajectory/utils/logger.hpp" + +namespace agi { + +std::unordered_map + Logger::publishing_variables_; + +Logger::Logger(const std::string& name, const bool color) + : name_("Logger/" + name + "/"), colored_(color) { + std::cout.precision(DEFAULT_PRECISION); + + // Format name + formatted_name_ = "[" + name + "]"; + if (formatted_name_.size() < NAME_PADDING) + formatted_name_ = + formatted_name_ + std::string(NAME_PADDING - formatted_name_.size(), ' '); + else + formatted_name_ = formatted_name_ + " "; +} + +Logger::~Logger() {} + + +inline std::streamsize Logger::precision(const std::streamsize n) { + return std::cout.precision(n); +} + +inline void Logger::scientific(const bool on) { + if (on) + std::cout << std::scientific; + else + std::cout << std::fixed; +} + +void Logger::info(const char* msg, ...) const { + std::va_list args; + va_start(args, msg); + char buf[MAX_CHARS]; + const int n = std::vsnprintf(buf, MAX_CHARS, msg, args); + va_end(args); + if (n < 0 || n >= MAX_CHARS) + std::cout << formatted_name_ << "=== Logging error ===" << std::endl; + if (colored_) + std::cout << formatted_name_ << buf << std::endl; + else + std::cout << formatted_name_ << INFO << buf << std::endl; +} + +void Logger::warn(const char* msg, ...) const { + std::va_list args; + va_start(args, msg); + char buf[MAX_CHARS]; + const int n = std::vsnprintf(buf, MAX_CHARS, msg, args); + va_end(args); + if (n < 0 || n >= MAX_CHARS) + std::cout << formatted_name_ << "=== Logging error ===" << std::endl; + if (colored_) + std::cout << YELLOW << formatted_name_ << buf << RESET << std::endl; + else + std::cout << formatted_name_ << WARN << buf << std::endl; +} + +void Logger::error(const char* msg, ...) const { + std::va_list args; + va_start(args, msg); + char buf[MAX_CHARS]; + const int n = std::vsnprintf(buf, MAX_CHARS, msg, args); + va_end(args); + if (n < 0 || n >= MAX_CHARS) + std::cout << formatted_name_ << "=== Logging error ===" << std::endl; + if (colored_) + std::cout << RED << formatted_name_ << buf << RESET << std::endl; + else + std::cout << formatted_name_ << ERROR << buf << std::endl; +} + +void Logger::fatal(const char* msg, ...) const { + std::va_list args; + va_start(args, msg); + char buf[MAX_CHARS]; + const int n = std::vsnprintf(buf, MAX_CHARS, msg, args); + va_end(args); + if (n < 0 || n >= MAX_CHARS) + std::cout << formatted_name_ << "=== Logging error ===" << std::endl; + if (colored_) + std::cout << RED << formatted_name_ << buf << RESET << std::endl; + else + std::cout << formatted_name_ << FATAL << buf << std::endl; + throw std::runtime_error(formatted_name_ + buf); +} + +#ifdef DEBUG_LOG +void Logger::debug(const char* msg, ...) const { + std::va_list args; + va_start(args, msg); + char buf[MAX_CHARS]; + const int n = std::vsnprintf(buf, MAX_CHARS, msg, args); + va_end(args); + if (n < 0 || n >= MAX_CHARS) + std::cout << formatted_name_ << "=== Logging error ===" << std::endl; + if (colored_) + std::cout << formatted_name_ << buf << std::endl; + else + std::cout << formatted_name_ << DEBUGPREFIX << buf << std::endl; +} + +std::ostream& Logger::debug() const { return std::cout << formatted_name_; } + +void Logger::debug(const std::function&& lambda) const { lambda(); } + +#endif + +} // namespace agi diff --git a/aerial_robot_control/src/trajectory/utils/timer.cpp b/aerial_robot_control/src/trajectory/utils/timer.cpp new file mode 100644 index 000000000..c7572d21a --- /dev/null +++ b/aerial_robot_control/src/trajectory/utils/timer.cpp @@ -0,0 +1,127 @@ +#include "aerial_robot_control/trajectory/utils/timer.hpp" + +#include +#include + +namespace agi { + +Timer::Timer(const std::string name, const std::string module) + : name_(name), + module_(module), + timing_mean_(0.0), + timing_last_(0.0), + timing_S_(0.0), + timing_min_(std::numeric_limits::max()), + timing_max_(0.0), + n_samples_(0) {} + +Timer::Timer(const Timer& other) + : name_(other.name_), + module_(other.module_), + t_start_(other.t_start_), + timing_mean_(other.timing_mean_), + timing_last_(other.timing_last_), + timing_S_(other.timing_S_), + timing_min_(other.timing_min_), + timing_max_(other.timing_max_), + n_samples_(other.n_samples_) {} + +void Timer::tic() { t_start_ = std::chrono::high_resolution_clock::now(); } + +Scalar Timer::toc() { + // Calculate timing. + const TimePoint t_end = std::chrono::high_resolution_clock::now(); + timing_last_ = 1e-9 * std::chrono::duration_cast( + t_end - t_start_) + .count(); + n_samples_++; + + // Set timing, filter if already initialized. + if (timing_mean_ <= 0.0) { + timing_mean_ = timing_last_; + } else { + const Scalar timing_mean_prev = timing_mean_; + timing_mean_ = + timing_mean_prev + (timing_last_ - timing_mean_prev) / n_samples_; + timing_S_ = timing_S_ + (timing_last_ - timing_mean_prev) * + (timing_last_ - timing_mean_); + } + timing_min_ = (timing_last_ < timing_min_) ? timing_last_ : timing_min_; + timing_max_ = (timing_last_ > timing_max_) ? timing_last_ : timing_max_; + + t_start_ = t_end; + + return timing_last_; +} + +Scalar Timer::operator()() const { return timing_mean_; } + +Scalar Timer::mean() const { return timing_mean_; } + +Scalar Timer::last() const { return timing_last_; } + +Scalar Timer::min() const { return timing_min_; } + +Scalar Timer::max() const { return timing_max_; } + +Scalar Timer::std() const { return std::sqrt(timing_S_ / n_samples_); } + +int Timer::count() const { return n_samples_; } + +Scalar Timer::startTime() const { + return 1e-9 * std::chrono::duration_cast( + t_start_.time_since_epoch()) + .count(); +} + +void Timer::reset() { + n_samples_ = 0u; + t_start_ = TimePoint(); + timing_mean_ = 0.0; + timing_last_ = 0.0; + timing_S_ = 0.0; + timing_min_ = std::numeric_limits::max(); + timing_max_ = 0.0; +} + +void Timer::print() const { std::cout << *this; } + +std::ostream& operator<<(std::ostream& os, const Timer& timer) { + if (!timer.module_.empty()) os << "[" << timer.module_ << "] "; + + if (timer.n_samples_ < 1) { + os << "Timing " << timer.name_ << " has no call yet." << std::endl; + return os; + } + + const std::streamsize prec = os.precision(); + os.precision(3); + + os << "Timing " << timer.name_ << " in " << timer.n_samples_ << " calls" + << std::endl; + + if (!timer.module_.empty()) os << "[" << timer.module_ << "] "; + os << "mean|std: " << 1000 * timer.timing_mean_ << " | " + << 1000 * timer.timing_S_ << " ms " + << "[min|max: " << 1000 * timer.timing_min_ << " | " + << 1000 * timer.timing_max_ << " ms]" << std::endl; + + os.precision(prec); + return os; +} + +ScopedTimer::ScopedTimer(const std::string name, const std::string module) + : Timer(name, module) { + this->tic(); +} + +ScopedTimer::~ScopedTimer() { + this->toc(); + this->print(); +} + +ScopedTicToc::ScopedTicToc(Timer& timer) : timer_(timer) { timer_.tic(); } + +ScopedTicToc::~ScopedTicToc() { timer_.toc(); } + +} // namespace agi diff --git a/aerial_robot_estimation/CMakeLists.txt b/aerial_robot_estimation/CMakeLists.txt index 035607546..2762f8d31 100644 --- a/aerial_robot_estimation/CMakeLists.txt +++ b/aerial_robot_estimation/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.0.2) project(aerial_robot_estimation) -add_compile_options(-std=c++14) +add_compile_options(-std=c++17) find_package(catkin REQUIRED COMPONENTS aerial_robot_model diff --git a/aerial_robot_estimation/include/aerial_robot_estimation/sensor/gps.h b/aerial_robot_estimation/include/aerial_robot_estimation/sensor/gps.h index 1c24d2cff..324afad5e 100644 --- a/aerial_robot_estimation/include/aerial_robot_estimation/sensor/gps.h +++ b/aerial_robot_estimation/include/aerial_robot_estimation/sensor/gps.h @@ -111,7 +111,7 @@ namespace sensor_plugin /* utc time */ /* https://github.com/KumarRobotics/ublox/blob/master/ublox_gps/include/ublox_gps/mkgmtime.h */ time_t mkgmtime(struct tm * const tmp); - int tmcomp(register const struct tm * const atmp, register const struct tm * const btmp); + int tmcomp(const struct tm * const atmp, const struct tm * const btmp); }; }; diff --git a/aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h b/aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h index b1281487a..c9c86128b 100644 --- a/aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h +++ b/aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h @@ -37,11 +37,14 @@ #include #include +#include #include #include #include #include #include +#include + namespace sensor_plugin { @@ -85,11 +88,13 @@ namespace sensor_plugin int fusion_mode_; bool vio_mode_; // visual + inertial mode bool z_vel_mode_; + bool local_vel_mode_; bool outdoor_no_vel_time_sync_; // very special flag for stereo cam such as zed mini, which has tricky behavier in outdoor mode /* heuristic sepecial flag for fusion */ bool outdoor_; bool z_no_delay_; + /* servo */ std::string joint_name_; bool servo_auto_change_flag_; @@ -109,6 +114,8 @@ namespace sensor_plugin double reference_timestamp_; aerial_robot_msgs::States vo_state_; + tf2_ros::StaticTransformBroadcaster static_broadcaster_; // publish the transfrom between the work and vo frame + void rosParamInit(); void servoControl(const ros::TimerEvent & e); void estimateProcess(); diff --git a/aerial_robot_estimation/src/sensor/gps.cpp b/aerial_robot_estimation/src/sensor/gps.cpp index 6ab63fc2d..eb365cc40 100644 --- a/aerial_robot_estimation/src/sensor/gps.cpp +++ b/aerial_robot_estimation/src/sensor/gps.cpp @@ -482,9 +482,9 @@ namespace sensor_plugin time_t Gps::mkgmtime(struct tm * const tmp) { - register int dir; - register int bits; - register int saved_seconds; + int dir; + int bits; + int saved_seconds; time_t t; struct tm yourtm, *mytm; @@ -531,9 +531,9 @@ namespace sensor_plugin return t; } - int Gps::tmcomp(register const struct tm * const atmp, register const struct tm * const btmp) + int Gps::tmcomp(const struct tm * const atmp, const struct tm * const btmp) { - register int result; + int result; if ((result = (atmp->tm_year - btmp->tm_year)) == 0 && (result = (atmp->tm_mon - btmp->tm_mon)) == 0 && diff --git a/aerial_robot_estimation/src/sensor/imu.cpp b/aerial_robot_estimation/src/sensor/imu.cpp index 9836c2da2..42cdf9e80 100644 --- a/aerial_robot_estimation/src/sensor/imu.cpp +++ b/aerial_robot_estimation/src/sensor/imu.cpp @@ -389,29 +389,20 @@ namespace sensor_plugin /* publish state date */ state_.header.stamp = imu_stamp_; - tf::Vector3 pos = estimator_->getPos(Frame::BASELINK, aerial_robot_estimation::EGOMOTION_ESTIMATE); - tf::Vector3 vel = estimator_->getVel(Frame::BASELINK, aerial_robot_estimation::EGOMOTION_ESTIMATE); - state_.states[0].state[0].x = pos.x(); - state_.states[1].state[0].x = pos.y(); - state_.states[2].state[0].x = pos.z(); - state_.states[0].state[0].y = vel.x(); - state_.states[1].state[0].y = vel.y(); - state_.states[2].state[0].y = vel.z(); - state_.states[0].state[0].z = acc_w_.x(); - state_.states[1].state[0].z = acc_w_.y(); - state_.states[2].state[0].z = acc_w_.z(); - pos = estimator_->getPos(Frame::BASELINK, aerial_robot_estimation::EXPERIMENT_ESTIMATE); - vel = estimator_->getVel(Frame::BASELINK, aerial_robot_estimation::EXPERIMENT_ESTIMATE); - state_.states[0].state[1].x = pos.x(); - state_.states[1].state[1].x = pos.y(); - state_.states[2].state[1].x = pos.z(); - state_.states[0].state[1].y = vel.x(); - state_.states[1].state[1].y = vel.y(); - state_.states[2].state[1].y = vel.z(); - state_.states[0].state[1].z = acc_w_.x(); - state_.states[1].state[1].z = acc_w_.y(); - state_.states[2].state[1].z = acc_w_.z(); - + for (int i = 0; i < 2; i++) + { + tf::Vector3 pos = estimator_->getPos(Frame::BASELINK, i); + tf::Vector3 vel = estimator_->getVel(Frame::BASELINK, i); + state_.states[0].state[i].x = pos.x(); + state_.states[1].state[i].x = pos.y(); + state_.states[2].state[i].x = pos.z(); + state_.states[0].state[i].y = vel.x(); + state_.states[1].state[i].y = vel.y(); + state_.states[2].state[i].y = vel.z(); + state_.states[0].state[i].z = acc_w_.x(); + state_.states[1].state[i].z = acc_w_.y(); + state_.states[2].state[i].z = acc_w_.z(); + } state_pub_.publish(state_); } prev_time = imu_stamp_; diff --git a/aerial_robot_estimation/src/sensor/mocap.cpp b/aerial_robot_estimation/src/sensor/mocap.cpp index 33c728608..7098045e4 100644 --- a/aerial_robot_estimation/src/sensor/mocap.cpp +++ b/aerial_robot_estimation/src/sensor/mocap.cpp @@ -80,7 +80,7 @@ namespace sensor_plugin mocap_sub_ = nh_.subscribe(topic_name, 1, &Mocap::poseCallback, this, hint); // buffer size 1: only need the latest value. nhp_.param("ground_truth_sub_name", topic_name, std::string("ground_truth")); - ground_truth_sub_ = nh_.subscribe(topic_name, 1, &Mocap::groundTruthCallback, this); + ground_truth_sub_ = nh_.subscribe(topic_name, 1, &Mocap::groundTruthCallback, this, ros::TransportHints().tcpNoDelay()); } ~Mocap() {} diff --git a/aerial_robot_estimation/src/sensor/vo.cpp b/aerial_robot_estimation/src/sensor/vo.cpp index cfb1b7d40..3e156dc37 100644 --- a/aerial_robot_estimation/src/sensor/vo.cpp +++ b/aerial_robot_estimation/src/sensor/vo.cpp @@ -80,6 +80,7 @@ namespace sensor_plugin if(time_sync_) queuse_size = 10; vo_sub_ = nh_.subscribe(topic_name, queuse_size, &VisualOdometry::voCallback, this); + /* servo control timer */ if(variable_sensor_tf_flag_) { @@ -92,6 +93,8 @@ namespace sensor_plugin servo_control_timer_ = indexed_nhp_.createTimer(ros::Duration(servo_control_rate_), &VisualOdometry::servoControl,this); // 10 Hz } + + prev_timestamp_ = 0; } void VisualOdometry::voCallback(const nav_msgs::Odometry::ConstPtr & vo_msg) @@ -238,33 +241,43 @@ namespace sensor_plugin } } - /* step1: set the init offset from world to the baselink of UAV from egomotion estimation (e.g. yaw) */ - /** ^{w}H_{b} **/ - world_offset_tf_.setRotation(tf::createQuaternionFromYaw(estimator_->getState(State::YAW_BASE, aerial_robot_estimation::EGOMOTION_ESTIMATE)[0])); + /** step1: ^{w}H_{b'}, b': level frame of b **/ + tf::Transform w_bdash_f(tf::createQuaternionFromYaw(estimator_->getState(State::YAW_BASE, aerial_robot_estimation::EGOMOTION_ESTIMATE)[0])); - tf::Vector3 world_offset_pos = estimator_->getPos(Frame::BASELINK, aerial_robot_estimation::EGOMOTION_ESTIMATE); + tf::Vector3 baselink_pos = estimator_->getPos(Frame::BASELINK, aerial_robot_estimation::EGOMOTION_ESTIMATE); if(estimator_->getStateStatus(State::X_BASE, aerial_robot_estimation::EGOMOTION_ESTIMATE)) - world_offset_tf_.getOrigin().setX(world_offset_pos.x()); + w_bdash_f.getOrigin().setX(baselink_pos.x()); if(estimator_->getStateStatus(State::Y_BASE, aerial_robot_estimation::EGOMOTION_ESTIMATE)) - world_offset_tf_.getOrigin().setY(world_offset_pos.y()); + w_bdash_f.getOrigin().setY(baselink_pos.y()); if(estimator_->getStateStatus(State::Z_BASE, aerial_robot_estimation::EGOMOTION_ESTIMATE)) - world_offset_tf_.getOrigin().setZ(world_offset_pos.z()); + w_bdash_f.getOrigin().setZ(baselink_pos.z()); - /* set the init offset from world to the baselink of UAV if we know the ground truth */ + /* set the offset if we know the ground truth */ if(estimator_->getStateStatus(State::YAW_BASE, aerial_robot_estimation::GROUND_TRUTH)) { - world_offset_tf_.setOrigin(estimator_->getPos(Frame::BASELINK, aerial_robot_estimation::GROUND_TRUTH)); - world_offset_tf_.setRotation(tf::createQuaternionFromYaw(estimator_->getState(State::YAW_BASE, aerial_robot_estimation::GROUND_TRUTH)[0])); - - double y, p, r; world_offset_tf_.getBasis().getRPY(r, p, y); + w_bdash_f.setOrigin(estimator_->getPos(Frame::BASELINK, aerial_robot_estimation::GROUND_TRUTH)); + w_bdash_f.setRotation(tf::createQuaternionFromYaw(estimator_->getState(State::YAW_BASE, aerial_robot_estimation::GROUND_TRUTH)[0])); } - /* step2: also consider the offset tf from baselink to sensor */ - /** ^{w}H_{b} * ^{b}H_{vo} * ^{vo}H_{w_vo} = ^{w}H_{w_vo} **/ - world_offset_tf_ *= (sensor_tf_ * raw_sensor_tf.inverse()); + /** step2: ^{vo}H_{b'} **/ + tf::Transform vo_bdash_f = raw_sensor_tf * sensor_tf_.inverse(); // ^{vo}H_{b} + double r,p,y; + vo_bdash_f.getBasis().getRPY(r,p,y); + vo_bdash_f.setRotation(tf::createQuaternionFromYaw(y)); // ^{vo}H_{b'} + + /** step3: ^{w}H_{vo} = ^{w}H_{b'} * ^{b'}H_{vo} **/ + world_offset_tf_ = w_bdash_f * vo_bdash_f.inverse(); + + /* publish the offset tf if necessary */ + geometry_msgs::TransformStamped static_transformStamped; + static_transformStamped.header.stamp = vo_msg->header.stamp; + static_transformStamped.header.frame_id = "world"; + static_transformStamped.child_frame_id = vo_msg->header.frame_id; + tf::transformTFToMsg(world_offset_tf_, static_transformStamped.transform); + static_broadcaster_.sendTransform(static_transformStamped); + + tf::Vector3 init_pos = w_bdash_f.getOrigin(); - //double y, p, r; raw_sensor_tf.getBasis().getRPY(r, p, y); - tf::Vector3 init_pos = (world_offset_tf_ * raw_sensor_tf * sensor_tf_.inverse()).getOrigin(); for(auto& fuser : estimator_->getFuser(aerial_robot_estimation::EGOMOTION_ESTIMATE)) { @@ -353,8 +366,8 @@ namespace sensor_plugin tf::quaternionMsgToTF(vo_msg->pose.pose.orientation, raw_q); // velocity: - tf::Vector3 raw_local_vel; - tf::vector3MsgToTF(vo_msg->twist.twist.linear, raw_local_vel); + tf::Vector3 raw_vel; + tf::vector3MsgToTF(vo_msg->twist.twist.linear, raw_vel); /* get the latest orientation and omega */ baselink_r = estimator_->getOrientation(Frame::BASELINK, aerial_robot_estimation::EGOMOTION_ESTIMATE); baselink_omega = estimator_->getAngularVel(Frame::BASELINK, aerial_robot_estimation::EGOMOTION_ESTIMATE); @@ -365,11 +378,11 @@ namespace sensor_plugin // int mode = estimator_->getStateStatus(State::YAW_BASE, aerial_robot_estimation::GROUND_TRUTH)?aerial_robot_estimation::GROUND_TRUTH:aerial_robot_estimation::EGOMOTION_ESTIMATE; int mode = aerial_robot_estimation::EGOMOTION_ESTIMATE; - if (raw_local_vel == tf::Vector3(0.0,0.0,0.0)) + if (raw_vel == tf::Vector3(0.0,0.0,0.0)) { /* the odometry message does not contain velocity information, we have to calulcate by ourselves. */ tf::Transform delta_tf = prev_sensor_tf.inverse() * raw_sensor_tf; - raw_local_vel = delta_tf.getOrigin() / (curr_timestamp_ - prev_timestamp_); + raw_vel = delta_tf.getOrigin() / (curr_timestamp_ - prev_timestamp_); reference_timestamp_ = (curr_timestamp_ + prev_timestamp_) / 2; estimator_->findRotOmega(reference_timestamp_, mode, baselink_r, baselink_omega); @@ -384,7 +397,16 @@ namespace sensor_plugin } } - raw_global_vel_ = baselink_r * ( sensor_tf_.getBasis() * raw_local_vel - baselink_omega.cross(sensor_tf_.getOrigin())); + raw_global_vel_ = world_offset_tf_.getBasis() * raw_vel; + if (local_vel_mode_) + { + // if the velocity is described in local frame (i.e., the sensor frame), + // we need to convert to global one + raw_global_vel_ = baselink_r * sensor_tf_.getBasis() * raw_vel; + } + // consider the offset between baselink and sensor frames + raw_global_vel_ -= baselink_r * baselink_omega.cross(sensor_tf_.getOrigin()); + if(debug_verbose_) { @@ -600,6 +622,7 @@ namespace sensor_plugin { getParam("fusion_mode", fusion_mode_, (int)ONLY_POS_MODE); getParam("vio_mode", vio_mode_, false); + getParam("local_vel_mode", local_vel_mode_, true); getParam("z_vel_mode", z_vel_mode_, false); getParam("z_no_delay", z_no_delay_, false); getParam("outdoor_no_vel_time_sync", outdoor_no_vel_time_sync_, false); diff --git a/aerial_robot_kinetic.rosinstall b/aerial_robot_kinetic.rosinstall index efc5f0e79..fc7b02212 100644 --- a/aerial_robot_kinetic.rosinstall +++ b/aerial_robot_kinetic.rosinstall @@ -2,7 +2,7 @@ - git: local-name: aerial_robot_3rdparty uri: https://github.com/JSKAerialRobot/aerial_robot_3rdparty.git - version: fb963df + version: b10f5bf # kalman filter - git: diff --git a/aerial_robot_melodic.rosinstall b/aerial_robot_melodic.rosinstall index 9445e1aaa..bd5dbf534 100644 --- a/aerial_robot_melodic.rosinstall +++ b/aerial_robot_melodic.rosinstall @@ -2,7 +2,7 @@ - git: local-name: aerial_robot_3rdparty uri: https://github.com/JSKAerialRobot/aerial_robot_3rdparty.git - version: fb963df + version: b10f5bf # kalman filter - git: @@ -31,6 +31,20 @@ uri: https://github.com/tongtybj/rosserial.git version: rosservice_server +# livox ros driver +# modification for quick build of ROS1 version +- git: + local-name: livox_ros_driver2 + uri: https://github.com/tongtybj/livox_ros_driver2.git + version: PR/ros1 + +# fast lio +# modification for fast odometry based on EKF +- git: + local-name: fast_lio + uri: https://github.com/tongtybj/FAST_LIO + version: PR/odometry + # realsense #- git: # local-name: realsense-ros diff --git a/aerial_robot_model/CMakeLists.txt b/aerial_robot_model/CMakeLists.txt index e39d73147..43babfb41 100644 --- a/aerial_robot_model/CMakeLists.txt +++ b/aerial_robot_model/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.0.2) project(aerial_robot_model) -add_compile_options(-std=c++14) +add_compile_options(-std=c++17) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/aerial_robot_model/include/aerial_robot_model/servo_bridge.h b/aerial_robot_model/include/aerial_robot_model/servo_bridge.h index 3ab50b5ef..bf94aa6c9 100644 --- a/aerial_robot_model/include/aerial_robot_model/servo_bridge.h +++ b/aerial_robot_model/include/aerial_robot_model/servo_bridge.h @@ -50,6 +50,8 @@ #include #include #include +#include +#include #include #include #include @@ -200,6 +202,8 @@ class ServoBridge ros::Publisher servo_states_pub_; ros::Publisher mujoco_control_input_pub_; + ros::Publisher joint_profile_pub_; + ros::Subscriber uav_info_sub_; map servo_states_subs_; map servo_ctrl_subs_; map no_real_state_flags_; @@ -219,6 +223,7 @@ class ServoBridge void servoStatesCallback(const spinal::ServoStatesConstPtr& state_msg, const std::string& servo_group_name); void servoCtrlCallback(const sensor_msgs::JointStateConstPtr& joints_ctrl_msg, const std::string& servo_group_name); bool servoTorqueCtrlCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res, const std::string& servo_group_name); + void uavInfoCallback(const spinal::UavInfoConstPtr& uav_msg); public: ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp); diff --git a/aerial_robot_model/src/servo_bridge/servo_bridge.cpp b/aerial_robot_model/src/servo_bridge/servo_bridge.cpp index 0e7927bbe..bd3545834 100644 --- a/aerial_robot_model/src/servo_bridge/servo_bridge.cpp +++ b/aerial_robot_model/src/servo_bridge/servo_bridge.cpp @@ -73,12 +73,16 @@ ServoBridge::ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh),nhp_( mujoco_control_input_pub_ = nh_.advertise("mujoco/ctrl_input", 1); /* common publisher: torque on/off command */ servo_torque_ctrl_pubs_.insert(make_pair("common", nh_.advertise(torque_pub_topic, 1))); + /* common publisher: joint profiles */ + joint_profile_pub_ = nh_.advertise("joint_profiles",1); + /* subscriber: uav info */ + uav_info_sub_ = nh_.subscribe("uav_info", 1, &ServoBridge::uavInfoCallback, this); /* get additional config for servos from ros parameters */ XmlRpc::XmlRpcValue all_servos_params; nh_.getParam("servo_controller", all_servos_params); - + spinal::JointProfiles joint_profiles_msg; for(auto servo_group_params: all_servos_params) { if (servo_group_params.second.getType() != XmlRpc::XmlRpcValue::TypeStruct) @@ -126,7 +130,8 @@ ServoBridge::ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh),nhp_( double lower_limit = urdf_model.getJoint(servo_params.second["name"])->limits->lower; /* get parameters from rosparam */ - int angle_sgn = servo_params.second.hasMember("angle_sgn")? + int servo_id = servo_params.second["id"]; + int angle_sgn = servo_params.second.hasMember("angle_sgn")? servo_params.second["angle_sgn"]:servo_group_params.second["angle_sgn"]; int zero_point_offset = servo_params.second.hasMember("zero_point_offset")? servo_params.second["zero_point_offset"]:servo_group_params.second["zero_point_offset"]; @@ -391,3 +396,29 @@ bool ServoBridge::servoTorqueCtrlCallback(std_srvs::SetBool::Request &req, std_s return true; } +void ServoBridge::uavInfoCallback(const spinal::UavInfoConstPtr& uav_msg) +{ + /* Send servo profiles to Spinal*/ + spinal::JointProfiles joint_profiles_msg; + for(auto servo_group : servos_handler_){ + for(auto servo : servo_group.second){ + spinal::JointProfile joint_profile; + if(servo_group.first == "joints"){ + joint_profile.type = spinal::JointProfile::JOINT; + } + else if(servo_group.first == "gimbals"){ + joint_profile.type = spinal::JointProfile::GIMBAL; + } + else{ + ROS_ERROR("Invalid servo type. Please define 'joints' or 'gimbals'."); + continue; + } + joint_profile.servo_id = servo->getId(); + joint_profile.angle_sgn = servo->getAngleSgn(); + joint_profile.angle_scale = servo->getAngleScale(); + joint_profile.zero_point_offset = servo->getZeroPointOffset(); + joint_profiles_msg.joints.push_back(joint_profile); + } + } + joint_profile_pub_.publish(joint_profiles_msg); +} diff --git a/aerial_robot_msgs/msg/FlightNav.msg b/aerial_robot_msgs/msg/FlightNav.msg index 3ba0a9046..f6774938b 100644 --- a/aerial_robot_msgs/msg/FlightNav.msg +++ b/aerial_robot_msgs/msg/FlightNav.msg @@ -24,6 +24,16 @@ float64 target_pos_y float32 target_vel_y float32 target_acc_y +uint8 roll_nav_mode + +float32 target_omega_x +float32 target_roll + +uint8 pitch_nav_mode + +float32 target_omega_y +float32 target_pitch + uint8 yaw_nav_mode float32 target_omega_z diff --git a/aerial_robot_nerve/spinal/CMakeLists.txt b/aerial_robot_nerve/spinal/CMakeLists.txt index 821f5cd3e..8724a9ff2 100644 --- a/aerial_robot_nerve/spinal/CMakeLists.txt +++ b/aerial_robot_nerve/spinal/CMakeLists.txt @@ -43,6 +43,8 @@ add_message_files( FlightConfigCmd.msg Vector3Int16.msg TorqueAllocationMatrixInv.msg + JointProfile.msg + JointProfiles.msg ) add_service_files( @@ -52,6 +54,7 @@ add_service_files( SetAttitudeGains.srv ImuCalib.srv MagDeclination.srv + SetDirectServoConfig.srv ) generate_messages( diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/ethernetif.h b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/ethernetif.h index 38b97f29d..e6eb67c1a 100644 --- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/ethernetif.h +++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/ethernetif.h @@ -6,7 +6,7 @@ ****************************************************************************** * @attention * - *

© Copyright (c) 2022 STMicroelectronics. + *

© Copyright (c) 2024 STMicroelectronics. * All rights reserved.

* * This software component is licensed by ST under Ultimate Liberty license diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/lwip.h b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/lwip.h index b3b4cfbbe..f050f5bba 100644 --- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/lwip.h +++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/lwip.h @@ -6,7 +6,7 @@ ****************************************************************************** * @attention * - *

© Copyright (c) 2022 STMicroelectronics. + *

© Copyright (c) 2024 STMicroelectronics. * All rights reserved.

* * This software component is licensed by ST under Ultimate Liberty license diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/lwipopts.h b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/lwipopts.h index 0872507f2..237c87efd 100644 --- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/lwipopts.h +++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/lwipopts.h @@ -7,7 +7,7 @@ ****************************************************************************** * @attention * - *

© Copyright (c) 2022 STMicroelectronics. + *

© Copyright (c) 2024 STMicroelectronics. * All rights reserved.

* * This software component is licensed by ST under Ultimate Liberty license diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.cproject b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.cproject index a83aeedc7..c8520e279 100644 --- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.cproject +++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.cproject @@ -25,7 +25,7 @@