diff --git a/aerial_robot_base/include/aerial_robot_base/aerial_robot_base.h b/aerial_robot_base/include/aerial_robot_base/aerial_robot_base.h index 07a79f132..d1576b9a6 100644 --- a/aerial_robot_base/include/aerial_robot_base/aerial_robot_base.h +++ b/aerial_robot_base/include/aerial_robot_base/aerial_robot_base.h @@ -6,7 +6,7 @@ #include #include #include -#include +#include using namespace std; diff --git a/aerial_robot_control/include/aerial_robot_control/control/control_base.h b/aerial_robot_control/include/aerial_robot_control/control/control_base.h index 90b54f195..9d2ae19cd 100644 --- a/aerial_robot_control/include/aerial_robot_control/control/control_base.h +++ b/aerial_robot_control/include/aerial_robot_control/control/control_base.h @@ -38,7 +38,7 @@ #include #include -#include +#include #include #include #include diff --git a/aerial_robot_estimation/include/aerial_robot_estimation/state_estimation.h b/aerial_robot_estimation/include/aerial_robot_estimation/state_estimation.h index ba5edb604..762134ace 100644 --- a/aerial_robot_estimation/include/aerial_robot_estimation/state_estimation.h +++ b/aerial_robot_estimation/include/aerial_robot_estimation/state_estimation.h @@ -35,7 +35,7 @@ #pragma once -#include +#include #include #include #include diff --git a/aerial_robot_model/CMakeLists.txt b/aerial_robot_model/CMakeLists.txt index 9814f2ba0..7529cbab8 100644 --- a/aerial_robot_model/CMakeLists.txt +++ b/aerial_robot_model/CMakeLists.txt @@ -52,7 +52,7 @@ generate_messages( catkin_package( INCLUDE_DIRS include test - LIBRARIES transformable_aerial_robot_model transformable_aerial_robot_model_ros numerical_jacobians + LIBRARIES aerial_robot_model aerial_robot_model_ros numerical_jacobians robot_model_pluginlib CATKIN_DEPENDS eigen_conversions geometry_msgs interactive_markers kalman_filter kdl_parser message_runtime pluginlib sensor_msgs spinal std_msgs tf tf_conversions tf2_eigen tf2_geometry_msgs tf2_kdl tf2_ros tf_conversions urdf visualization_msgs DEPENDS orocos_kdl urdfdom_headers ) @@ -73,17 +73,24 @@ include_directories( ${EIGEN3_INCLUDE_DIRS} ) -add_library(transformable_aerial_robot_model - src/transformable_aerial_robot_model/robot_model.cpp - src/transformable_aerial_robot_model/jacobians.cpp - src/transformable_aerial_robot_model/kinematics.cpp - src/transformable_aerial_robot_model/stability.cpp - src/transformable_aerial_robot_model/statics.cpp) -target_link_libraries(transformable_aerial_robot_model ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES} ${EIGEN3_LIBRARIES}) -add_library(transformable_aerial_robot_model_ros src/transformable_aerial_robot_model/robot_model_ros.cpp) -target_link_libraries(transformable_aerial_robot_model_ros transformable_aerial_robot_model ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES} ${EIGEN3_LIBRARIES}) -add_dependencies(transformable_aerial_robot_model_ros ${PROJECT_NAME}_generate_messages_cpp spinal_generate_messages_cpp) +add_library(aerial_robot_model + src/model/base_model/robot_model.cpp + src/model/transformable_model/robot_model.cpp + src/model/transformable_model/jacobians.cpp + src/model/transformable_model/kinematics.cpp + src/model/transformable_model/stability.cpp + src/model/transformable_model/statics.cpp) +target_link_libraries(aerial_robot_model ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES} ${EIGEN3_LIBRARIES}) + +add_library(aerial_robot_model_ros src/model/base_model/robot_model_ros.cpp) +target_link_libraries(aerial_robot_model_ros aerial_robot_model ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES} ${EIGEN3_LIBRARIES}) +add_dependencies(aerial_robot_model_ros ${PROJECT_NAME}_generate_messages_cpp spinal_generate_messages_cpp) + +add_library(robot_model_pluginlib + src/model/plugin/multirotor_robot_model.cpp + src/model/plugin/tilted_underactuated_robot_model.cpp) +target_link_libraries(robot_model_pluginlib ${catkin_LIBRARIES} aerial_robot_model) add_library(servo_bridge src/servo_bridge/servo_bridge.cpp) target_link_libraries(servo_bridge ${catkin_LIBRARIES}) @@ -98,13 +105,13 @@ target_link_libraries(interactive_marker_tf_broadcaster ${catkin_LIBRARIES}) ## for rostest add_library(numerical_jacobians test/aerial_robot_model/numerical_jacobians.cpp) -target_link_libraries(numerical_jacobians transformable_aerial_robot_model ${catkin_LIBRARIES}) +target_link_libraries(numerical_jacobians aerial_robot_model ${catkin_LIBRARIES}) install(DIRECTORY include/${PROJECT_NAME}/ test/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(TARGETS transformable_aerial_robot_model transformable_aerial_robot_model_ros servo_bridge numerical_jacobians +install(TARGETS aerial_robot_model aerial_robot_model_ros robot_model_pluginlib servo_bridge numerical_jacobians DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) @@ -112,7 +119,7 @@ install(TARGETS rotor_tf_publisher interactive_marker_tf_broadcaster servo_bridg DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(DIRECTORY launch script +install(DIRECTORY launch script plugins DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS ) diff --git a/aerial_robot_model/include/aerial_robot_model/transformable_aerial_robot_model.h b/aerial_robot_model/include/aerial_robot_model/model/aerial_robot_model.h similarity index 75% rename from aerial_robot_model/include/aerial_robot_model/transformable_aerial_robot_model.h rename to aerial_robot_model/include/aerial_robot_model/model/aerial_robot_model.h index 5817e62f5..2dbdb1e4a 100644 --- a/aerial_robot_model/include/aerial_robot_model/transformable_aerial_robot_model.h +++ b/aerial_robot_model/include/aerial_robot_model/model/aerial_robot_model.h @@ -35,8 +35,8 @@ #pragma once -#include -#include +#include +#include #include #include #include @@ -58,32 +58,25 @@ namespace aerial_robot_model { - - //Transformable Aerial Robot Model + //Basic Aerial Robot Model class RobotModel { public: - RobotModel(bool init_with_rosparam = true, bool verbose = false, double fc_f_min_thre = 0, double fc_t_min_thre = 0, double epsilon = 10.0); + RobotModel(bool init_with_rosparam = true, bool verbose = false, bool fixed_model = true, double fc_f_min_thre = 0, double fc_t_min_thre = 0, double epsilon = 10.0); virtual ~RobotModel() = default; - virtual void updateJacobians(); - virtual void updateJacobians(const KDL::JntArray& joint_positions, bool update_model = true); - void updateRobotModel(const KDL::JntArray& joint_positions) { updateRobotModelImpl(joint_positions); } - void updateRobotModel(const sensor_msgs::JointState& state) { updateRobotModel(jointMsgToKdl(state)); } + void updateRobotModel(); + void updateRobotModel(const KDL::JntArray& joint_positions); + void updateRobotModel(const sensor_msgs::JointState& state); // kinematics - bool addExtraModule(std::string module_name, std::string parent_link_name, KDL::Frame transform, KDL::RigidBodyInertia inertia); - virtual void calcBasicKinematicsJacobian(); - virtual void calcCoGMomentumJacobian(); - const Eigen::MatrixXd& getCOGJacobian() const {return cog_jacobian_;} - - template T forwardKinematics(std::string link, const KDL::JntArray& joint_positions) const; - template T forwardKinematics(std::string link, const sensor_msgs::JointState& state) const; - std::map fullForwardKinematics(const KDL::JntArray& joint_positions) {return fullForwardKinematicsImpl(joint_positions); } - std::map fullForwardKinematics(const sensor_msgs::JointState& state) {return fullForwardKinematics(jointMsgToKdl(state)); } - + const bool initialized() const { return initialized_; } + const bool isModelFixed() const {return fixed_model_; } const std::string getBaselinkName() const { return baselink_; } - const std::vector& getCOGCoordJacobians() const {return cog_coord_jacobians_;} const std::map& getInertiaMap() const { return inertia_map_; } + const double getMass() const { return mass_; } + const int getRotorNum() const { return rotor_num_; } + const std::map& getRotorDirection() { return rotor_direction_; } + const std::string getRootFrameName() const { return GetTreeElementSegment(tree_.getRootSegment()->second).getName(); } const int getJointNum() const { return joint_num_;} const KDL::JntArray& getJointPositions() const { return joint_positions_; } const std::map& getJointIndexMap() const { return joint_index_map_; } @@ -92,32 +85,24 @@ namespace aerial_robot_model { const std::vector& getJointNames() const { return joint_names_; } const std::vector& getJointIndices() const { return joint_indices_; } const std::vector& getJointParentLinkNames() const { return joint_parent_link_names_; } - const std::vector& getLinkJointNames() const { return link_joint_names_; } - const std::vector& getLinkJointIndices() const { return link_joint_indices_; } - const std::vector& getLinkJointLowerLimits() const { return link_joint_lower_limits_; } - const std::vector& getLinkJointUpperLimits() const { return link_joint_upper_limits_; } - const Eigen::MatrixXd& getLMomentumJacobian() const {return l_momentum_jacobian_;} - const double getLinkLength() const { return link_length_; } - const double getMass() const { return mass_; } - const std::vector& getPJacobians() const {return p_jacobians_;} - const int getRotorNum() const { return rotor_num_; } - const std::map& getRotorDirection() { return rotor_direction_; } - const std::string getRootFrameName() const { return GetTreeElementSegment(tree_.getRootSegment()->second).getName(); } - const std::map getSegmentsTf() - { + + const std::map getSegmentsTf() { std::lock_guard lock(mutex_seg_tf_); return seg_tf_map_; } - const KDL::Frame getSegmentTf(const std::string seg_name) - { + + const KDL::Frame getSegmentTf(const std::string seg_name) { std::lock_guard lock(mutex_seg_tf_); return seg_tf_map_.at(seg_name); } - const std::vector& getThrustCoordJacobians() const {return thrust_coord_jacobians_;} + template T forwardKinematics(std::string link, const KDL::JntArray& joint_positions) const; + template T forwardKinematics(std::string link, const sensor_msgs::JointState& state) const; + std::map fullForwardKinematics(const KDL::JntArray& joint_positions) {return fullForwardKinematicsImpl(joint_positions); } + std::map fullForwardKinematics(const sensor_msgs::JointState& state) {return fullForwardKinematics(jointMsgToKdl(state)); } + const KDL::Tree& getTree() const { return tree_; } const urdf::Model& getUrdfModel() const { return model_; } - const std::vector& getUJacobians() const {return u_jacobians_;} const double getVerbose() const { return verbose_; } template T getCog(); @@ -131,8 +116,6 @@ namespace aerial_robot_model { KDL::JntArray jointMsgToKdl(const sensor_msgs::JointState& state) const; sensor_msgs::JointState kdlJointToMsg(const KDL::JntArray& joint_positions) const; - bool removeExtraModule(std::string module_name); - void setBaselinkName(const std::string baselink) { baselink_ = baselink; } void setCogDesireOrientation(double roll, double pitch, double yaw) { @@ -144,29 +127,17 @@ namespace aerial_robot_model { cog_desire_orientation_ = cog_desire_orientation; } - KDL::JntArray convertEigenToKDL(const Eigen::VectorXd& joint_vector) { - const auto& joint_indices = getJointIndices(); - KDL::JntArray joint_positions(getTree().getNrOfJoints()); - for (unsigned int i = 0; i < joint_indices.size(); ++i) { - joint_positions(joint_indices.at(i)) = joint_vector(i); - } - return joint_positions; - } + bool addExtraModule(std::string module_name, std::string parent_link_name, KDL::Frame transform, KDL::RigidBodyInertia inertia); + bool removeExtraModule(std::string module_name); // statics (static thrust, joint torque) Eigen::VectorXd calcGravityWrenchOnRoot(); - virtual void calcLambdaJacobian(); - virtual void calcJointTorque(const bool update_jacobian = true); - virtual void calcJointTorqueJacobian(); virtual void calcStaticThrust(); Eigen::MatrixXd calcWrenchMatrixOnCoG(); virtual void calcWrenchMatrixOnRoot(); const Eigen::VectorXd& getGravity() const {return gravity_;} const Eigen::VectorXd& getGravity3d() const {return gravity_3d_;} - const Eigen::VectorXd& getJointTorque() const {return joint_torque_;} - const Eigen::MatrixXd& getJointTorqueJacobian() const {return joint_torque_jacobian_;} - const Eigen::MatrixXd& getLambdaJacobian() const {return lambda_jacobian_;} const double getMFRate() const {return m_f_rate_;} const Eigen::VectorXd& getStaticThrust() const {return static_thrust_;} const std::vector& getThrustWrenchAllocations() const {return thrust_wrench_allocations_;} @@ -176,20 +147,15 @@ namespace aerial_robot_model { const double getThrustLowerLimit() const {return thrust_min_;} // control stability - virtual void calcFeasibleControlJacobian(); virtual void calcFeasibleControlFDists(); virtual void calcFeasibleControlTDists(); double calcTripleProduct(const Eigen::Vector3d& ui, const Eigen::Vector3d& uj, const Eigen::Vector3d& uk); std::vector calcV(); - const Eigen::VectorXd& getApproxFeasibleControlFDists() const {return approx_fc_f_dists_;} - const Eigen::VectorXd& getApproxFeasibleControlTDists() const {return approx_fc_t_dists_;} const double getEpsilon() const {return epsilon_;} const Eigen::VectorXd& getFeasibleControlFDists() const {return fc_f_dists_;} - const Eigen::MatrixXd& getFeasibleControlFDistsJacobian() const { return fc_f_dists_jacobian_; } const double& getFeasibleControlFMin() const {return fc_f_min_;} const double& getFeasibleControlFMinThre() const {return fc_f_min_thre_;} const Eigen::VectorXd& getFeasibleControlTDists() const {return fc_t_dists_;} - const Eigen::MatrixXd& getFeasibleControlTDistsJacobian() const { return fc_t_dists_jacobian_; } const double& getFeasibleControlTMin() const {return fc_t_min_;} const double& getFeasibleControlTMinThre() const {return fc_t_min_thre_;} @@ -198,57 +164,31 @@ namespace aerial_robot_model { virtual bool stabilityCheck(bool verbose = true); - - // jacobian - virtual Eigen::MatrixXd convertJacobian(const Eigen::MatrixXd& in); - virtual Eigen::MatrixXd getJacobian(const KDL::JntArray& joint_positions, std::string segment_name, KDL::Vector offset = KDL::Vector::Zero()); - Eigen::MatrixXd getSecondDerivative(std::string ref_frame, int joint_i, KDL::Vector offset = KDL::Vector::Zero()); - Eigen::MatrixXd getSecondDerivativeRoot(std::string ref_frame, KDL::Vector offset = KDL::Vector::Zero()); - Eigen::VectorXd getHessian(std::string ref_frame, int joint_i, int joint_j, KDL::Vector offset = KDL::Vector::Zero()); - + KDL::JntArray convertEigenToKDL(const Eigen::VectorXd& joint_vector); private: - //private attributes - // kinematics + bool initialized_; + bool fixed_model_; + double mass_; + urdf::Model model_; std::string baselink_; KDL::Frame cog_; KDL::Rotation cog_desire_orientation_; KDL::Frame cog2baselink_transform_; - std::map extra_module_map_; - std::map inertia_map_; - std::map joint_index_map_; // index in KDL::JntArray - std::map > joint_segment_map_; - std::map joint_hierachy_; - std::vector joint_names_; // index in KDL::JntArray std::vector joint_indices_; // index in KDL::JntArray std::vector joint_parent_link_names_; // index in KDL::JntArray KDL::JntArray joint_positions_; KDL::RotationalInertia link_inertia_cog_; - std::vector link_joint_names_; // index in KDL::JntArray - std::vector link_joint_indices_; // index in KDL::JntArray - std::vector link_joint_lower_limits_, link_joint_upper_limits_; - double link_length_; - - double mass_; - urdf::Model model_; - std::mutex mutex_cog_; - std::mutex mutex_cog2baselink_; - std::mutex mutex_inertia_; - std::mutex mutex_rotor_origin_; - std::mutex mutex_rotor_normal_; - std::mutex mutex_seg_tf_; - std::mutex mutex_desired_baselink_rot_; - - + std::map extra_module_map_; + std::map inertia_map_; + std::map joint_index_map_; // index in KDL::JntArray + std::map > joint_segment_map_; + std::map joint_hierachy_; std::map seg_tf_map_; - - std::vector u_jacobians_; //thrust direction vector index:rotor - std::vector p_jacobians_; //thrust position index:rotor - int joint_num_; int rotor_num_; std::vector rotors_origin_from_cog_; @@ -256,51 +196,56 @@ namespace aerial_robot_model { KDL::Tree tree_; std::string thrust_link_; bool verbose_; - Eigen::MatrixXd cog_jacobian_; //cog jacobian - Eigen::MatrixXd l_momentum_jacobian_; //angular_momemtum jacobian + // statics (static thrust, joint torque) - std::vector cog_coord_jacobians_; Eigen::VectorXd gravity_; Eigen::VectorXd gravity_3d_; - Eigen::VectorXd joint_torque_; - Eigen::MatrixXd joint_torque_jacobian_; // joint torque - Eigen::MatrixXd lambda_jacobian_; //thrust force double m_f_rate_; //moment / force rate Eigen::MatrixXd q_mat_; std::map rotor_direction_; Eigen::VectorXd static_thrust_; - std::vector thrust_coord_jacobians_; double thrust_max_; double thrust_min_; std::vector thrust_wrench_units_; std::vector thrust_wrench_allocations_; // control stability - Eigen::VectorXd approx_fc_f_dists_; - Eigen::VectorXd approx_fc_t_dists_; double epsilon_; Eigen::VectorXd fc_f_dists_; // distances to the plane of feasible control force convex Eigen::VectorXd fc_t_dists_; // distances to the plane of feasible control torque convex - Eigen::MatrixXd fc_f_dists_jacobian_; - Eigen::MatrixXd fc_t_dists_jacobian_; double fc_f_min_; double fc_t_min_; double fc_f_min_thre_; double fc_t_min_thre_; + // mutex + std::mutex mutex_cog_; + std::mutex mutex_cog2baselink_; + std::mutex mutex_inertia_; + std::mutex mutex_rotor_origin_; + std::mutex mutex_rotor_normal_; + std::mutex mutex_seg_tf_; + std::mutex mutex_desired_baselink_rot_; + + //private functions - KDL::Frame forwardKinematicsImpl(std::string link, const KDL::JntArray& joint_positions) const; - std::map fullForwardKinematicsImpl(const KDL::JntArray& joint_positions); void getParamFromRos(); - KDL::RigidBodyInertia inertialSetup(const KDL::TreeElement& tree_element); - void jointSegmentSetupRecursive(const KDL::TreeElement& tree_element, std::vector current_joints); - void makeJointSegmentMap(); - void resolveLinkLength(); void kinematicsInit(); void stabilityInit(); void staticsInit(); + + KDL::RigidBodyInertia inertialSetup(const KDL::TreeElement& tree_element); + void jointSegmentSetupRecursive(const KDL::TreeElement& tree_element, std::vector current_joints); + void makeJointSegmentMap(); + + KDL::Frame forwardKinematicsImpl(std::string link, const KDL::JntArray& joint_positions) const; + std::map fullForwardKinematicsImpl(const KDL::JntArray& joint_positions); + + protected: + virtual void updateRobotModelImpl(const KDL::JntArray& joint_positions); + void setCog(const KDL::Frame cog) { std::lock_guard lock(mutex_cog_); @@ -332,24 +277,12 @@ namespace aerial_robot_model { seg_tf_map_ = seg_tf_map; } - protected: - - // folllowing functions can be only accessed from derived class - void setCOGCoordJacobians(const std::vector cog_coord_jacobians) {cog_coord_jacobians_ = cog_coord_jacobians;} - void setCOGJacobian(const Eigen::MatrixXd cog_jacobian) {cog_jacobian_ = cog_jacobian;} - void setJointTorque(const Eigen::VectorXd joint_torque) {joint_torque_ = joint_torque;} - void setJointTorqueJacobian(const Eigen::MatrixXd joint_torque_jacobian) {joint_torque_jacobian_ = joint_torque_jacobian;} - void setLambdaJacobian(const Eigen::MatrixXd lambda_jacobian) {lambda_jacobian_ = lambda_jacobian;} - void setLMomentumJacobian(const Eigen::MatrixXd l_momentum_jacobian) {l_momentum_jacobian_ = l_momentum_jacobian;} - void setPJacobians(const std::vector p_jacobians) {p_jacobians_ = p_jacobians;} void setStaticThrust(const Eigen::VectorXd static_thrust) {static_thrust_ = static_thrust;} - void setThrustTCoordJacobians(const std::vector thrust_coord_jacobians) {thrust_coord_jacobians_ = thrust_coord_jacobians;} void setThrustWrenchMatrix(const Eigen::MatrixXd q_mat) {q_mat_ = q_mat;} - void setUJacobians(const std::vector u_jacobians) {u_jacobians_ = u_jacobians;} - virtual void updateRobotModelImpl(const KDL::JntArray& joint_positions); }; + template<> inline Eigen::Affine3d RobotModel::forwardKinematics(std::string link, const KDL::JntArray& joint_positions) const { return aerial_robot_model::kdlToEigen(forwardKinematicsImpl(link, joint_positions)); @@ -495,4 +428,4 @@ namespace aerial_robot_model { { return aerial_robot_model::kdlToTf2(RobotModel::getRotorsOriginFromCog()); } -} //namespace aerial_robot_model +} // namespace aerial_robot_model diff --git a/aerial_robot_model/include/aerial_robot_model/transformable_aerial_robot_model_ros.h b/aerial_robot_model/include/aerial_robot_model/model/aerial_robot_model_ros.h similarity index 95% rename from aerial_robot_model/include/aerial_robot_model/transformable_aerial_robot_model_ros.h rename to aerial_robot_model/include/aerial_robot_model/model/aerial_robot_model_ros.h index f10d86368..3b9fc9c7b 100644 --- a/aerial_robot_model/include/aerial_robot_model/transformable_aerial_robot_model_ros.h +++ b/aerial_robot_model/include/aerial_robot_model/model/aerial_robot_model_ros.h @@ -35,12 +35,14 @@ #pragma once -#include +#include #include #include #include #include #include +#include + namespace aerial_robot_model { @@ -61,6 +63,7 @@ namespace aerial_robot_model { ros::Subscriber desire_coordinate_sub_; ros::Subscriber joint_state_sub_; tf2_ros::TransformBroadcaster br_; + tf2_ros::StaticTransformBroadcaster static_br_; sensor_msgs::JointState joint_state_; ros::NodeHandle nh_; ros::NodeHandle nhp_; diff --git a/aerial_robot_model/include/aerial_robot_model/model/plugin/multirotor_robot_model.h b/aerial_robot_model/include/aerial_robot_model/model/plugin/multirotor_robot_model.h new file mode 100644 index 000000000..64226bbc2 --- /dev/null +++ b/aerial_robot_model/include/aerial_robot_model/model/plugin/multirotor_robot_model.h @@ -0,0 +1,46 @@ + // -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include + +class MultirotorRobotModel : public aerial_robot_model::RobotModel { + +public: + MultirotorRobotModel(bool init_with_rosparam = true, bool verbose = false, double fc_t_min_thre = 0, double epsilon = 10.0); + virtual ~MultirotorRobotModel() = default; +}; + diff --git a/aerial_robot_model/include/aerial_robot_model/model/plugin/underactuated_tilted_robot_model.h b/aerial_robot_model/include/aerial_robot_model/model/plugin/underactuated_tilted_robot_model.h new file mode 100644 index 000000000..ee0b8003d --- /dev/null +++ b/aerial_robot_model/include/aerial_robot_model/model/plugin/underactuated_tilted_robot_model.h @@ -0,0 +1,47 @@ + // -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include + +using namespace aerial_robot_model; + +class UnderactuatedTiltedRobotModel : public RobotModel { + +public: + UnderactuatedTiltedRobotModel(bool init_with_rosparam = true, bool verbose = false, double fc_t_min_thre = 0, double epsilon = 10.0); + virtual ~UnderactuatedTiltedRobotModel() = default; +}; diff --git a/aerial_robot_model/include/aerial_robot_model/model/transformable_aerial_robot_model.h b/aerial_robot_model/include/aerial_robot_model/model/transformable_aerial_robot_model.h new file mode 100644 index 000000000..907e070e4 --- /dev/null +++ b/aerial_robot_model/include/aerial_robot_model/model/transformable_aerial_robot_model.h @@ -0,0 +1,138 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include + +namespace aerial_robot_model { + namespace transformable { + + //Transformable Aerial Robot Model + class RobotModel : public ::aerial_robot_model::RobotModel { + public: + RobotModel(bool init_with_rosparam = true, bool verbose = false, double fc_f_min_thre = 0, double fc_t_min_thre = 0, double epsilon = 10.0); + virtual ~RobotModel() = default; + + virtual void updateJacobians(); + virtual void updateJacobians(const KDL::JntArray& joint_positions, bool update_model = true); + + // kinematics + const std::vector& getLinkJointNames() const { return link_joint_names_; } + const std::vector& getLinkJointIndices() const { return link_joint_indices_; } + const std::vector& getLinkJointLowerLimits() const { return link_joint_lower_limits_; } + const std::vector& getLinkJointUpperLimits() const { return link_joint_upper_limits_; } + const double getLinkLength() const { return link_length_; } + + // statics (static thrust, joint torque) + virtual void calcJointTorque(const bool update_jacobian = true); + + // stability + const Eigen::VectorXd& getApproxFeasibleControlFDists() const {return approx_fc_f_dists_;} + const Eigen::VectorXd& getApproxFeasibleControlTDists() const {return approx_fc_t_dists_;} + + // jacobian + virtual void calcBasicKinematicsJacobian(); + virtual void calcCoGMomentumJacobian(); + const Eigen::MatrixXd& getCOGJacobian() const {return cog_jacobian_;} + const std::vector& getCOGCoordJacobians() const {return cog_coord_jacobians_;} + const Eigen::MatrixXd& getLMomentumJacobian() const {return l_momentum_jacobian_;} + const std::vector& getPJacobians() const {return p_jacobians_;} + const std::vector& getThrustCoordJacobians() const {return thrust_coord_jacobians_;} + const std::vector& getUJacobians() const {return u_jacobians_;} + virtual void calcLambdaJacobian(); + virtual void calcJointTorqueJacobian(); + const Eigen::VectorXd& getJointTorque() const {return joint_torque_;} + const Eigen::MatrixXd& getJointTorqueJacobian() const {return joint_torque_jacobian_;} + const Eigen::MatrixXd& getLambdaJacobian() const {return lambda_jacobian_;} + virtual void calcFeasibleControlJacobian(); + const Eigen::MatrixXd& getFeasibleControlFDistsJacobian() const { return fc_f_dists_jacobian_; } + const Eigen::MatrixXd& getFeasibleControlTDistsJacobian() const { return fc_t_dists_jacobian_; } + + // utils + virtual Eigen::MatrixXd convertJacobian(const Eigen::MatrixXd& in); + virtual Eigen::MatrixXd getJacobian(const KDL::JntArray& joint_positions, std::string segment_name, KDL::Vector offset = KDL::Vector::Zero()); + Eigen::MatrixXd getSecondDerivative(std::string ref_frame, int joint_i, KDL::Vector offset = KDL::Vector::Zero()); + Eigen::MatrixXd getSecondDerivativeRoot(std::string ref_frame, KDL::Vector offset = KDL::Vector::Zero()); + Eigen::VectorXd getHessian(std::string ref_frame, int joint_i, int joint_j, KDL::Vector offset = KDL::Vector::Zero()); + + private: + + //private attributes + + // kinematics + std::vector link_joint_names_; // index in KDL::JntArray + std::vector link_joint_indices_; // index in KDL::JntArray + std::vector link_joint_lower_limits_, link_joint_upper_limits_; + double link_length_; + + // statics + Eigen::VectorXd joint_torque_; + + // stability + Eigen::VectorXd approx_fc_f_dists_; + Eigen::VectorXd approx_fc_t_dists_; + + // jacobians + std::vector u_jacobians_; //thrust direction vector index:rotor + std::vector p_jacobians_; //thrust position index:rotor + Eigen::MatrixXd cog_jacobian_; //cog jacobian + Eigen::MatrixXd l_momentum_jacobian_; //angular_momemtum jacobian + std::vector cog_coord_jacobians_; + Eigen::MatrixXd joint_torque_jacobian_; // joint torque + Eigen::MatrixXd lambda_jacobian_; //thrust force + std::vector thrust_coord_jacobians_; + Eigen::MatrixXd fc_f_dists_jacobian_; + Eigen::MatrixXd fc_t_dists_jacobian_; + + //private functions + void resolveLinkLength(); + + protected: + + void setJointTorque(const Eigen::VectorXd joint_torque) {joint_torque_ = joint_torque;} + + // Jacobians + void setCOGCoordJacobians(const std::vector cog_coord_jacobians) {cog_coord_jacobians_ = cog_coord_jacobians;} + void setCOGJacobian(const Eigen::MatrixXd cog_jacobian) {cog_jacobian_ = cog_jacobian;} + void setJointTorqueJacobian(const Eigen::MatrixXd joint_torque_jacobian) {joint_torque_jacobian_ = joint_torque_jacobian;} + void setLambdaJacobian(const Eigen::MatrixXd lambda_jacobian) {lambda_jacobian_ = lambda_jacobian;} + void setLMomentumJacobian(const Eigen::MatrixXd l_momentum_jacobian) {l_momentum_jacobian_ = l_momentum_jacobian;} + void setPJacobians(const std::vector p_jacobians) {p_jacobians_ = p_jacobians;} + void setThrustTCoordJacobians(const std::vector thrust_coord_jacobians) {thrust_coord_jacobians_ = thrust_coord_jacobians;} + void setUJacobians(const std::vector u_jacobians) {u_jacobians_ = u_jacobians;} + }; + } // namespace transformable +} //namespace aerial_robot_model diff --git a/aerial_robot_model/include/aerial_robot_model/kdl_utils.h b/aerial_robot_model/include/aerial_robot_model/utils/kdl_utils.h similarity index 100% rename from aerial_robot_model/include/aerial_robot_model/kdl_utils.h rename to aerial_robot_model/include/aerial_robot_model/utils/kdl_utils.h diff --git a/aerial_robot_model/include/aerial_robot_model/math_utils.h b/aerial_robot_model/include/aerial_robot_model/utils/math_utils.h similarity index 100% rename from aerial_robot_model/include/aerial_robot_model/math_utils.h rename to aerial_robot_model/include/aerial_robot_model/utils/math_utils.h diff --git a/aerial_robot_model/package.xml b/aerial_robot_model/package.xml index 42d4c11f2..b4886e6bc 100644 --- a/aerial_robot_model/package.xml +++ b/aerial_robot_model/package.xml @@ -55,4 +55,8 @@ visualization_msgs xacro + + + + diff --git a/aerial_robot_model/plugins/robot_model_plugins.xml b/aerial_robot_model/plugins/robot_model_plugins.xml new file mode 100644 index 000000000..2d204daba --- /dev/null +++ b/aerial_robot_model/plugins/robot_model_plugins.xml @@ -0,0 +1,11 @@ + + + + plugin for general multirotor model + + + + plugin for underactuated tilted robot model + + + diff --git a/aerial_robot_model/src/transformable_aerial_robot_model/kinematics.cpp b/aerial_robot_model/src/model/base_model/robot_model.cpp similarity index 50% rename from aerial_robot_model/src/transformable_aerial_robot_model/kinematics.cpp rename to aerial_robot_model/src/model/base_model/robot_model.cpp index a915405e5..726a68289 100644 --- a/aerial_robot_model/src/transformable_aerial_robot_model/kinematics.cpp +++ b/aerial_robot_model/src/model/base_model/robot_model.cpp @@ -1,174 +1,47 @@ -#include +#include namespace aerial_robot_model { - bool RobotModel::addExtraModule(std::string module_name, std::string parent_link_name, KDL::Frame transform, KDL::RigidBodyInertia inertia) - { - if(extra_module_map_.find(module_name) == extra_module_map_.end()) - { - if(inertia_map_.find(parent_link_name) == inertia_map_.end()) - { - ROS_WARN("[extra module]: fail to add new extra module %s, because its parent link (%s) does not exist", module_name.c_str(), parent_link_name.c_str()); - return false; - } - - if(!aerial_robot_model::isValidRotation(transform.M)) - { - ROS_WARN("[extra module]: fail to add new extra module %s, because its orientation is invalid", module_name.c_str()); - return false; - } - - if(inertia.getMass() <= 0) - { - ROS_WARN("[extra module]: fail to add new extra module %s, becuase its mass %f is invalid", module_name.c_str(), inertia.getMass()); - return false; - } - - KDL::Segment extra_module(parent_link_name, KDL::Joint(KDL::Joint::None), transform, inertia); - extra_module_map_.insert(std::make_pair(module_name, extra_module)); - ROS_INFO("[extra module]: succeed to add new extra module %s", module_name.c_str()); - return true; - } - else - { - ROS_WARN("[extra module]: fail to add new extra module %s, becuase it already exists", module_name.c_str()); - return false; - } - } - - void RobotModel::calcBasicKinematicsJacobian() - { - const std::vector p = getRotorsOriginFromCog(); - const std::vector u = getRotorsNormalFromCog(); - const auto& joint_positions = getJointPositions(); - const auto& sigma = getRotorDirection(); - const int rotor_num = getRotorNum(); - const double m_f_rate = getMFRate(); - - //calc jacobian of u(thrust direction, force vector), p(thrust position) - for (int i = 0; i < rotor_num; ++i) { - std::string seg_name = std::string("thrust") + std::to_string(i + 1); - Eigen::MatrixXd thrust_coord_jacobian = RobotModel::getJacobian(joint_positions, seg_name); - thrust_coord_jacobians_.at(i) = thrust_coord_jacobian; - u_jacobians_.at(i) = -skew(u.at(i)) * thrust_coord_jacobian.bottomRows(3); - p_jacobians_.at(i) = thrust_coord_jacobian.topRows(3) - cog_jacobian_; - } - } - - void RobotModel::calcCoGMomentumJacobian() + RobotModel::RobotModel(bool init_with_rosparam, bool verbose, bool fixed_model, double fc_f_min_thre, double fc_t_min_thre, double epsilon): + verbose_(verbose), + fixed_model_(fixed_model), + fc_f_min_thre_(fc_f_min_thre), + fc_t_min_thre_(fc_t_min_thre), + epsilon_(epsilon), + baselink_("fc"), + thrust_link_("thrust"), + rotor_num_(0), + joint_num_(0), + thrust_max_(0), + thrust_min_(0), + mass_(0), + initialized_(false) { - double mass_all = getMass(); - const auto cog_all = getCog().p; - const auto& segment_map = getTree().getSegments(); - const auto seg_frames = getSegmentsTf(); - const auto& inertia_map = getInertiaMap(); - const auto& joint_names = getJointNames(); - const auto& joint_segment_map = getJointSegmentMap(); - const auto& joint_parent_link_names = getJointParentLinkNames(); - const auto joint_num = getJointNum(); + if (init_with_rosparam) + getParamFromRos(); - Eigen::MatrixXd root_rot = aerial_robot_model::kdlToEigen(getCogDesireOrientation() * seg_frames.at(baselink_).M.Inverse()); - /* - Note: the jacobian about the cog velocity (linear momentum) and angular momentum. - - Please refer to Eq.13 ~ 22 of following paper: - ============================================================================ - S. Kajita et al., "Resolved momentum control: humanoid motion planning based on the linear and angular momentum,". - ============================================================================ - - 1. the cog velocity is w.r.t in the root link frame. - 2. the angular momentum is w.r.t in cog frame - */ - - - int col_index = 0; - /* fix bug: the joint_segment_map is reordered, which is not match the order of joint_indeices_ or joint_names_ */ - // joint part - for (const auto& joint_name : joint_names){ - std::string joint_child_segment_name = joint_segment_map.at(joint_name).at(0); - KDL::Segment joint_child_segment = GetTreeElementSegment(segment_map.at(joint_child_segment_name)); - KDL::Vector a = seg_frames.at(joint_parent_link_names.at(col_index)).M * joint_child_segment.getJoint().JointAxis(); - - KDL::Vector r = seg_frames.at(joint_child_segment_name).p; - KDL::RigidBodyInertia inertia = KDL::RigidBodyInertia::Zero(); - for (const auto& seg : joint_segment_map.at(joint_name)) { - if (seg.find("thrust") == std::string::npos) { - KDL::Frame f = seg_frames.at(seg); - inertia = inertia + f * inertia_map.at(seg); - } - } - KDL::Vector c = inertia.getCOG(); - double m = inertia.getMass(); + gravity_.resize(6); + gravity_ << 0, 0, 9.80665, 0, 0, 0; + gravity_3d_.resize(3); + gravity_3d_ << 0, 0, 9.80665; - KDL::Vector p_momentum_jacobian_col = a * (c - r) * m; - KDL::Vector l_momentum_jacobian_col = (c - cog_all) * p_momentum_jacobian_col + inertia.RefPoint(c).getRotationalInertia() * a; + kinematicsInit(); + stabilityInit(); + staticsInit(); - cog_jacobian_.col(6 + col_index) = aerial_robot_model::kdlToEigen(p_momentum_jacobian_col / mass_all); - l_momentum_jacobian_.col(6 + col_index) = aerial_robot_model::kdlToEigen(l_momentum_jacobian_col); - col_index++; + // update robot model instantly for fixed model + if (fixed_model_) { + updateRobotModel(); } - - // virtual 6dof root - cog_jacobian_.leftCols(3) = Eigen::MatrixXd::Identity(3, 3); - cog_jacobian_.middleCols(3, 3) = - aerial_robot_model::skew(aerial_robot_model::kdlToEigen(cog_all)); - cog_jacobian_ = root_rot * cog_jacobian_; - - l_momentum_jacobian_.leftCols(3) = Eigen::MatrixXd::Zero(3, 3); - l_momentum_jacobian_.middleCols(3, 3) = getInertia() * root_rot; // aready converted - l_momentum_jacobian_.rightCols(joint_num) = root_rot * l_momentum_jacobian_.rightCols(joint_num); } - KDL::Frame RobotModel::forwardKinematicsImpl(std::string link, const KDL::JntArray& joint_positions) const + void RobotModel::getParamFromRos() { - if (joint_positions.rows() != tree_.getNrOfJoints()) - throw std::runtime_error("joint num is invalid"); - - KDL::TreeFkSolverPos_recursive fk_solver(tree_); - KDL::Frame f; - int status = fk_solver.JntToCart(joint_positions, f, link); - if(status < 0) ROS_ERROR("can not solve FK to link: %s", link.c_str()); - - return f; - } - - std::map RobotModel::fullForwardKinematicsImpl(const KDL::JntArray& joint_positions) - { - if (joint_positions.rows() != tree_.getNrOfJoints()) - throw std::runtime_error("joint num is invalid"); - - std::map seg_tf_map; - std::function recursiveFullFk = [&recursiveFullFk, &seg_tf_map, &joint_positions](const KDL::TreeElement& tree_element, const KDL::Frame& parrent_f) - { - for (const auto& elem: GetTreeElementChildren(tree_element)) - { - const KDL::TreeElement& curr_element = elem->second; - KDL::Frame curr_f = parrent_f * GetTreeElementSegment(curr_element).pose(joint_positions(GetTreeElementQNr(curr_element))); - seg_tf_map.insert(std::make_pair(GetTreeElementSegment(curr_element).getName(), curr_f)); - recursiveFullFk(curr_element, curr_f); - } - }; - - recursiveFullFk(tree_.getRootSegment()->second, KDL::Frame::Identity()); - - return seg_tf_map; - } - - - TiXmlDocument RobotModel::getRobotModelXml(const std::string param, ros::NodeHandle nh) - { - std::string xml_string; - TiXmlDocument xml_doc; - - if (!nh.hasParam(param)) - { - ROS_ERROR("Could not find parameter %s on parameter server with namespace '%s'", param.c_str(), nh.getNamespace().c_str()); - return xml_doc; - } - - nh.getParam(param, xml_string); - xml_doc.Parse(xml_string.c_str()); - - return xml_doc; + ros::NodeHandle nh; + nh.param("kinematic_verbose", verbose_, false); + nh.param("fc_f_min_thre", fc_f_min_thre_, 0.0); + nh.param("fc_t_min_thre", fc_t_min_thre_, 0.0); + nh.param("epsilon", epsilon_, 10.0); } void RobotModel::kinematicsInit() @@ -217,32 +90,52 @@ namespace aerial_robot_model { return; } - inertialSetup(tree_.getRootSegment()->second); makeJointSegmentMap(); - resolveLinkLength(); rotors_origin_from_cog_.resize(rotor_num_); rotors_normal_from_cog_.resize(rotor_num_); + } + + void RobotModel::stabilityInit() + { + fc_f_dists_.resize(rotor_num_ * (rotor_num_ - 1)); + fc_t_dists_.resize(rotor_num_ * (rotor_num_ - 1)); + } + + void RobotModel::staticsInit() + { + /* get baselink and thrust_link from robot model */ + auto robot_model_xml = getRobotModelXml("robot_description"); + + /* set rotor property */ + TiXmlElement* m_f_rate_attr = robot_model_xml.FirstChildElement("robot")->FirstChildElement("m_f_rate"); + if(!m_f_rate_attr) + ROS_ERROR("Can not get m_f_rate attribute from urdf model"); + else + m_f_rate_attr->Attribute("value", &m_f_rate_); - for(auto itr : link_joint_names_) + std::vector urdf_links; + model_.getLinks(urdf_links); + for(const auto& link: urdf_links) { - auto joint_ptr = model_.getJoint(itr); - link_joint_lower_limits_.push_back(joint_ptr->limits->lower); - link_joint_upper_limits_.push_back(joint_ptr->limits->upper); + if(link->parent_joint) + { + if(link->parent_joint->name == "rotor1") + { + thrust_max_ = link->parent_joint->limits->upper; + thrust_min_ = link->parent_joint->limits->lower; + break; + } + } } - // jacobian - const int full_body_dof = 6 + joint_num_; - u_jacobians_.resize(rotor_num_); - p_jacobians_.resize(rotor_num_); - thrust_coord_jacobians_.resize(rotor_num_); - cog_coord_jacobians_.resize(getInertiaMap().size()); - cog_jacobian_.resize(3, full_body_dof); - l_momentum_jacobian_.resize(3, full_body_dof); + q_mat_.resize(6, rotor_num_); + static_thrust_.resize(rotor_num_); + thrust_wrench_units_.resize(rotor_num_); + thrust_wrench_allocations_.resize(rotor_num_); } - KDL::RigidBodyInertia RobotModel::inertialSetup(const KDL::TreeElement& tree_element) { const KDL::Segment current_seg = GetTreeElementSegment(tree_element); @@ -275,13 +168,6 @@ namespace aerial_robot_model { joint_indices_.push_back(tree_element.q_nr); joint_parent_link_names_.push_back(GetTreeElementParent(tree_element)->first); - /* extract link joint */ - if(current_seg.getJoint().getName().find("joint") == 0) - { - link_joint_names_.push_back(current_seg.getJoint().getName()); - link_joint_indices_.push_back(tree_element.q_nr); - } - if(verbose_) ROS_WARN("Add new inertia base link: %s", current_seg.getName().c_str()); } } @@ -323,6 +209,18 @@ namespace aerial_robot_model { return current_seg_inertia; } + void RobotModel::makeJointSegmentMap() + { + joint_segment_map_.clear(); + for (const auto joint_index : joint_index_map_) { + std::vector empty_vec; + joint_segment_map_[joint_index.first] = empty_vec; + } + + std::vector current_joints; + jointSegmentSetupRecursive(getTree().getRootSegment()->second, current_joints); + } + void RobotModel::jointSegmentSetupRecursive(const KDL::TreeElement& tree_element, std::vector current_joints) { const auto inertia_map = getInertiaMap(); @@ -353,18 +251,45 @@ namespace aerial_robot_model { return; } - void RobotModel::makeJointSegmentMap() + bool RobotModel::addExtraModule(std::string module_name, std::string parent_link_name, KDL::Frame transform, KDL::RigidBodyInertia inertia) { - joint_segment_map_.clear(); - for (const auto joint_index : joint_index_map_) { - std::vector empty_vec; - joint_segment_map_[joint_index.first] = empty_vec; - } + if(extra_module_map_.find(module_name) == extra_module_map_.end()) + { + if(inertia_map_.find(parent_link_name) == inertia_map_.end()) + { + ROS_WARN("[extra module]: fail to add new extra module %s, because its parent link (%s) does not exist", module_name.c_str(), parent_link_name.c_str()); + return false; + } - std::vector current_joints; - jointSegmentSetupRecursive(getTree().getRootSegment()->second, current_joints); - } + if(!aerial_robot_model::isValidRotation(transform.M)) + { + ROS_WARN("[extra module]: fail to add new extra module %s, because its orientation is invalid", module_name.c_str()); + return false; + } + + if(inertia.getMass() <= 0) + { + ROS_WARN("[extra module]: fail to add new extra module %s, becuase its mass %f is invalid", module_name.c_str(), inertia.getMass()); + return false; + } + + KDL::Segment extra_module(parent_link_name, KDL::Joint(KDL::Joint::None), transform, inertia); + extra_module_map_.insert(std::make_pair(module_name, extra_module)); + ROS_INFO("[extra module]: succeed to add new extra module %s", module_name.c_str()); + if (fixed_model_) { + // update robot model instantly + updateRobotModel(); + } + + return true; + } + else + { + ROS_WARN("[extra module]: fail to add new extra module %s, becuase it already exists", module_name.c_str()); + return false; + } + } bool RobotModel::removeExtraModule(std::string module_name) { @@ -378,17 +303,350 @@ namespace aerial_robot_model { { extra_module_map_.erase(module_name); ROS_INFO("[extra module]: succeed to remove the extra module %s", module_name.c_str()); + + if (fixed_model_) { + // update robot model instantly + updateRobotModel(); + } + return true; } } - void RobotModel::resolveLinkLength() + void RobotModel::updateRobotModel() + { + KDL::JntArray dummy_joint_positions(tree_.getNrOfJoints()); + KDL::SetToZero(dummy_joint_positions); + updateRobotModelImpl(dummy_joint_positions); + } + + void RobotModel::updateRobotModel(const KDL::JntArray& joint_positions) + { + updateRobotModelImpl(joint_positions); + } + + void RobotModel::updateRobotModel(const sensor_msgs::JointState& state) + { + updateRobotModel(jointMsgToKdl(state)); + } + + void RobotModel::updateRobotModelImpl(const KDL::JntArray& joint_positions) + { + joint_positions_ = joint_positions; + + KDL::RigidBodyInertia link_inertia = KDL::RigidBodyInertia::Zero(); + const auto seg_tf_map = fullForwardKinematics(joint_positions); + setSegmentsTf(seg_tf_map); + + for(const auto& inertia : inertia_map_) + { + KDL::Frame f = seg_tf_map.at(inertia.first); + link_inertia = link_inertia + f * inertia.second; + + /* process for the extra module */ + for(const auto& extra : extra_module_map_) + { + if(extra.second.getName() == inertia.first) + { + link_inertia = link_inertia + f * (extra.second.getFrameToTip() * extra.second.getInertia()); + } + } + } + + /* CoG */ + KDL::Frame f_baselink = seg_tf_map.at(baselink_); + KDL::Frame cog; + cog.M = f_baselink.M * cog_desire_orientation_.Inverse(); + cog.p = link_inertia.getCOG(); + setCog(cog); + mass_ = link_inertia.getMass(); + + setInertia((cog.Inverse() * link_inertia).getRotationalInertia()); + setCog2Baselink(cog.Inverse() * f_baselink); + + /* thrust point based on COG */ + std::vector rotors_origin_from_cog, rotors_normal_from_cog; + for(int i = 0; i < rotor_num_; ++i) + { + std::string rotor = thrust_link_ + std::to_string(i + 1); + KDL::Frame f = seg_tf_map.at(rotor); + if(verbose_) ROS_WARN(" %s : [%f, %f, %f]", rotor.c_str(), f.p.x(), f.p.y(), f.p.z()); + rotors_origin_from_cog.push_back((cog.Inverse() * f).p); + rotors_normal_from_cog.push_back((cog.Inverse() * f).M * KDL::Vector(0, 0, 1)); + } + setRotorsNormalFromCog(rotors_normal_from_cog); + setRotorsOriginFromCog(rotors_origin_from_cog); + + /* statics */ + calcStaticThrust(); + calcFeasibleControlFDists(); + calcFeasibleControlTDists(); + + if (!initialized_) initialized_ = true; + } + + KDL::Frame RobotModel::forwardKinematicsImpl(std::string link, const KDL::JntArray& joint_positions) const + { + if (joint_positions.rows() != tree_.getNrOfJoints()) + throw std::runtime_error("joint num is invalid"); + + KDL::TreeFkSolverPos_recursive fk_solver(tree_); + KDL::Frame f; + int status = fk_solver.JntToCart(joint_positions, f, link); + if(status < 0) ROS_ERROR("can not solve FK to link: %s", link.c_str()); + + return f; + } + + std::map RobotModel::fullForwardKinematicsImpl(const KDL::JntArray& joint_positions) + { + if (joint_positions.rows() != tree_.getNrOfJoints()) + throw std::runtime_error("joint num is invalid"); + + std::map seg_tf_map; + std::function recursiveFullFk = [&recursiveFullFk, &seg_tf_map, &joint_positions](const KDL::TreeElement& tree_element, const KDL::Frame& parrent_f) + { + for (const auto& elem: GetTreeElementChildren(tree_element)) + { + const KDL::TreeElement& curr_element = elem->second; + KDL::Frame curr_f = parrent_f * GetTreeElementSegment(curr_element).pose(joint_positions(GetTreeElementQNr(curr_element))); + seg_tf_map.insert(std::make_pair(GetTreeElementSegment(curr_element).getName(), curr_f)); + recursiveFullFk(curr_element, curr_f); + } + }; + + recursiveFullFk(tree_.getRootSegment()->second, KDL::Frame::Identity()); + + return seg_tf_map; + } + + Eigen::VectorXd RobotModel::calcGravityWrenchOnRoot() + { + const auto seg_frames = getSegmentsTf(); + const auto& inertia_map = getInertiaMap(); + + Eigen::MatrixXd root_rot = aerial_robot_model::kdlToEigen(getCogDesireOrientation() * seg_frames.at(baselink_).M.Inverse()); + Eigen::VectorXd wrench_g = Eigen::VectorXd::Zero(6); + for(const auto& inertia : inertia_map) + { + Eigen::MatrixXd jacobi_root = Eigen::MatrixXd::Identity(3, 6); + Eigen::Vector3d p = root_rot * aerial_robot_model::kdlToEigen(seg_frames.at(inertia.first).p + seg_frames.at(inertia.first).M * inertia.second.getCOG()); + jacobi_root.rightCols(3) = - aerial_robot_model::skew(p); + wrench_g += jacobi_root.transpose() * inertia.second.getMass() * (-gravity_3d_); + } + return wrench_g; + } + + Eigen::MatrixXd RobotModel::calcWrenchMatrixOnCoG() + { + const std::vector p = getRotorsOriginFromCog(); + const std::vector u = getRotorsNormalFromCog(); + const auto& sigma = getRotorDirection(); + const int rotor_num = getRotorNum(); + const double m_f_rate = getMFRate(); + + //Q : WrenchAllocationMatrix + Eigen::MatrixXd Q(6, rotor_num); + for (unsigned int i = 0; i < rotor_num; ++i) { + Q.block(0, i, 3, 1) = u.at(i); + Q.block(3, i, 3, 1) = p.at(i).cross(u.at(i)) + m_f_rate * sigma.at(i + 1) * u.at(i); + } + return Q; + } + + void RobotModel::calcWrenchMatrixOnRoot() + { + const auto seg_frames = getSegmentsTf(); + const std::vector& u = getRotorsNormalFromCog(); + const auto& sigma = getRotorDirection(); + const int rotor_num = getRotorNum(); + const double m_f_rate = getMFRate(); + + Eigen::MatrixXd root_rot = aerial_robot_model::kdlToEigen(getCogDesireOrientation() * seg_frames.at(baselink_).M.Inverse()); + + q_mat_ = Eigen::MatrixXd::Zero(6, rotor_num); + for (unsigned int i = 0; i < rotor_num; ++i) { + std::string rotor = "thrust" + std::to_string(i + 1); + Eigen::MatrixXd q_i = Eigen::MatrixXd::Identity(6, 6); + Eigen::Vector3d p = root_rot * aerial_robot_model::kdlToEigen(seg_frames.at(rotor).p); + q_i.bottomLeftCorner(3,3) = aerial_robot_model::skew(p); + + Eigen::VectorXd wrench_unit = Eigen::VectorXd::Zero(6); + wrench_unit.head(3) = u.at(i); + wrench_unit.tail(3) = m_f_rate * sigma.at(i + 1) * u.at(i); + + thrust_wrench_units_.at(i) = wrench_unit; + thrust_wrench_allocations_.at(i) = q_i; + q_mat_.col(i) = q_i * wrench_unit; + } + } + + void RobotModel::calcStaticThrust() + { + calcWrenchMatrixOnRoot(); // update Q matrix + Eigen::VectorXd wrench_g = calcGravityWrenchOnRoot(); + static_thrust_ = aerial_robot_model::pseudoinverse(q_mat_) * (-wrench_g); + } + + + bool RobotModel::stabilityCheck(bool verbose) + { + if(fc_f_min_ < fc_f_min_thre_) + { + if(verbose) + ROS_ERROR_STREAM("the min distance to the plane of feasible control force convex " << fc_f_min_ << " is lower than the threshold " << fc_f_min_thre_); + return false; + } + + if(fc_t_min_ < fc_t_min_thre_) + { + if(verbose) + ROS_ERROR_STREAM("the min distance to the plane of feasible control torque convex " << fc_t_min_ << " is lower than the threshold " << fc_t_min_thre_); + return false; + } + + if(static_thrust_.maxCoeff() > thrust_max_ || static_thrust_.minCoeff() < thrust_min_) + { + if(verbose) + ROS_ERROR("Invalid static thrust, max: %f, min: %f", static_thrust_.maxCoeff(), static_thrust_.minCoeff()); + return false; + } + + return true; + } + + double RobotModel::calcTripleProduct(const Eigen::Vector3d& ui, const Eigen::Vector3d& uj, const Eigen::Vector3d& uk) + { + Eigen::Vector3d uixuj = ui.cross(uj); + if (uixuj.norm() < 0.00001) { + return 0.0; + } + return uixuj.dot(uk) / uixuj.norm(); + } + + std::vector RobotModel::calcV() + { + const std::vector p = getRotorsOriginFromCog(); + const std::vector u = getRotorsNormalFromCog(); + const auto& sigma = getRotorDirection(); + const int rotor_num = getRotorNum(); + const double m_f_rate = getMFRate(); + std::vector v(rotor_num); + + for (int i = 0; i < rotor_num; ++i) + v.at(i) = p.at(i).cross(u.at(i)) + m_f_rate * sigma.at(i + 1) * u.at(i); + return v; + } + + void RobotModel::calcFeasibleControlFDists() + { + const int rotor_num = getRotorNum(); + const double thrust_max = getThrustUpperLimit(); + + const auto& u = getRotorsNormalFromCog(); + Eigen::Vector3d gravity_force = getMass() * gravity_3d_; + + int index = 0; + for (int i = 0; i < rotor_num; ++i) { + const Eigen::Vector3d& u_i = u.at(i); + for (int j = 0; j < rotor_num; ++j) { + if (i == j) continue; + const Eigen::Vector3d& u_j = u.at(j); + + double dist_ij = 0.0; + for (int k = 0; k < rotor_num; ++k) { + if (i == k || j == k) continue; + const Eigen::Vector3d& u_k = u.at(k); + double u_triple_product = calcTripleProduct(u_i, u_j, u_k); + dist_ij += std::max(0.0, u_triple_product * thrust_max); + } + + Eigen::Vector3d uixuj = u_i.cross(u_j); + fc_f_dists_(index) = fabs(dist_ij - (uixuj.dot(gravity_force) / uixuj.norm())); + index++; + } + } + fc_f_min_ = fc_f_dists_.minCoeff(); + } + + void RobotModel::calcFeasibleControlTDists() + { + const int rotor_num = getRotorNum(); + const double thrust_max = getThrustUpperLimit(); + + const auto v = calcV(); + int index = 0; + + for (int i = 0; i < rotor_num; ++i) { + const Eigen::Vector3d& v_i = v.at(i); + for (int j = 0; j < rotor_num; ++j) { + if (i == j) continue; + const Eigen::Vector3d& v_j = v.at(j); + double dist_ij = 0.0; + for (int k = 0; k < rotor_num; ++k) { + if (i == k || j == k) continue; + const Eigen::Vector3d& v_k = v.at(k); + double v_triple_product = calcTripleProduct(v_i, v_j, v_k); + dist_ij += std::max(0.0, v_triple_product * thrust_max); + } + fc_t_dists_(index) = dist_ij; + index++; + } + } + + fc_t_min_ = fc_t_dists_.minCoeff(); + } + + TiXmlDocument RobotModel::getRobotModelXml(const std::string param, ros::NodeHandle nh) + { + std::string xml_string; + TiXmlDocument xml_doc; + + if (!nh.hasParam(param)) + { + ROS_ERROR("Could not find parameter %s on parameter server with namespace '%s'", param.c_str(), nh.getNamespace().c_str()); + return xml_doc; + } + + nh.getParam(param, xml_string); + xml_doc.Parse(xml_string.c_str()); + + return xml_doc; + } + + KDL::JntArray RobotModel::jointMsgToKdl(const sensor_msgs::JointState& state) const { KDL::JntArray joint_positions(tree_.getNrOfJoints()); - //hard coding - KDL::Frame f_link2 = forwardKinematics("link2", joint_positions); - KDL::Frame f_link3 = forwardKinematics("link3", joint_positions); - link_length_ = (f_link3.p - f_link2.p).Norm(); + for(unsigned int i = 0; i < state.position.size(); ++i) + { + auto itr = joint_index_map_.find(state.name[i]); + if(itr != joint_index_map_.end()) joint_positions(itr->second) = state.position[i]; + } + return joint_positions; + } + + sensor_msgs::JointState RobotModel::kdlJointToMsg(const KDL::JntArray& joint_positions) const + { + sensor_msgs::JointState state; + state.name.reserve(joint_index_map_.size()); + state.position.reserve(joint_index_map_.size()); + for(const auto& actuator : joint_index_map_) + { + state.name.push_back(actuator.first); + state.position.push_back(joint_positions(actuator.second)); + } + return state; + } + + KDL::JntArray RobotModel::convertEigenToKDL(const Eigen::VectorXd& joint_vector) + { + const auto& joint_indices = getJointIndices(); + KDL::JntArray joint_positions(getTree().getNrOfJoints()); + for (unsigned int i = 0; i < joint_indices.size(); ++i) { + joint_positions(joint_indices.at(i)) = joint_vector(i); + } + return joint_positions; } } //namespace aerial_robot_model + diff --git a/aerial_robot_model/src/transformable_aerial_robot_model/robot_model_ros.cpp b/aerial_robot_model/src/model/base_model/robot_model_ros.cpp similarity index 82% rename from aerial_robot_model/src/transformable_aerial_robot_model/robot_model_ros.cpp rename to aerial_robot_model/src/model/base_model/robot_model_ros.cpp index 434f2ebe6..77de238e2 100644 --- a/aerial_robot_model/src/transformable_aerial_robot_model/robot_model_ros.cpp +++ b/aerial_robot_model/src/model/base_model/robot_model_ros.cpp @@ -1,4 +1,4 @@ -#include +#include namespace aerial_robot_model { RobotModelRos::RobotModelRos(ros::NodeHandle nh, ros::NodeHandle nhp): @@ -6,12 +6,6 @@ namespace aerial_robot_model { nhp_(nhp), robot_model_loader_("aerial_robot_model", "aerial_robot_model::RobotModel") { - // subscriber - desire_coordinate_sub_ = nh_.subscribe("desire_coordinate", 1, &RobotModelRos::desireCoordinateCallback, this); - joint_state_sub_ = nh_.subscribe("joint_states", 1, &RobotModelRos::jointStateCallback, this); - // service server - add_extra_module_service_ = nh_.advertiseService("add_extra_module", &RobotModelRos::addExtraModuleCallback, this); - // rosparam nhp_.param("tf_prefix", tf_prefix_, std::string("")); @@ -33,6 +27,22 @@ namespace aerial_robot_model { ROS_ERROR("can not find plugin rosparameter for robot model, use default class: aerial_robot_model::RobotModel"); robot_model_ = boost::make_shared(); } + + if (robot_model_->isModelFixed()) { + // broadcast static tf between root and CoG + // refering: https://github.com/ros/robot_state_publisher/blob/rolling/src/robot_state_publisher.cpp#L130 + geometry_msgs::TransformStamped tf = robot_model_->getCog(); + tf.header.stamp = ros::Time::now(); + tf.header.frame_id = tf::resolve(tf_prefix_, robot_model_->getRootFrameName()); + tf.child_frame_id = tf::resolve(tf_prefix_, std::string("cog")); + static_br_.sendTransform(tf); + } + else { + joint_state_sub_ = nh_.subscribe("joint_states", 1, &RobotModelRos::jointStateCallback, this); + } + + desire_coordinate_sub_ = nh_.subscribe("desire_coordinate", 1, &RobotModelRos::desireCoordinateCallback, this); + add_extra_module_service_ = nh_.advertiseService("add_extra_module", &RobotModelRos::addExtraModuleCallback, this); } void RobotModelRos::jointStateCallback(const sensor_msgs::JointStateConstPtr& state) @@ -40,8 +50,6 @@ namespace aerial_robot_model { joint_state_ = *state; robot_model_->updateRobotModel(*state); - ROS_INFO_ONCE("initialized robot model, the mass is %f", robot_model_->getMass()); - geometry_msgs::TransformStamped tf = robot_model_->getCog(); tf.header = state->header; tf.header.frame_id = tf::resolve(tf_prefix_, robot_model_->getRootFrameName()); diff --git a/aerial_robot_model/src/model/plugin/multirotor_robot_model.cpp b/aerial_robot_model/src/model/plugin/multirotor_robot_model.cpp new file mode 100644 index 000000000..a12f7dbc5 --- /dev/null +++ b/aerial_robot_model/src/model/plugin/multirotor_robot_model.cpp @@ -0,0 +1,45 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +MultirotorRobotModel::MultirotorRobotModel(bool init_with_rosparam, bool verbose, double fc_t_min_thre, double epsilon): + RobotModel(init_with_rosparam, verbose, 0, fc_t_min_thre, epsilon) +{ +} + +/* plugin registration */ +#include +PLUGINLIB_EXPORT_CLASS(MultirotorRobotModel, aerial_robot_model::RobotModel); diff --git a/aerial_robot_model/src/model/plugin/tilted_underactuated_robot_model.cpp b/aerial_robot_model/src/model/plugin/tilted_underactuated_robot_model.cpp new file mode 100644 index 000000000..990392250 --- /dev/null +++ b/aerial_robot_model/src/model/plugin/tilted_underactuated_robot_model.cpp @@ -0,0 +1,71 @@ + // -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +UnderactuatedTiltedRobotModel::UnderactuatedTiltedRobotModel(bool init_with_rosparam, bool verbose, double fc_t_min_thre, double epsilon): + RobotModel(init_with_rosparam, verbose, fc_t_min_thre, 0, epsilon) +{ + + // calc static thrust + calcWrenchMatrixOnRoot(); // update Q matrix + + /* calculate the static thrust on CoG frame */ + /* note: can not calculate in root frame, since the projected f_x, f_y is different in CoG and root */ + Eigen::MatrixXd wrench_mat_on_cog = calcWrenchMatrixOnCoG(); + Eigen::VectorXd static_thrust = aerial_robot_model::pseudoinverse(wrench_mat_on_cog.middleRows(2, 4)) * getGravity().segment(2,4) * getMass(); + + // update robot model + /* special process to find the hovering axis for tilt model */ + Eigen::MatrixXd cog_rot_inv = aerial_robot_model::kdlToEigen(getCogDesireOrientation().Inverse()); + Eigen::VectorXd f = cog_rot_inv * wrench_mat_on_cog.topRows(3) * static_thrust; + + double f_norm_roll = atan2(f(1), f(2)); + double f_norm_pitch = atan2(-f(0), sqrt(f(1)*f(1) + f(2)*f(2))); + + /* set the hoverable frame as CoG and reupdate model */ + setCogDesireOrientation(f_norm_roll, f_norm_pitch, 0); + updateRobotModel(); + + if(getVerbose()) + { + ROS_INFO_STREAM("f_norm_pitch: " << f_norm_pitch << "; f_norm_roll: " << f_norm_roll); + ROS_INFO_STREAM("rescaled static thrust: " << getStaticThrust().transpose()); + } +} + +/* plugin registration */ +#include +PLUGINLIB_EXPORT_CLASS(UnderactuatedTiltedRobotModel, aerial_robot_model::RobotModel); diff --git a/aerial_robot_model/src/model/transformable_model/jacobians.cpp b/aerial_robot_model/src/model/transformable_model/jacobians.cpp new file mode 100644 index 000000000..dc8def598 --- /dev/null +++ b/aerial_robot_model/src/model/transformable_model/jacobians.cpp @@ -0,0 +1,189 @@ +#include + +using namespace aerial_robot_model::transformable; + +Eigen::MatrixXd RobotModel::getJacobian(const KDL::JntArray& joint_positions, std::string segment_name, KDL::Vector offset) +{ + const auto& tree = getTree(); + const auto seg_frames = getSegmentsTf(); + + Eigen::MatrixXd root_rot = aerial_robot_model::kdlToEigen(getCogDesireOrientation() * seg_frames.at(getBaselinkName()).M.Inverse()); + + KDL::TreeJntToJacSolver solver(tree); + KDL::Jacobian jac(tree.getNrOfJoints()); + int status = solver.JntToJac(joint_positions, jac, segment_name); + jac.changeRefPoint(seg_frames.at(segment_name).M * offset); + + // joint part + Eigen::MatrixXd jac_joint = convertJacobian(jac.data); + // return jac_joint; // only joint jacobian + + // add virtual 6dof root + Eigen::MatrixXd jac_all = Eigen::MatrixXd::Identity(6, 6 + getJointNum()); + jac_all.rightCols(getJointNum()) = jac_joint; + Eigen::Vector3d p = aerial_robot_model::kdlToEigen(seg_frames.at(segment_name).p + seg_frames.at(segment_name).M * offset); + jac_all.block(0,3,3,3) = -aerial_robot_model::skew(p); + + jac_all.topRows(3) = root_rot * jac_all.topRows(3); + jac_all.bottomRows(3) = root_rot * jac_all.bottomRows(3); + return jac_all; + +} + +Eigen::MatrixXd RobotModel::convertJacobian(const Eigen::MatrixXd& in) +{ + const auto& joint_indices = getJointIndices(); + Eigen::MatrixXd out(6, getJointNum()); + + int col_index = 0; + for (const auto& joint_index : joint_indices) { // fix bug of order + out.col(col_index) = in.col(joint_index); + col_index++; + } + return out; +} + + +// @deprecated +Eigen::VectorXd RobotModel::getHessian(std::string ref_frame, int joint_i, int joint_j, KDL::Vector offset) +{ + const auto& segment_map = getTree().getSegments(); + const auto seg_frames = getSegmentsTf(); + const auto& joint_hierachy = getJointHierachy(); + const auto& joint_segment_map = getJointSegmentMap(); + const auto& joint_names = getJointNames(); + const auto& joint_parent_link_names = getJointParentLinkNames(); + + Eigen::MatrixXd root_rot = aerial_robot_model::kdlToEigen(getCogDesireOrientation() * seg_frames.at(getBaselinkName()).M.Inverse()); + + Eigen::Vector3d p_e = aerial_robot_model::kdlToEigen(seg_frames.at(ref_frame).p + seg_frames.at(ref_frame).M * offset); + + std::string joint_i_name = joint_names.at(joint_i); + std::string joint_j_name = joint_names.at(joint_j); + + if (joint_hierachy.at(joint_i_name) > joint_hierachy.at(joint_j_name)) { + std::swap(joint_i_name, joint_j_name); + } + + std::vector joint_i_child_segments = joint_segment_map.at(joint_i_name); + std::vector joint_j_child_segments = joint_segment_map.at(joint_j_name); + + if (std::find(joint_i_child_segments.begin(), joint_i_child_segments.end(), ref_frame) == joint_i_child_segments.end() || std::find(joint_j_child_segments.begin(), joint_j_child_segments.end(), ref_frame) == joint_j_child_segments.end()) { + return Eigen::VectorXd::Zero(6); + } + + std::string joint_i_child_segment_name = joint_i_child_segments.at(0); + std::string joint_j_child_segment_name = joint_j_child_segments.at(0); + const KDL::Segment& joint_i_child_segment = GetTreeElementSegment(segment_map.at(joint_i_child_segment_name)); + const KDL::Segment& joint_j_child_segment = GetTreeElementSegment(segment_map.at(joint_j_child_segment_name)); + Eigen::Vector3d a_i = aerial_robot_model::kdlToEigen(seg_frames.at(joint_parent_link_names.at(joint_i)).M * joint_i_child_segment.getJoint().JointAxis()); + Eigen::Vector3d a_j = aerial_robot_model::kdlToEigen(seg_frames.at(joint_parent_link_names.at(joint_j)).M * joint_j_child_segment.getJoint().JointAxis()); + Eigen::Vector3d p_j = aerial_robot_model::kdlToEigen(seg_frames.at(joint_j_child_segment_name).p); //joint pos + + Eigen::VectorXd derivative(6); + derivative.topRows(3) = root_rot * a_i.cross(a_j.cross(p_e - p_j)); + derivative.bottomRows(3) = root_rot * a_i.cross(a_j); + + return derivative; +} + +Eigen::MatrixXd RobotModel::getSecondDerivative(std::string ref_frame, int joint_i, KDL::Vector offset) +{ + const auto& segment_map = getTree().getSegments(); + const auto seg_frames = getSegmentsTf(); + const auto& joint_hierachy = getJointHierachy(); + const auto& joint_segment_map = getJointSegmentMap(); + const auto& joint_names = getJointNames(); + const auto& joint_parent_link_names = getJointParentLinkNames(); + const auto& joint_num = getJointNum(); + const int full_body_ndof = 6 + joint_num; + Eigen::MatrixXd root_rot = aerial_robot_model::kdlToEigen(getCogDesireOrientation() * seg_frames.at(getBaselinkName()).M.Inverse()); + + Eigen::Vector3d p_e = aerial_robot_model::kdlToEigen(seg_frames.at(ref_frame).p + seg_frames.at(ref_frame).M * offset); + + std::string joint_i_name = joint_names.at(joint_i); + std::vector joint_i_child_segments = joint_segment_map.at(joint_i_name); + std::string joint_i_child_segment_name = joint_i_child_segments.at(0); + const KDL::Segment& joint_i_child_segment = GetTreeElementSegment(segment_map.at(joint_i_child_segment_name)); + Eigen::Vector3d a_i = aerial_robot_model::kdlToEigen(seg_frames.at(joint_parent_link_names.at(joint_i)).M * joint_i_child_segment.getJoint().JointAxis()); + Eigen::Vector3d p_i = aerial_robot_model::kdlToEigen(seg_frames.at(joint_i_child_segment_name).p); + + Eigen::MatrixXd jacobian_i = Eigen::MatrixXd::Zero(6, full_body_ndof); + + if (std::find(joint_i_child_segments.begin(), joint_i_child_segments.end(), ref_frame) == joint_i_child_segments.end()) { + return jacobian_i; + } + + // joint part + for(int j = 0; j < joint_num; j++) + { + std::string joint_j_name = joint_names.at(j); + std::vector joint_j_child_segments = joint_segment_map.at(joint_j_name); + if (std::find(joint_j_child_segments.begin(), joint_j_child_segments.end(), ref_frame) == joint_j_child_segments.end()) { + continue; + } + + std::string joint_j_child_segment_name = joint_j_child_segments.at(0); + const KDL::Segment& joint_j_child_segment = GetTreeElementSegment(segment_map.at(joint_j_child_segment_name)); + Eigen::Vector3d a_j = aerial_robot_model::kdlToEigen(seg_frames.at(joint_parent_link_names.at(j)).M * joint_j_child_segment.getJoint().JointAxis()); + Eigen::Vector3d p_j = aerial_robot_model::kdlToEigen(seg_frames.at(joint_j_child_segment_name).p); + + Eigen::Vector3d a_i_j(0,0,0); + Eigen::Vector3d p_i_j(0,0,0); + Eigen::Vector3d p_e_j = a_j.cross(p_e - p_j); + if ( joint_hierachy.at(joint_j_name) <= joint_hierachy.at(joint_i_name)){ + // joint_j is close to root, or i = j + + // both i and j is revolute jont + a_i_j = a_j.cross(a_i); + p_i_j = a_j.cross(p_i - p_j); + } + + jacobian_i.block(0, 6 + j, 3, 1) = root_rot * a_i_j.cross(p_e - p_i) + root_rot * a_i.cross(p_e_j - p_i_j); // force + jacobian_i.block(3, 6 + j, 3, 1) = root_rot * a_i_j; // torque + } + + // virtual 6dof root + jacobian_i.block(0, 3, 3, 3) = - root_rot * aerial_robot_model::skew(a_i.cross(p_e - p_i)); + jacobian_i.block(3, 3, 3, 3) = - root_rot * aerial_robot_model::skew(a_i); + return jacobian_i; +} + +Eigen::MatrixXd RobotModel::getSecondDerivativeRoot(std::string ref_frame, KDL::Vector offset) +{ + const int joint_num = getJointNum(); + const int full_body_ndof = 6 + joint_num; + const auto& joint_parent_link_names = getJointParentLinkNames(); + const auto& segment_map = getTree().getSegments(); + const auto seg_frames = getSegmentsTf(); + Eigen::MatrixXd root_rot = aerial_robot_model::kdlToEigen(getCogDesireOrientation() * seg_frames.at(getBaselinkName()).M.Inverse()); + + Eigen::Vector3d p_e = aerial_robot_model::kdlToEigen(seg_frames.at(ref_frame).p + seg_frames.at(ref_frame).M * offset); + Eigen::MatrixXd out = Eigen::MatrixXd::Zero(3, full_body_ndof); + + // joint part + for (int i = 0; i < joint_num; ++i) { + + std::string joint_name = getJointNames().at(i); + std::vector joint_child_segments = getJointSegmentMap().at(joint_name); + + if (std::find(joint_child_segments.begin(), joint_child_segments.end(), ref_frame) == joint_child_segments.end()) + { + continue; + } + + std::string joint_child_segment_name = joint_child_segments.at(0); + const KDL::Segment& joint_child_segment = GetTreeElementSegment(segment_map.at(joint_child_segment_name)); + Eigen::Vector3d a = aerial_robot_model::kdlToEigen(seg_frames.at(joint_parent_link_names.at(i)).M * joint_child_segment.getJoint().JointAxis()); // fix bug + Eigen::Vector3d p = aerial_robot_model::kdlToEigen(seg_frames.at(joint_child_segment_name).p); + + out.col(6 + i) = root_rot * a.cross(p_e - p); + } + + // virtual root 6dof + out.leftCols(3) = root_rot; + out.middleCols(3,3) = - root_rot * aerial_robot_model::skew(p_e); + + return out; +} + diff --git a/aerial_robot_model/src/model/transformable_model/kinematics.cpp b/aerial_robot_model/src/model/transformable_model/kinematics.cpp new file mode 100644 index 000000000..3bec15e73 --- /dev/null +++ b/aerial_robot_model/src/model/transformable_model/kinematics.cpp @@ -0,0 +1,95 @@ +#include + +using namespace aerial_robot_model::transformable; + +void RobotModel::resolveLinkLength() +{ + KDL::JntArray joint_positions(getTree().getNrOfJoints()); + //hard coding + KDL::Frame f_link2 = forwardKinematics("link2", joint_positions); + KDL::Frame f_link3 = forwardKinematics("link3", joint_positions); + link_length_ = (f_link3.p - f_link2.p).Norm(); +} + + +void RobotModel::calcBasicKinematicsJacobian() +{ + const std::vector p = getRotorsOriginFromCog(); + const std::vector u = getRotorsNormalFromCog(); + const auto& joint_positions = getJointPositions(); + const auto& sigma = getRotorDirection(); + const int rotor_num = getRotorNum(); + const double m_f_rate = getMFRate(); + + //calc jacobian of u(thrust direction, force vector), p(thrust position) + for (int i = 0; i < rotor_num; ++i) { + std::string seg_name = std::string("thrust") + std::to_string(i + 1); + Eigen::MatrixXd thrust_coord_jacobian = RobotModel::getJacobian(joint_positions, seg_name); + thrust_coord_jacobians_.at(i) = thrust_coord_jacobian; + u_jacobians_.at(i) = -skew(u.at(i)) * thrust_coord_jacobian.bottomRows(3); + p_jacobians_.at(i) = thrust_coord_jacobian.topRows(3) - cog_jacobian_; + } +} + +void RobotModel::calcCoGMomentumJacobian() +{ + double mass_all = getMass(); + const auto cog_all = getCog().p; + const auto& segment_map = getTree().getSegments(); + const auto seg_frames = getSegmentsTf(); + const auto& inertia_map = getInertiaMap(); + const auto& joint_names = getJointNames(); + const auto& joint_segment_map = getJointSegmentMap(); + const auto& joint_parent_link_names = getJointParentLinkNames(); + const auto joint_num = getJointNum(); + + Eigen::MatrixXd root_rot = aerial_robot_model::kdlToEigen(getCogDesireOrientation() * seg_frames.at(getBaselinkName()).M.Inverse()); + /* + Note: the jacobian about the cog velocity (linear momentum) and angular momentum. + + Please refer to Eq.13 ~ 22 of following paper: + ============================================================================ + S. Kajita et al., "Resolved momentum control: humanoid motion planning based on the linear and angular momentum,". + ============================================================================ + + 1. the cog velocity is w.r.t in the root link frame. + 2. the angular momentum is w.r.t in cog frame + */ + + + int col_index = 0; + /* fix bug: the joint_segment_map is reordered, which is not match the order of joint_indeices_ or joint_names_ */ + // joint part + for (const auto& joint_name : joint_names){ + std::string joint_child_segment_name = joint_segment_map.at(joint_name).at(0); + KDL::Segment joint_child_segment = GetTreeElementSegment(segment_map.at(joint_child_segment_name)); + KDL::Vector a = seg_frames.at(joint_parent_link_names.at(col_index)).M * joint_child_segment.getJoint().JointAxis(); + + KDL::Vector r = seg_frames.at(joint_child_segment_name).p; + KDL::RigidBodyInertia inertia = KDL::RigidBodyInertia::Zero(); + for (const auto& seg : joint_segment_map.at(joint_name)) { + if (seg.find("thrust") == std::string::npos) { + KDL::Frame f = seg_frames.at(seg); + inertia = inertia + f * inertia_map.at(seg); + } + } + KDL::Vector c = inertia.getCOG(); + double m = inertia.getMass(); + + KDL::Vector p_momentum_jacobian_col = a * (c - r) * m; + KDL::Vector l_momentum_jacobian_col = (c - cog_all) * p_momentum_jacobian_col + inertia.RefPoint(c).getRotationalInertia() * a; + + cog_jacobian_.col(6 + col_index) = aerial_robot_model::kdlToEigen(p_momentum_jacobian_col / mass_all); + l_momentum_jacobian_.col(6 + col_index) = aerial_robot_model::kdlToEigen(l_momentum_jacobian_col); + col_index++; + } + + // virtual 6dof root + cog_jacobian_.leftCols(3) = Eigen::MatrixXd::Identity(3, 3); + cog_jacobian_.middleCols(3, 3) = - aerial_robot_model::skew(aerial_robot_model::kdlToEigen(cog_all)); + cog_jacobian_ = root_rot * cog_jacobian_; + + l_momentum_jacobian_.leftCols(3) = Eigen::MatrixXd::Zero(3, 3); + l_momentum_jacobian_.middleCols(3, 3) = getInertia() * root_rot; // aready converted + l_momentum_jacobian_.rightCols(joint_num) = root_rot * l_momentum_jacobian_.rightCols(joint_num); +} diff --git a/aerial_robot_model/src/model/transformable_model/robot_model.cpp b/aerial_robot_model/src/model/transformable_model/robot_model.cpp new file mode 100644 index 000000000..dea570791 --- /dev/null +++ b/aerial_robot_model/src/model/transformable_model/robot_model.cpp @@ -0,0 +1,74 @@ +#include + +using namespace aerial_robot_model::transformable; + +RobotModel::RobotModel(bool init_with_rosparam, bool verbose, double fc_f_min_thre, double fc_t_min_thre, double epsilon): + aerial_robot_model::RobotModel(init_with_rosparam, verbose, false /* non-fixed model */, fc_f_min_thre, fc_t_min_thre, epsilon) +{ + std::function recursiveSegmentCheck = [&recursiveSegmentCheck, this](const KDL::TreeElement& tree_element) + { + const KDL::Segment current_seg = GetTreeElementSegment(tree_element); + + if (current_seg.getJoint().getType() != KDL::Joint::None && + current_seg.getJoint().getName().find("joint") == 0) { + link_joint_names_.push_back(current_seg.getJoint().getName()); + link_joint_indices_.push_back(tree_element.q_nr); + } + + // recursive process + for (const auto& elem: GetTreeElementChildren(tree_element)) { + recursiveSegmentCheck(elem->second); + } + + }; + recursiveSegmentCheck(getTree().getRootSegment()->second); + + + for(auto itr : link_joint_names_) { + auto joint_ptr = getUrdfModel().getJoint(itr); + link_joint_lower_limits_.push_back(joint_ptr->limits->lower); + link_joint_upper_limits_.push_back(joint_ptr->limits->upper); + } + + resolveLinkLength(); + + // jacobian + const int rotor_num = getRotorNum(); + const int joint_num = getJointNum(); + const int full_body_dof = 6 + joint_num; + u_jacobians_.resize(rotor_num); + p_jacobians_.resize(rotor_num); + thrust_coord_jacobians_.resize(rotor_num); + cog_coord_jacobians_.resize(getInertiaMap().size()); + cog_jacobian_.resize(3, full_body_dof); + l_momentum_jacobian_.resize(3, full_body_dof); + approx_fc_f_dists_.resize(rotor_num * (rotor_num - 1)); + approx_fc_t_dists_.resize(rotor_num * (rotor_num - 1)); + fc_f_dists_jacobian_.resize(rotor_num * (rotor_num - 1), full_body_dof); + fc_t_dists_jacobian_.resize(rotor_num * (rotor_num - 1), full_body_dof); + lambda_jacobian_.resize(rotor_num, full_body_dof); + joint_torque_.resize(joint_num); + joint_torque_jacobian_.resize(joint_num, full_body_dof); +} + +void RobotModel::updateJacobians() +{ + updateJacobians(getJointPositions(), false); +} + +void RobotModel::updateJacobians(const KDL::JntArray& joint_positions, bool update_model) +{ + if(update_model) updateRobotModel(joint_positions); + + calcCoGMomentumJacobian(); // should be processed first + + calcBasicKinematicsJacobian(); // need cog_jacobian_ + + calcLambdaJacobian(); + + calcJointTorque(false); + + calcJointTorqueJacobian(); + + calcFeasibleControlJacobian(); +} diff --git a/aerial_robot_model/src/model/transformable_model/stability.cpp b/aerial_robot_model/src/model/transformable_model/stability.cpp new file mode 100644 index 000000000..d34c1c9a8 --- /dev/null +++ b/aerial_robot_model/src/model/transformable_model/stability.cpp @@ -0,0 +1,93 @@ +#include + +using namespace aerial_robot_model::transformable; + +void RobotModel::calcFeasibleControlJacobian() +{ + // reference: + // https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8967725 + + const int rotor_num = getRotorNum(); + const int joint_num = getJointNum(); + const int ndof = 6 + joint_num; + const auto& p = getRotorsOriginFromCog(); + const auto& u = getRotorsNormalFromCog(); + const auto& sigma = getRotorDirection(); + const double thrust_max = getThrustUpperLimit(); + Eigen::Vector3d fg = getMass() * getGravity3d(); + const double m_f_rate = getMFRate(); + const double epsilon = getEpsilon(); + + const auto v = calcV(); + std::vector v_jacobians; + for (int i = 0; i < rotor_num; ++i) { + v_jacobians.push_back(-skew(u.at(i)) * p_jacobians_.at(i) + skew(p.at(i)) * u_jacobians_.at(i) + m_f_rate * sigma.at(i + 1) * u_jacobians_.at(i)); + } + + //calc jacobian of f_min_ij, t_min_ij + int index = 0; + for (int i = 0; i < rotor_num; ++i) { + for (int j = 0; j < rotor_num; ++j) { + double approx_f_dist = 0.0; + double approx_t_dist = 0.0; + Eigen::MatrixXd d_f_min = Eigen::MatrixXd::Zero(1, ndof); + Eigen::MatrixXd d_t_min = Eigen::MatrixXd::Zero(1, ndof); + const Eigen::Vector3d& u_i = u.at(i); + const Eigen::Vector3d& u_j = u.at(j); + const Eigen::Vector3d uixuj = u_i.cross(u_j); + const Eigen::MatrixXd& d_u_i = u_jacobians_.at(i); + const Eigen::MatrixXd& d_u_j = u_jacobians_.at(j); + const Eigen::MatrixXd d_uixuj = -skew(u_j) * d_u_i + skew(u_i) * d_u_j; + + const Eigen::Vector3d& v_i = v.at(i); + const Eigen::Vector3d& v_j = v.at(j); + const Eigen::Vector3d vixvj = v_i.cross(v_j); + const Eigen::MatrixXd& d_v_i = v_jacobians.at(i); + const Eigen::MatrixXd& d_v_j = v_jacobians.at(j); + const Eigen::MatrixXd d_vixvj = -skew(v_j) * d_v_i + skew(v_i) * d_v_j; + + for (int k = 0; k < rotor_num; ++k) { + if (i == j || j == k || k == i) { + } else { + + // u + const Eigen::Vector3d& u_k = u.at(k); + const double u_triple_product = calcTripleProduct(u_i, u_j, u_k); + const Eigen::MatrixXd& d_u_k = u_jacobians_.at(k); + Eigen::MatrixXd d_u_triple_product = (uixuj / uixuj.norm()).transpose() * d_u_k + u_k.transpose() * (d_uixuj / uixuj.norm() - uixuj / (uixuj.norm() * uixuj.squaredNorm()) * uixuj.transpose() * d_uixuj); + d_f_min += sigmoid(u_triple_product * thrust_max, epsilon) * d_u_triple_product * thrust_max; + approx_f_dist += reluApprox(u_triple_product * thrust_max, epsilon); + + // v + const Eigen::Vector3d& v_k = v.at(k); + const double v_triple_product = calcTripleProduct(v_i, v_j, v_k); + const Eigen::MatrixXd& d_v_k = v_jacobians.at(k); + Eigen::MatrixXd d_v_triple_product = (vixvj / vixvj.norm()).transpose() * d_v_k + v_k.transpose() * (d_vixvj / vixvj.norm() - vixvj / (vixvj.norm() * vixvj.squaredNorm()) * vixvj.transpose() * d_vixvj); + d_t_min += sigmoid(v_triple_product * thrust_max, epsilon) * d_v_triple_product * thrust_max; + approx_t_dist += reluApprox(v_triple_product * thrust_max, epsilon); + } //if + } //k + + if (i != j) { + double uixuj_fg = uixuj.dot(fg)/uixuj.norm(); + Eigen::MatrixXd d_uixuj_fg = Eigen::MatrixXd::Zero(1, ndof); + d_uixuj_fg = fg.transpose() * (d_uixuj / uixuj.norm() - uixuj / (uixuj.norm() * uixuj.squaredNorm()) * uixuj.transpose() * d_uixuj); + + approx_fc_f_dists_(index) = absApprox(approx_f_dist - uixuj_fg, epsilon); + fc_f_dists_jacobian_.row(index) = tanh(approx_f_dist - uixuj_fg, epsilon) * (d_f_min - d_uixuj_fg); + + approx_fc_t_dists_(index) = approx_t_dist; + fc_t_dists_jacobian_.row(index) = d_t_min; + + for(int l = 0; l < ndof; l++) + { + if(std::isnan(fc_f_dists_jacobian_.row(index)(0, l))) + fc_f_dists_jacobian_.row(index)(0, l) = 0; + if(std::isnan(fc_t_dists_jacobian_.row(index)(0, l))) + fc_t_dists_jacobian_.row(index)(0, l) = 0; + } + index++; + } + } //j + } //i +} diff --git a/aerial_robot_model/src/model/transformable_model/statics.cpp b/aerial_robot_model/src/model/transformable_model/statics.cpp new file mode 100644 index 000000000..de81752e7 --- /dev/null +++ b/aerial_robot_model/src/model/transformable_model/statics.cpp @@ -0,0 +1,148 @@ +#include + +using namespace aerial_robot_model::transformable; + +void RobotModel::calcJointTorque(const bool update_jacobian) +{ + const auto& sigma = getRotorDirection(); + const auto& joint_positions = getJointPositions(); + const auto& inertia_map = getInertiaMap(); + const int joint_num = getJointNum(); + const int rotor_num = getRotorNum(); + const double m_f_rate = getMFRate(); + const auto gravity = getGravity(); + const auto& static_thrust = getStaticThrust(); + const auto& thrust_wrench_units = getThrustWrenchUnits(); + + if(update_jacobian) + calcBasicKinematicsJacobian(); // update thrust_coord_jacobians_ + + joint_torque_ = Eigen::VectorXd::Zero(joint_num); + + // update coord jacobians for cog point and convert to joint torque + int seg_index = 0; + for(const auto& inertia : inertia_map) + { + cog_coord_jacobians_.at(seg_index) = RobotModel::getJacobian(joint_positions, inertia.first, inertia.second.getCOG()); + joint_torque_ -= cog_coord_jacobians_.at(seg_index).rightCols(joint_num).transpose() * inertia.second.getMass() * (-gravity); + seg_index ++; + } + + // thrust + for (int i = 0; i < rotor_num; ++i) { + Eigen::VectorXd wrench = thrust_wrench_units.at(i) * static_thrust(i); + joint_torque_ -= thrust_coord_jacobians_.at(i).rightCols(joint_num).transpose() * wrench; + } +} + +void RobotModel::calcLambdaJacobian() +{ + // w.r.t root + const auto& inertia_map = getInertiaMap(); + const auto& rotor_direction = getRotorDirection(); + const auto& joint_positions = getJointPositions(); + const int rotor_num = getRotorNum(); + const int joint_num = getJointNum(); + const int ndof = thrust_coord_jacobians_.at(0).cols(); + const double m_f_rate = getMFRate(); + const auto& q_mat = getThrustWrenchMatrix(); + const int wrench_dof = q_mat.rows(); // default: 6, under-actuated: 4 + Eigen::MatrixXd q_pseudo_inv = aerial_robot_model::pseudoinverse(q_mat); + const auto gravity_3d = getGravity3d(); + const auto& static_thrust = getStaticThrust(); + const auto& thrust_wrench_units = getThrustWrenchUnits(); + const auto& thrust_wrench_allocations = getThrustWrenchAllocations(); + + /* derivative for gravity jacobian */ + Eigen::MatrixXd wrench_gravity_jacobian = Eigen::MatrixXd::Zero(6, ndof); + for(const auto& inertia : inertia_map){ + wrench_gravity_jacobian.bottomRows(3) -= aerial_robot_model::skew(-inertia.second.getMass() * gravity_3d) * getSecondDerivativeRoot(inertia.first, inertia.second.getCOG()); + } + + ROS_DEBUG_STREAM("wrench_gravity_jacobian w.r.t. root : \n" << wrench_gravity_jacobian); + + if(wrench_dof == 6) // fully-actuated + lambda_jacobian_ = -q_pseudo_inv * wrench_gravity_jacobian; // trans, rot + else // under-actuated + lambda_jacobian_ = -q_pseudo_inv * (wrench_gravity_jacobian).middleRows(2, wrench_dof); // z, rot + + /* derivative for thrust jacobian */ + std::vector q_mat_jacobians; + Eigen::MatrixXd q_inv_jacobian = Eigen::MatrixXd::Zero(6, ndof); + for (int i = 0; i < rotor_num; ++i) { + std::string thrust_name = std::string("thrust") + std::to_string(i + 1); + Eigen::MatrixXd q_mat_jacobian = Eigen::MatrixXd::Zero(6, ndof); + + q_mat_jacobian.bottomRows(3) -= aerial_robot_model::skew(thrust_wrench_units.at(i).head(3)) * getSecondDerivativeRoot(thrust_name); + + Eigen::MatrixXd wrench_unit_jacobian = Eigen::MatrixXd::Zero(6, ndof); + wrench_unit_jacobian.topRows(3) = -skew(thrust_wrench_units.at(i).head(3)) * thrust_coord_jacobians_.at(i).bottomRows(3); + wrench_unit_jacobian.bottomRows(3) = -skew(thrust_wrench_units.at(i).tail(3)) * thrust_coord_jacobians_.at(i).bottomRows(3); + q_mat_jacobian += (thrust_wrench_allocations.at(i) * wrench_unit_jacobian); + + q_inv_jacobian += (q_mat_jacobian * static_thrust(i)); + q_mat_jacobians.push_back(q_mat_jacobian); + } + + if(wrench_dof == 6) // fully-actuated + lambda_jacobian_ += -q_pseudo_inv * q_inv_jacobian; // trans, rot + else // under-actuated + lambda_jacobian_ += -q_pseudo_inv * (q_inv_jacobian).middleRows(2, wrench_dof); // z, rot + + // https://mathoverflow.net/questions/25778/analytical-formula-for-numerical-derivative-of-the-matrix-pseudo-inverse, the third tmer + Eigen::MatrixXd q_pseudo_inv_jacobian = Eigen::MatrixXd::Zero(rotor_num, ndof); + Eigen::VectorXd pseudo_wrench = q_pseudo_inv.transpose() * static_thrust; + for(int i = 0; i < rotor_num; i++) + { + if(wrench_dof == 6) // fully-actuated + q_pseudo_inv_jacobian.row(i) = pseudo_wrench.transpose() * q_mat_jacobians.at(i); + else // under-actuated + q_pseudo_inv_jacobian.row(i) = pseudo_wrench.transpose() * q_mat_jacobians.at(i).middleRows(2, wrench_dof); + } + lambda_jacobian_ += (Eigen::MatrixXd::Identity(rotor_num, rotor_num) - q_pseudo_inv * q_mat) * q_pseudo_inv_jacobian; + + ROS_DEBUG_STREAM("lambda_jacobian: \n" << lambda_jacobian_); +} + +void RobotModel::calcJointTorqueJacobian() +{ + const auto& sigma = getRotorDirection(); + const auto& inertia_map = getInertiaMap(); + const double m_f_rate = getMFRate(); + const int rotor_num = getRotorNum(); + const int joint_num = getJointNum(); + const int ndof = lambda_jacobian_.cols(); + const auto gravity = getGravity(); + const auto& static_thrust = getStaticThrust(); + const auto& thrust_wrench_units = getThrustWrenchUnits(); + + joint_torque_jacobian_ = Eigen::MatrixXd::Zero(joint_num, ndof); + + // gravity + for(const auto& inertia : inertia_map) + { + for (int j = 0; j < joint_num; ++j) { + joint_torque_jacobian_.row(j) += inertia.second.getMass() * (-gravity.transpose()) * getSecondDerivative(inertia.first, j, inertia.second.getCOG()); + } + } + + // thrust + for (int i = 0; i < rotor_num; ++i) { + Eigen::VectorXd wrench = thrust_wrench_units.at(i) * static_thrust(i); + std::string thrust_name = std::string("thrust") + std::to_string(i + 1); + + for (int j = 0; j < joint_num; ++j) { + joint_torque_jacobian_.row(j) += wrench.transpose() * getSecondDerivative(thrust_name, j); + } + + Eigen::MatrixXd wrench_unit_jacobian = Eigen::MatrixXd::Zero(6, ndof); + wrench_unit_jacobian.topRows(3) = -skew(thrust_wrench_units.at(i).head(3)) * thrust_coord_jacobians_.at(i).bottomRows(3); + wrench_unit_jacobian.bottomRows(3) = -skew(thrust_wrench_units.at(i).tail(3)) * thrust_coord_jacobians_.at(i).bottomRows(3); + + joint_torque_jacobian_ += thrust_coord_jacobians_.at(i).rightCols(joint_num).transpose() * (wrench_unit_jacobian * static_thrust(i) + thrust_wrench_units.at(i) * lambda_jacobian_.row(i)); + } + joint_torque_jacobian_ *= -1; +} + + + diff --git a/aerial_robot_model/src/transformable_aerial_robot_model/jacobians.cpp b/aerial_robot_model/src/transformable_aerial_robot_model/jacobians.cpp deleted file mode 100644 index 0e91749c5..000000000 --- a/aerial_robot_model/src/transformable_aerial_robot_model/jacobians.cpp +++ /dev/null @@ -1,188 +0,0 @@ -#include - -namespace aerial_robot_model { - - Eigen::MatrixXd RobotModel::getJacobian(const KDL::JntArray& joint_positions, std::string segment_name, KDL::Vector offset) - { - const auto& tree = getTree(); - const auto seg_frames = getSegmentsTf(); - Eigen::MatrixXd root_rot = aerial_robot_model::kdlToEigen(getCogDesireOrientation() * seg_frames.at(baselink_).M.Inverse()); - - KDL::TreeJntToJacSolver solver(tree); - KDL::Jacobian jac(tree.getNrOfJoints()); - int status = solver.JntToJac(joint_positions, jac, segment_name); - jac.changeRefPoint(seg_frames.at(segment_name).M * offset); - - // joint part - Eigen::MatrixXd jac_joint = convertJacobian(jac.data); - // return jac_joint; // only joint jacobian - - // add virtual 6dof root - Eigen::MatrixXd jac_all = Eigen::MatrixXd::Identity(6, 6 + getJointNum()); - jac_all.rightCols(getJointNum()) = jac_joint; - Eigen::Vector3d p = aerial_robot_model::kdlToEigen(seg_frames.at(segment_name).p + seg_frames.at(segment_name).M * offset); - jac_all.block(0,3,3,3) = -aerial_robot_model::skew(p); - - jac_all.topRows(3) = root_rot * jac_all.topRows(3); - jac_all.bottomRows(3) = root_rot * jac_all.bottomRows(3); - return jac_all; - - } - - Eigen::MatrixXd RobotModel::convertJacobian(const Eigen::MatrixXd& in) - { - const auto& joint_indices = getJointIndices(); - Eigen::MatrixXd out(6, getJointNum()); - - int col_index = 0; - for (const auto& joint_index : joint_indices) { // fix bug of order - out.col(col_index) = in.col(joint_index); - col_index++; - } - return out; - } - - - // @deprecated - Eigen::VectorXd RobotModel::getHessian(std::string ref_frame, int joint_i, int joint_j, KDL::Vector offset) - { - const auto& segment_map = getTree().getSegments(); - const auto seg_frames = getSegmentsTf(); - const auto& joint_hierachy = getJointHierachy(); - const auto& joint_segment_map = getJointSegmentMap(); - const auto& joint_names = getJointNames(); - const auto& joint_parent_link_names = getJointParentLinkNames(); - - Eigen::MatrixXd root_rot = aerial_robot_model::kdlToEigen(getCogDesireOrientation() * seg_frames.at(baselink_).M.Inverse()); - - Eigen::Vector3d p_e = aerial_robot_model::kdlToEigen(seg_frames.at(ref_frame).p + seg_frames.at(ref_frame).M * offset); - - std::string joint_i_name = joint_names.at(joint_i); - std::string joint_j_name = joint_names.at(joint_j); - - if (joint_hierachy.at(joint_i_name) > joint_hierachy.at(joint_j_name)) { - std::swap(joint_i_name, joint_j_name); - } - - std::vector joint_i_child_segments = joint_segment_map.at(joint_i_name); - std::vector joint_j_child_segments = joint_segment_map.at(joint_j_name); - - if (std::find(joint_i_child_segments.begin(), joint_i_child_segments.end(), ref_frame) == joint_i_child_segments.end() || std::find(joint_j_child_segments.begin(), joint_j_child_segments.end(), ref_frame) == joint_j_child_segments.end()) { - return Eigen::VectorXd::Zero(6); - } - - std::string joint_i_child_segment_name = joint_i_child_segments.at(0); - std::string joint_j_child_segment_name = joint_j_child_segments.at(0); - const KDL::Segment& joint_i_child_segment = GetTreeElementSegment(segment_map.at(joint_i_child_segment_name)); - const KDL::Segment& joint_j_child_segment = GetTreeElementSegment(segment_map.at(joint_j_child_segment_name)); - Eigen::Vector3d a_i = aerial_robot_model::kdlToEigen(seg_frames.at(joint_parent_link_names.at(joint_i)).M * joint_i_child_segment.getJoint().JointAxis()); - Eigen::Vector3d a_j = aerial_robot_model::kdlToEigen(seg_frames.at(joint_parent_link_names.at(joint_j)).M * joint_j_child_segment.getJoint().JointAxis()); - Eigen::Vector3d p_j = aerial_robot_model::kdlToEigen(seg_frames.at(joint_j_child_segment_name).p); //joint pos - - Eigen::VectorXd derivative(6); - derivative.topRows(3) = root_rot * a_i.cross(a_j.cross(p_e - p_j)); - derivative.bottomRows(3) = root_rot * a_i.cross(a_j); - - return derivative; - } - - Eigen::MatrixXd RobotModel::getSecondDerivative(std::string ref_frame, int joint_i, KDL::Vector offset) - { - const auto& segment_map = getTree().getSegments(); - const auto seg_frames = getSegmentsTf(); - const auto& joint_hierachy = getJointHierachy(); - const auto& joint_segment_map = getJointSegmentMap(); - const auto& joint_names = getJointNames(); - const auto& joint_parent_link_names = getJointParentLinkNames(); - const auto& joint_num = getJointNum(); - const int full_body_ndof = 6 + joint_num; - Eigen::MatrixXd root_rot = aerial_robot_model::kdlToEigen(getCogDesireOrientation() * seg_frames.at(baselink_).M.Inverse()); - - Eigen::Vector3d p_e = aerial_robot_model::kdlToEigen(seg_frames.at(ref_frame).p + seg_frames.at(ref_frame).M * offset); - - std::string joint_i_name = joint_names.at(joint_i); - std::vector joint_i_child_segments = joint_segment_map.at(joint_i_name); - std::string joint_i_child_segment_name = joint_i_child_segments.at(0); - const KDL::Segment& joint_i_child_segment = GetTreeElementSegment(segment_map.at(joint_i_child_segment_name)); - Eigen::Vector3d a_i = aerial_robot_model::kdlToEigen(seg_frames.at(joint_parent_link_names.at(joint_i)).M * joint_i_child_segment.getJoint().JointAxis()); - Eigen::Vector3d p_i = aerial_robot_model::kdlToEigen(seg_frames.at(joint_i_child_segment_name).p); - - Eigen::MatrixXd jacobian_i = Eigen::MatrixXd::Zero(6, full_body_ndof); - - if (std::find(joint_i_child_segments.begin(), joint_i_child_segments.end(), ref_frame) == joint_i_child_segments.end()) { - return jacobian_i; - } - - // joint part - for(int j = 0; j < joint_num; j++) - { - std::string joint_j_name = joint_names.at(j); - std::vector joint_j_child_segments = joint_segment_map.at(joint_j_name); - if (std::find(joint_j_child_segments.begin(), joint_j_child_segments.end(), ref_frame) == joint_j_child_segments.end()) { - continue; - } - - std::string joint_j_child_segment_name = joint_j_child_segments.at(0); - const KDL::Segment& joint_j_child_segment = GetTreeElementSegment(segment_map.at(joint_j_child_segment_name)); - Eigen::Vector3d a_j = aerial_robot_model::kdlToEigen(seg_frames.at(joint_parent_link_names.at(j)).M * joint_j_child_segment.getJoint().JointAxis()); - Eigen::Vector3d p_j = aerial_robot_model::kdlToEigen(seg_frames.at(joint_j_child_segment_name).p); - - Eigen::Vector3d a_i_j(0,0,0); - Eigen::Vector3d p_i_j(0,0,0); - Eigen::Vector3d p_e_j = a_j.cross(p_e - p_j); - if ( joint_hierachy.at(joint_j_name) <= joint_hierachy.at(joint_i_name)){ - // joint_j is close to root, or i = j - - // both i and j is revolute jont - a_i_j = a_j.cross(a_i); - p_i_j = a_j.cross(p_i - p_j); - } - - jacobian_i.block(0, 6 + j, 3, 1) = root_rot * a_i_j.cross(p_e - p_i) + root_rot * a_i.cross(p_e_j - p_i_j); // force - jacobian_i.block(3, 6 + j, 3, 1) = root_rot * a_i_j; // torque - } - - // virtual 6dof root - jacobian_i.block(0, 3, 3, 3) = - root_rot * aerial_robot_model::skew(a_i.cross(p_e - p_i)); - jacobian_i.block(3, 3, 3, 3) = - root_rot * aerial_robot_model::skew(a_i); - return jacobian_i; - } - - Eigen::MatrixXd RobotModel::getSecondDerivativeRoot(std::string ref_frame, KDL::Vector offset) - { - const int joint_num = getJointNum(); - const int full_body_ndof = 6 + joint_num; - const auto& joint_parent_link_names = getJointParentLinkNames(); - const auto& segment_map = getTree().getSegments(); - const auto seg_frames = getSegmentsTf(); - Eigen::MatrixXd root_rot = aerial_robot_model::kdlToEigen(getCogDesireOrientation() * seg_frames.at(baselink_).M.Inverse()); - - Eigen::Vector3d p_e = aerial_robot_model::kdlToEigen(seg_frames.at(ref_frame).p + seg_frames.at(ref_frame).M * offset); - Eigen::MatrixXd out = Eigen::MatrixXd::Zero(3, full_body_ndof); - - // joint part - for (int i = 0; i < joint_num; ++i) { - - std::string joint_name = getJointNames().at(i); - std::vector joint_child_segments = getJointSegmentMap().at(joint_name); - - if (std::find(joint_child_segments.begin(), joint_child_segments.end(), ref_frame) == joint_child_segments.end()) - { - continue; - } - - std::string joint_child_segment_name = joint_child_segments.at(0); - const KDL::Segment& joint_child_segment = GetTreeElementSegment(segment_map.at(joint_child_segment_name)); - Eigen::Vector3d a = aerial_robot_model::kdlToEigen(seg_frames.at(joint_parent_link_names.at(i)).M * joint_child_segment.getJoint().JointAxis()); // fix bug - Eigen::Vector3d p = aerial_robot_model::kdlToEigen(seg_frames.at(joint_child_segment_name).p); - - out.col(6 + i) = root_rot * a.cross(p_e - p); - } - - // virtual root 6dof - out.leftCols(3) = root_rot; - out.middleCols(3,3) = - root_rot * aerial_robot_model::skew(p_e); - - return out; - } -} //namespace aerial_robot_model diff --git a/aerial_robot_model/src/transformable_aerial_robot_model/robot_model.cpp b/aerial_robot_model/src/transformable_aerial_robot_model/robot_model.cpp deleted file mode 100644 index e8cf4a18f..000000000 --- a/aerial_robot_model/src/transformable_aerial_robot_model/robot_model.cpp +++ /dev/null @@ -1,136 +0,0 @@ -#include - -namespace aerial_robot_model { - - RobotModel::RobotModel(bool init_with_rosparam, bool verbose, double fc_f_min_thre, double fc_t_min_thre, double epsilon): - verbose_(verbose), - fc_f_min_thre_(fc_f_min_thre), - fc_t_min_thre_(fc_t_min_thre), - epsilon_(epsilon), - baselink_("fc"), - thrust_link_("thrust"), - rotor_num_(0), - joint_num_(0), - thrust_max_(0), - thrust_min_(0), - mass_(0) - { - if (init_with_rosparam) - getParamFromRos(); - - kinematicsInit(); - stabilityInit(); - staticsInit(); - } - - void RobotModel::getParamFromRos() - { - ros::NodeHandle nh; - nh.param("kinematic_verbose", verbose_, false); - nh.param("fc_f_min_thre", fc_f_min_thre_, 0.0); - nh.param("fc_t_min_thre", fc_t_min_thre_, 0.0); - nh.param("epsilon", epsilon_, 10.0); - } - - KDL::JntArray RobotModel::jointMsgToKdl(const sensor_msgs::JointState& state) const - { - KDL::JntArray joint_positions(tree_.getNrOfJoints()); - for(unsigned int i = 0; i < state.position.size(); ++i) - { - auto itr = joint_index_map_.find(state.name[i]); - if(itr != joint_index_map_.end()) joint_positions(itr->second) = state.position[i]; - } - return joint_positions; - } - - sensor_msgs::JointState RobotModel::kdlJointToMsg(const KDL::JntArray& joint_positions) const - { - sensor_msgs::JointState state; - state.name.reserve(joint_index_map_.size()); - state.position.reserve(joint_index_map_.size()); - for(const auto& actuator : joint_index_map_) - { - state.name.push_back(actuator.first); - state.position.push_back(joint_positions(actuator.second)); - } - return state; - } - - void RobotModel::updateJacobians() - { - updateJacobians(getJointPositions(), false); - } - - void RobotModel::updateJacobians(const KDL::JntArray& joint_positions, bool update_model) - { - if(update_model) updateRobotModel(joint_positions); - - calcCoGMomentumJacobian(); // should be processed first - - calcBasicKinematicsJacobian(); // need cog_jacobian_ - - calcLambdaJacobian(); - - calcJointTorque(false); - - calcJointTorqueJacobian(); - - calcFeasibleControlJacobian(); - } - - - void RobotModel::updateRobotModelImpl(const KDL::JntArray& joint_positions) - { - joint_positions_ = joint_positions; - - KDL::RigidBodyInertia link_inertia = KDL::RigidBodyInertia::Zero(); - const auto seg_tf_map = fullForwardKinematics(joint_positions); - setSegmentsTf(seg_tf_map); - - for(const auto& inertia : inertia_map_) - { - KDL::Frame f = seg_tf_map.at(inertia.first); - link_inertia = link_inertia + f * inertia.second; - - /* process for the extra module */ - for(const auto& extra : extra_module_map_) - { - if(extra.second.getName() == inertia.first) - { - link_inertia = link_inertia + f * (extra.second.getFrameToTip() * extra.second.getInertia()); - } - } - } - - /* CoG */ - KDL::Frame f_baselink = seg_tf_map.at(baselink_); - KDL::Frame cog; - cog.M = f_baselink.M * cog_desire_orientation_.Inverse(); - cog.p = link_inertia.getCOG(); - setCog(cog); - mass_ = link_inertia.getMass(); - - setInertia((cog.Inverse() * link_inertia).getRotationalInertia()); - setCog2Baselink(cog.Inverse() * f_baselink); - - /* thrust point based on COG */ - std::vector rotors_origin_from_cog, rotors_normal_from_cog; - for(int i = 0; i < rotor_num_; ++i) - { - std::string rotor = thrust_link_ + std::to_string(i + 1); - KDL::Frame f = seg_tf_map.at(rotor); - if(verbose_) ROS_WARN(" %s : [%f, %f, %f]", rotor.c_str(), f.p.x(), f.p.y(), f.p.z()); - rotors_origin_from_cog.push_back((cog.Inverse() * f).p); - rotors_normal_from_cog.push_back((cog.Inverse() * f).M * KDL::Vector(0, 0, 1)); - } - setRotorsNormalFromCog(rotors_normal_from_cog); - setRotorsOriginFromCog(rotors_origin_from_cog); - - /* statics */ - calcStaticThrust(); - calcFeasibleControlFDists(); - calcFeasibleControlTDists(); - } - -} //namespace aerial_robot_model - diff --git a/aerial_robot_model/src/transformable_aerial_robot_model/stability.cpp b/aerial_robot_model/src/transformable_aerial_robot_model/stability.cpp deleted file mode 100644 index b2f9f00d3..000000000 --- a/aerial_robot_model/src/transformable_aerial_robot_model/stability.cpp +++ /dev/null @@ -1,218 +0,0 @@ -#include - -namespace aerial_robot_model { - - - bool RobotModel::stabilityCheck(bool verbose) - { - if(fc_f_min_ < fc_f_min_thre_) - { - if(verbose) - ROS_ERROR_STREAM("the min distance to the plane of feasible control force convex " << fc_f_min_ << " is lower than the threshold " << fc_f_min_thre_); - return false; - } - - if(fc_t_min_ < fc_t_min_thre_) - { - if(verbose) - ROS_ERROR_STREAM("the min distance to the plane of feasible control torque convex " << fc_t_min_ << " is lower than the threshold " << fc_t_min_thre_); - return false; - } - - if(static_thrust_.maxCoeff() > thrust_max_ || static_thrust_.minCoeff() < thrust_min_) - { - if(verbose) - ROS_ERROR("Invalid static thrust, max: %f, min: %f", static_thrust_.maxCoeff(), static_thrust_.minCoeff()); - return false; - } - - return true; - } - - - double RobotModel::calcTripleProduct(const Eigen::Vector3d& ui, const Eigen::Vector3d& uj, const Eigen::Vector3d& uk) - { - Eigen::Vector3d uixuj = ui.cross(uj); - if (uixuj.norm() < 0.00001) { - return 0.0; - } - return uixuj.dot(uk) / uixuj.norm(); - } - - std::vector RobotModel::calcV() - { - const std::vector p = getRotorsOriginFromCog(); - const std::vector u = getRotorsNormalFromCog(); - const auto& sigma = getRotorDirection(); - const int rotor_num = getRotorNum(); - const double m_f_rate = getMFRate(); - std::vector v(rotor_num); - - for (int i = 0; i < rotor_num; ++i) - v.at(i) = p.at(i).cross(u.at(i)) + m_f_rate * sigma.at(i + 1) * u.at(i); - return v; - } - - void RobotModel::calcFeasibleControlFDists() - { - const int rotor_num = getRotorNum(); - const double thrust_max = getThrustUpperLimit(); - - const auto& u = getRotorsNormalFromCog(); - Eigen::Vector3d gravity_force = getMass() * gravity_3d_; - - int index = 0; - for (int i = 0; i < rotor_num; ++i) { - const Eigen::Vector3d& u_i = u.at(i); - for (int j = 0; j < rotor_num; ++j) { - if (i == j) continue; - const Eigen::Vector3d& u_j = u.at(j); - - double dist_ij = 0.0; - for (int k = 0; k < rotor_num; ++k) { - if (i == k || j == k) continue; - const Eigen::Vector3d& u_k = u.at(k); - double u_triple_product = calcTripleProduct(u_i, u_j, u_k); - dist_ij += std::max(0.0, u_triple_product * thrust_max); - } - - Eigen::Vector3d uixuj = u_i.cross(u_j); - fc_f_dists_(index) = fabs(dist_ij - (uixuj.dot(gravity_force) / uixuj.norm())); - index++; - } - } - fc_f_min_ = fc_f_dists_.minCoeff(); - - } - - void RobotModel::calcFeasibleControlTDists() - { - const int rotor_num = getRotorNum(); - const double thrust_max = getThrustUpperLimit(); - - const auto v = calcV(); - int index = 0; - - for (int i = 0; i < rotor_num; ++i) { - const Eigen::Vector3d& v_i = v.at(i); - for (int j = 0; j < rotor_num; ++j) { - if (i == j) continue; - const Eigen::Vector3d& v_j = v.at(j); - double dist_ij = 0.0; - for (int k = 0; k < rotor_num; ++k) { - if (i == k || j == k) continue; - const Eigen::Vector3d& v_k = v.at(k); - double v_triple_product = calcTripleProduct(v_i, v_j, v_k); - dist_ij += std::max(0.0, v_triple_product * thrust_max); - } - fc_t_dists_(index) = dist_ij; - index++; - } - } - - fc_t_min_ = fc_t_dists_.minCoeff(); - } - - void RobotModel::calcFeasibleControlJacobian() - { - // reference: - // https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8967725 - - const int rotor_num = getRotorNum(); - const int joint_num = getJointNum(); - const int ndof = 6 + joint_num; - const auto& p = getRotorsOriginFromCog(); - const auto& u = getRotorsNormalFromCog(); - const auto& sigma = getRotorDirection(); - const double thrust_max = getThrustUpperLimit(); - Eigen::Vector3d fg = getMass() * gravity_3d_; - const double m_f_rate = getMFRate(); - - const auto v = calcV(); - std::vector v_jacobians; - for (int i = 0; i < rotor_num; ++i) { - v_jacobians.push_back(-skew(u.at(i)) * p_jacobians_.at(i) + skew(p.at(i)) * u_jacobians_.at(i) + m_f_rate * sigma.at(i + 1) * u_jacobians_.at(i)); - } - - //calc jacobian of f_min_ij, t_min_ij - int index = 0; - for (int i = 0; i < rotor_num; ++i) { - for (int j = 0; j < rotor_num; ++j) { - double approx_f_dist = 0.0; - double approx_t_dist = 0.0; - Eigen::MatrixXd d_f_min = Eigen::MatrixXd::Zero(1, ndof); - Eigen::MatrixXd d_t_min = Eigen::MatrixXd::Zero(1, ndof); - const Eigen::Vector3d& u_i = u.at(i); - const Eigen::Vector3d& u_j = u.at(j); - const Eigen::Vector3d uixuj = u_i.cross(u_j); - const Eigen::MatrixXd& d_u_i = u_jacobians_.at(i); - const Eigen::MatrixXd& d_u_j = u_jacobians_.at(j); - const Eigen::MatrixXd d_uixuj = -skew(u_j) * d_u_i + skew(u_i) * d_u_j; - - const Eigen::Vector3d& v_i = v.at(i); - const Eigen::Vector3d& v_j = v.at(j); - const Eigen::Vector3d vixvj = v_i.cross(v_j); - const Eigen::MatrixXd& d_v_i = v_jacobians.at(i); - const Eigen::MatrixXd& d_v_j = v_jacobians.at(j); - const Eigen::MatrixXd d_vixvj = -skew(v_j) * d_v_i + skew(v_i) * d_v_j; - - for (int k = 0; k < rotor_num; ++k) { - if (i == j || j == k || k == i) { - } else { - - // u - const Eigen::Vector3d& u_k = u.at(k); - const double u_triple_product = calcTripleProduct(u_i, u_j, u_k); - const Eigen::MatrixXd& d_u_k = u_jacobians_.at(k); - Eigen::MatrixXd d_u_triple_product = (uixuj / uixuj.norm()).transpose() * d_u_k + u_k.transpose() * (d_uixuj / uixuj.norm() - uixuj / (uixuj.norm() * uixuj.squaredNorm()) * uixuj.transpose() * d_uixuj); - d_f_min += sigmoid(u_triple_product * thrust_max, epsilon_) * d_u_triple_product * thrust_max; - approx_f_dist += reluApprox(u_triple_product * thrust_max, epsilon_); - - // v - const Eigen::Vector3d& v_k = v.at(k); - const double v_triple_product = calcTripleProduct(v_i, v_j, v_k); - const Eigen::MatrixXd& d_v_k = v_jacobians.at(k); - Eigen::MatrixXd d_v_triple_product = (vixvj / vixvj.norm()).transpose() * d_v_k + v_k.transpose() * (d_vixvj / vixvj.norm() - vixvj / (vixvj.norm() * vixvj.squaredNorm()) * vixvj.transpose() * d_vixvj); - d_t_min += sigmoid(v_triple_product * thrust_max, epsilon_) * d_v_triple_product * thrust_max; - approx_t_dist += reluApprox(v_triple_product * thrust_max, epsilon_); - } //if - } //k - - if (i != j) { - double uixuj_fg = uixuj.dot(fg)/uixuj.norm(); - Eigen::MatrixXd d_uixuj_fg = Eigen::MatrixXd::Zero(1, ndof); - d_uixuj_fg = fg.transpose() * (d_uixuj / uixuj.norm() - uixuj / (uixuj.norm() * uixuj.squaredNorm()) * uixuj.transpose() * d_uixuj); - - approx_fc_f_dists_(index) = absApprox(approx_f_dist - uixuj_fg, epsilon_); - fc_f_dists_jacobian_.row(index) = tanh(approx_f_dist - uixuj_fg, epsilon_) * (d_f_min - d_uixuj_fg); - - approx_fc_t_dists_(index) = approx_t_dist; - fc_t_dists_jacobian_.row(index) = d_t_min; - - for(int l = 0; l < ndof; l++) - { - if(std::isnan(fc_f_dists_jacobian_.row(index)(0, l))) - fc_f_dists_jacobian_.row(index)(0, l) = 0; - if(std::isnan(fc_t_dists_jacobian_.row(index)(0, l))) - fc_t_dists_jacobian_.row(index)(0, l) = 0; - } - index++; - } - } //j - } //i - } - - - void RobotModel::stabilityInit() - { - const int full_body_dof = 6 + joint_num_; - - approx_fc_f_dists_.resize(rotor_num_ * (rotor_num_ - 1)); - approx_fc_t_dists_.resize(rotor_num_ * (rotor_num_ - 1)); - fc_f_dists_.resize(rotor_num_ * (rotor_num_ - 1)); - fc_t_dists_.resize(rotor_num_ * (rotor_num_ - 1)); - fc_f_dists_jacobian_.resize(rotor_num_ * (rotor_num_ - 1), full_body_dof); - fc_t_dists_jacobian_.resize(rotor_num_ * (rotor_num_ - 1), full_body_dof); - } - -} //namespace aerial_robot_model diff --git a/aerial_robot_model/src/transformable_aerial_robot_model/statics.cpp b/aerial_robot_model/src/transformable_aerial_robot_model/statics.cpp deleted file mode 100644 index e535447a6..000000000 --- a/aerial_robot_model/src/transformable_aerial_robot_model/statics.cpp +++ /dev/null @@ -1,245 +0,0 @@ -#include - -namespace aerial_robot_model { - - void RobotModel::calcStaticThrust() - { - calcWrenchMatrixOnRoot(); // update Q matrix - Eigen::VectorXd wrench_g = calcGravityWrenchOnRoot(); - static_thrust_ = aerial_robot_model::pseudoinverse(q_mat_) * (-wrench_g); - } - - void RobotModel::calcJointTorque(const bool update_jacobian) - { - const auto& sigma = getRotorDirection(); - const auto& joint_positions = getJointPositions(); - const auto& inertia_map = getInertiaMap(); - const int joint_num = getJointNum(); - const int rotor_num = getRotorNum(); - const double m_f_rate = getMFRate(); - - if(update_jacobian) - calcBasicKinematicsJacobian(); // update thrust_coord_jacobians_ - - joint_torque_ = Eigen::VectorXd::Zero(joint_num); - - // update coord jacobians for cog point and convert to joint torque - int seg_index = 0; - for(const auto& inertia : inertia_map) - { - cog_coord_jacobians_.at(seg_index) = RobotModel::getJacobian(joint_positions, inertia.first, inertia.second.getCOG()); - joint_torque_ -= cog_coord_jacobians_.at(seg_index).rightCols(joint_num).transpose() * inertia.second.getMass() * (-gravity_); - seg_index ++; - } - - // thrust - for (int i = 0; i < rotor_num; ++i) { - Eigen::VectorXd wrench = thrust_wrench_units_.at(i) * static_thrust_(i); - joint_torque_ -= thrust_coord_jacobians_.at(i).rightCols(joint_num).transpose() * wrench; - } - } - - void RobotModel::calcLambdaJacobian() - { - // w.r.t root - const auto& inertia_map = getInertiaMap(); - const auto& rotor_direction = getRotorDirection(); - const auto& joint_positions = getJointPositions(); - const int rotor_num = getRotorNum(); - const int joint_num = getJointNum(); - const int ndof = thrust_coord_jacobians_.at(0).cols(); - const double m_f_rate = getMFRate(); - const int wrench_dof = q_mat_.rows(); // default: 6, under-actuated: 4 - Eigen::MatrixXd q_pseudo_inv = aerial_robot_model::pseudoinverse(q_mat_); - - /* derivative for gravity jacobian */ - Eigen::MatrixXd wrench_gravity_jacobian = Eigen::MatrixXd::Zero(6, ndof); - for(const auto& inertia : inertia_map){ - wrench_gravity_jacobian.bottomRows(3) -= aerial_robot_model::skew(-inertia.second.getMass() * gravity_3d_) * getSecondDerivativeRoot(inertia.first, inertia.second.getCOG()); - } - - ROS_DEBUG_STREAM("wrench_gravity_jacobian w.r.t. root : \n" << wrench_gravity_jacobian); - - if(wrench_dof == 6) // fully-actuated - lambda_jacobian_ = -q_pseudo_inv * wrench_gravity_jacobian; // trans, rot - else // under-actuated - lambda_jacobian_ = -q_pseudo_inv * (wrench_gravity_jacobian).middleRows(2, wrench_dof); // z, rot - - /* derivative for thrust jacobian */ - std::vector q_mat_jacobians; - Eigen::MatrixXd q_inv_jacobian = Eigen::MatrixXd::Zero(6, ndof); - for (int i = 0; i < rotor_num; ++i) { - std::string thrust_name = std::string("thrust") + std::to_string(i + 1); - Eigen::MatrixXd q_mat_jacobian = Eigen::MatrixXd::Zero(6, ndof); - - q_mat_jacobian.bottomRows(3) -= aerial_robot_model::skew(thrust_wrench_units_.at(i).head(3)) * getSecondDerivativeRoot(thrust_name); - - Eigen::MatrixXd wrench_unit_jacobian = Eigen::MatrixXd::Zero(6, ndof); - wrench_unit_jacobian.topRows(3) = -skew(thrust_wrench_units_.at(i).head(3)) * thrust_coord_jacobians_.at(i).bottomRows(3); - wrench_unit_jacobian.bottomRows(3) = -skew(thrust_wrench_units_.at(i).tail(3)) * thrust_coord_jacobians_.at(i).bottomRows(3); - q_mat_jacobian += (thrust_wrench_allocations_.at(i) * wrench_unit_jacobian); - - q_inv_jacobian += (q_mat_jacobian * static_thrust_(i)); - q_mat_jacobians.push_back(q_mat_jacobian); - } - - if(wrench_dof == 6) // fully-actuated - lambda_jacobian_ += -q_pseudo_inv * q_inv_jacobian; // trans, rot - else // under-actuated - lambda_jacobian_ += -q_pseudo_inv * (q_inv_jacobian).middleRows(2, wrench_dof); // z, rot - - // https://mathoverflow.net/questions/25778/analytical-formula-for-numerical-derivative-of-the-matrix-pseudo-inverse, the third tmer - Eigen::MatrixXd q_pseudo_inv_jacobian = Eigen::MatrixXd::Zero(rotor_num, ndof); - Eigen::VectorXd pseudo_wrench = q_pseudo_inv.transpose() * static_thrust_; - for(int i = 0; i < rotor_num; i++) - { - if(wrench_dof == 6) // fully-actuated - q_pseudo_inv_jacobian.row(i) = pseudo_wrench.transpose() * q_mat_jacobians.at(i); - else // under-actuated - q_pseudo_inv_jacobian.row(i) = pseudo_wrench.transpose() * q_mat_jacobians.at(i).middleRows(2, wrench_dof); - } - lambda_jacobian_ += (Eigen::MatrixXd::Identity(rotor_num, rotor_num) - q_pseudo_inv * q_mat_) * q_pseudo_inv_jacobian; - - ROS_DEBUG_STREAM("lambda_jacobian: \n" << lambda_jacobian_); - } - - void RobotModel::calcJointTorqueJacobian() - { - const auto& sigma = getRotorDirection(); - const auto& inertia_map = getInertiaMap(); - const double m_f_rate = getMFRate(); - const int rotor_num = getRotorNum(); - const int joint_num = getJointNum(); - const int ndof = lambda_jacobian_.cols(); - - joint_torque_jacobian_ = Eigen::MatrixXd::Zero(joint_num, ndof); - - // gravity - for(const auto& inertia : inertia_map) - { - for (int j = 0; j < joint_num; ++j) { - joint_torque_jacobian_.row(j) += inertia.second.getMass() * (-gravity_.transpose()) * getSecondDerivative(inertia.first, j, inertia.second.getCOG()); - } - } - - // thrust - for (int i = 0; i < rotor_num; ++i) { - Eigen::VectorXd wrench = thrust_wrench_units_.at(i) * static_thrust_(i); - std::string thrust_name = std::string("thrust") + std::to_string(i + 1); - - for (int j = 0; j < joint_num; ++j) { - joint_torque_jacobian_.row(j) += wrench.transpose() * getSecondDerivative(thrust_name, j); - } - - Eigen::MatrixXd wrench_unit_jacobian = Eigen::MatrixXd::Zero(6, ndof); - wrench_unit_jacobian.topRows(3) = -skew(thrust_wrench_units_.at(i).head(3)) * thrust_coord_jacobians_.at(i).bottomRows(3); - wrench_unit_jacobian.bottomRows(3) = -skew(thrust_wrench_units_.at(i).tail(3)) * thrust_coord_jacobians_.at(i).bottomRows(3); - - joint_torque_jacobian_ += thrust_coord_jacobians_.at(i).rightCols(joint_num).transpose() * (wrench_unit_jacobian * static_thrust_(i) + thrust_wrench_units_.at(i) * lambda_jacobian_.row(i)); - } - joint_torque_jacobian_ *= -1; - } - - Eigen::VectorXd RobotModel::calcGravityWrenchOnRoot() - { - const auto seg_frames = getSegmentsTf(); - const auto& inertia_map = getInertiaMap(); - - Eigen::MatrixXd root_rot = aerial_robot_model::kdlToEigen(getCogDesireOrientation() * seg_frames.at(baselink_).M.Inverse()); - Eigen::VectorXd wrench_g = Eigen::VectorXd::Zero(6); - for(const auto& inertia : inertia_map) - { - Eigen::MatrixXd jacobi_root = Eigen::MatrixXd::Identity(3, 6); - Eigen::Vector3d p = root_rot * aerial_robot_model::kdlToEigen(seg_frames.at(inertia.first).p + seg_frames.at(inertia.first).M * inertia.second.getCOG()); - jacobi_root.rightCols(3) = - aerial_robot_model::skew(p); - wrench_g += jacobi_root.transpose() * inertia.second.getMass() * (-gravity_3d_); - } - return wrench_g; - } - - Eigen::MatrixXd RobotModel::calcWrenchMatrixOnCoG() - { - const std::vector p = getRotorsOriginFromCog(); - const std::vector u = getRotorsNormalFromCog(); - const auto& sigma = getRotorDirection(); - const int rotor_num = getRotorNum(); - const double m_f_rate = getMFRate(); - - //Q : WrenchAllocationMatrix - Eigen::MatrixXd Q(6, rotor_num); - for (unsigned int i = 0; i < rotor_num; ++i) { - Q.block(0, i, 3, 1) = u.at(i); - Q.block(3, i, 3, 1) = p.at(i).cross(u.at(i)) + m_f_rate * sigma.at(i + 1) * u.at(i); - } - return Q; - } - - void RobotModel::calcWrenchMatrixOnRoot() - { - const auto seg_frames = getSegmentsTf(); - const std::vector& u = getRotorsNormalFromCog(); - const auto& sigma = getRotorDirection(); - const int rotor_num = getRotorNum(); - const double m_f_rate = getMFRate(); - - Eigen::MatrixXd root_rot = aerial_robot_model::kdlToEigen(getCogDesireOrientation() * seg_frames.at(baselink_).M.Inverse()); - - q_mat_ = Eigen::MatrixXd::Zero(6, rotor_num); - for (unsigned int i = 0; i < rotor_num; ++i) { - std::string rotor = "thrust" + std::to_string(i + 1); - Eigen::MatrixXd q_i = Eigen::MatrixXd::Identity(6, 6); - Eigen::Vector3d p = root_rot * aerial_robot_model::kdlToEigen(seg_frames.at(rotor).p); - q_i.bottomLeftCorner(3,3) = aerial_robot_model::skew(p); - - Eigen::VectorXd wrench_unit = Eigen::VectorXd::Zero(6); - wrench_unit.head(3) = u.at(i); - wrench_unit.tail(3) = m_f_rate * sigma.at(i + 1) * u.at(i); - - thrust_wrench_units_.at(i) = wrench_unit; - thrust_wrench_allocations_.at(i) = q_i; - q_mat_.col(i) = q_i * wrench_unit; - } - } - - void RobotModel::staticsInit() - { - /* get baselink and thrust_link from robot model */ - auto robot_model_xml = getRobotModelXml("robot_description"); - - /* set rotor property */ - TiXmlElement* m_f_rate_attr = robot_model_xml.FirstChildElement("robot")->FirstChildElement("m_f_rate"); - if(!m_f_rate_attr) - ROS_ERROR("Can not get m_f_rate attribute from urdf model"); - else - m_f_rate_attr->Attribute("value", &m_f_rate_); - - std::vector urdf_links; - model_.getLinks(urdf_links); - for(const auto& link: urdf_links) - { - if(link->parent_joint) - { - if(link->parent_joint->name == "rotor1") - { - thrust_max_ = link->parent_joint->limits->upper; - thrust_min_ = link->parent_joint->limits->lower; - break; - } - } - } - - const int full_body_dof = 6 + joint_num_; - q_mat_.resize(6, rotor_num_); - gravity_.resize(6); - gravity_ << 0, 0, 9.80665, 0, 0, 0; - gravity_3d_.resize(3); - gravity_3d_ << 0, 0, 9.80665; - lambda_jacobian_.resize(rotor_num_, full_body_dof); - joint_torque_.resize(joint_num_); - joint_torque_jacobian_.resize(joint_num_, full_body_dof); - static_thrust_.resize(rotor_num_); - thrust_wrench_units_.resize(rotor_num_); - thrust_wrench_allocations_.resize(rotor_num_); - } - -} //namespace aerial_robot_model diff --git a/aerial_robot_model/test/aerial_robot_model/numerical_jacobians.cpp b/aerial_robot_model/test/aerial_robot_model/numerical_jacobians.cpp index 5ab124c26..9074c1104 100644 --- a/aerial_robot_model/test/aerial_robot_model/numerical_jacobians.cpp +++ b/aerial_robot_model/test/aerial_robot_model/numerical_jacobians.cpp @@ -2,7 +2,7 @@ namespace aerial_robot_model { - NumericalJacobian::NumericalJacobian(ros::NodeHandle nh, ros::NodeHandle nhp, std::unique_ptr robot_model): + NumericalJacobian::NumericalJacobian(ros::NodeHandle nh, ros::NodeHandle nhp, std::unique_ptr robot_model): nh_(nh), nhp_(nhp), robot_model_(std::move(robot_model)), diff --git a/aerial_robot_model/test/aerial_robot_model/numerical_jacobians.h b/aerial_robot_model/test/aerial_robot_model/numerical_jacobians.h index a5d35c108..db36896b2 100644 --- a/aerial_robot_model/test/aerial_robot_model/numerical_jacobians.h +++ b/aerial_robot_model/test/aerial_robot_model/numerical_jacobians.h @@ -35,7 +35,7 @@ #pragma once -#include +#include #include namespace aerial_robot_model { @@ -43,7 +43,7 @@ namespace aerial_robot_model { class NumericalJacobian { public: - NumericalJacobian(ros::NodeHandle nh, ros::NodeHandle nhp, std::unique_ptr robot_model = std::make_unique(true)); + NumericalJacobian(ros::NodeHandle nh, ros::NodeHandle nhp, std::unique_ptr robot_model = std::make_unique(true)); virtual ~NumericalJacobian() = default; virtual bool checkJacobians(); @@ -60,7 +60,7 @@ namespace aerial_robot_model { ros::Subscriber desire_coordinate_sub_; ros::NodeHandle nh_; ros::NodeHandle nhp_; - std::unique_ptr robot_model_; + std::unique_ptr robot_model_; bool rostest_; @@ -79,7 +79,7 @@ namespace aerial_robot_model { double feasible_control_force_diff_thre_; double feasible_control_torque_diff_thre_; - aerial_robot_model::RobotModel& getRobotModel() const { return *robot_model_; } + aerial_robot_model::transformable::RobotModel& getRobotModel() const { return *robot_model_; } void jointStateCallback(const sensor_msgs::JointStateConstPtr& state); void desireCoordinateCallback(const spinal::DesireCoordConstPtr& msg); diff --git a/aerial_robot_simulation/include/aerial_robot_simulation/aerial_robot_hw_sim.h b/aerial_robot_simulation/include/aerial_robot_simulation/aerial_robot_hw_sim.h index 0d460a611..84d068341 100644 --- a/aerial_robot_simulation/include/aerial_robot_simulation/aerial_robot_hw_sim.h +++ b/aerial_robot_simulation/include/aerial_robot_simulation/aerial_robot_hw_sim.h @@ -40,7 +40,7 @@ #ifndef _GAZEBO_ROS_CONTROL___AERIAL_ROBOT_HW_SIM_H_ #define _GAZEBO_ROS_CONTROL___AERIAL_ROBOT_HW_SIM_H_ -#include +#include #include #include #include diff --git a/robots/dragon/src/model/hydrus_like_robot_model.cpp b/robots/dragon/src/model/hydrus_like_robot_model.cpp index 8c0d3e8fc..219ff1adf 100644 --- a/robots/dragon/src/model/hydrus_like_robot_model.cpp +++ b/robots/dragon/src/model/hydrus_like_robot_model.cpp @@ -149,13 +149,13 @@ void HydrusLikeRobotModel::updateRobotModelImpl(const KDL::JntArray& joint_posit Eigen::MatrixXd HydrusLikeRobotModel::getJacobian(const KDL::JntArray& joint_positions, std::string segment_name, KDL::Vector offset) { - Eigen::MatrixXd jacobian = aerial_robot_model::RobotModel::getJacobian(joint_positions, segment_name, offset); + Eigen::MatrixXd jacobian = aerial_robot_model::transformable::RobotModel::getJacobian(joint_positions, segment_name, offset); return jacobian * gimbal_jacobian_; } void HydrusLikeRobotModel::calcBasicKinematicsJacobian() { - aerial_robot_model::RobotModel::calcBasicKinematicsJacobian(); + aerial_robot_model::transformable::RobotModel::calcBasicKinematicsJacobian(); const auto& joint_names = getJointNames(); const auto& link_joint_names = getLinkJointNames(); @@ -203,7 +203,7 @@ void HydrusLikeRobotModel::calcCoGMomentumJacobian() setCOGJacobian(Eigen::MatrixXd::Zero(3, 6 + getJointNum())); setLMomentumJacobian(Eigen::MatrixXd::Zero(3, 6 + getJointNum())); - aerial_robot_model::RobotModel::calcCoGMomentumJacobian(); + aerial_robot_model::transformable::RobotModel::calcCoGMomentumJacobian(); } void HydrusLikeRobotModel::updateJacobians(const KDL::JntArray& joint_positions, bool update_model) diff --git a/robots/dragon/test/dragon/numerical_jacobians.cpp b/robots/dragon/test/dragon/numerical_jacobians.cpp index 7c5144181..e04945118 100644 --- a/robots/dragon/test/dragon/numerical_jacobians.cpp +++ b/robots/dragon/test/dragon/numerical_jacobians.cpp @@ -1,6 +1,6 @@ #include -DragonNumericalJacobian::DragonNumericalJacobian(ros::NodeHandle nh, ros::NodeHandle nhp, std::unique_ptr robot_model): +DragonNumericalJacobian::DragonNumericalJacobian(ros::NodeHandle nh, ros::NodeHandle nhp, std::unique_ptr robot_model): HydrusNumericalJacobian(nh, nhp, std::move(robot_model)) { nhp_.param("check_rotor_overlap", check_rotor_overlap_, true); diff --git a/robots/dragon/test/dragon/numerical_jacobians.h b/robots/dragon/test/dragon/numerical_jacobians.h index def021b57..059549ca3 100644 --- a/robots/dragon/test/dragon/numerical_jacobians.h +++ b/robots/dragon/test/dragon/numerical_jacobians.h @@ -41,7 +41,7 @@ class DragonNumericalJacobian : public HydrusNumericalJacobian { public: - DragonNumericalJacobian(ros::NodeHandle nh, ros::NodeHandle nhp, std::unique_ptr robot_model = std::make_unique(true)); + DragonNumericalJacobian(ros::NodeHandle nh, ros::NodeHandle nhp, std::unique_ptr robot_model = std::make_unique(true)); virtual ~DragonNumericalJacobian() = default; virtual bool checkJacobians() override; diff --git a/robots/hydrus/include/hydrus/hydrus_robot_model.h b/robots/hydrus/include/hydrus/hydrus_robot_model.h index 0ccce4a34..3b3427841 100644 --- a/robots/hydrus/include/hydrus/hydrus_robot_model.h +++ b/robots/hydrus/include/hydrus/hydrus_robot_model.h @@ -35,9 +35,9 @@ #pragma once -#include +#include -class HydrusRobotModel : public aerial_robot_model::RobotModel { +class HydrusRobotModel : public aerial_robot_model::transformable::RobotModel { public: HydrusRobotModel(bool init_with_rosparam = true, bool verbose = false, diff --git a/robots/hydrus/src/hydrus_lqi_controller.cpp b/robots/hydrus/src/hydrus_lqi_controller.cpp index cbf4c944c..2fc0cb84f 100644 --- a/robots/hydrus/src/hydrus_lqi_controller.cpp +++ b/robots/hydrus/src/hydrus_lqi_controller.cpp @@ -191,7 +191,7 @@ bool HydrusLQIController::checkRobotModel() { lqi_mode_ = hydrus_robot_model_->getWrenchDof(); - if(robot_model_->getMass() == 0) + if(!robot_model_->initialized()) { ROS_DEBUG_NAMED("LQI gain generator", "LQI gain generator: robot model is not initiliazed"); return false; diff --git a/robots/hydrus/src/hydrus_robot_model.cpp b/robots/hydrus/src/hydrus_robot_model.cpp index 66d4af3e9..39109d63d 100644 --- a/robots/hydrus/src/hydrus_robot_model.cpp +++ b/robots/hydrus/src/hydrus_robot_model.cpp @@ -1,6 +1,6 @@ #include -using namespace aerial_robot_model; +using namespace aerial_robot_model::transformable; HydrusRobotModel::HydrusRobotModel(bool init_with_rosparam, bool verbose, double fc_t_min_thre, double fc_rp_min_thre, double epsilon, int wrench_dof): RobotModel(init_with_rosparam, verbose, 0, fc_t_min_thre, epsilon), @@ -17,6 +17,9 @@ HydrusRobotModel::HydrusRobotModel(bool init_with_rosparam, bool verbose, double fc_rp_dists_.resize(getRotorNum()); approx_fc_rp_dists_.resize(getRotorNum()); + if (getJointNum() == 0) { + updateRobotModel(); // workaround for tilt quadrotor robot model + } } void HydrusRobotModel::calcFeasibleControlRollPitchDists() @@ -48,6 +51,8 @@ void HydrusRobotModel::calcFeasibleControlRollPitchDists() void HydrusRobotModel::calcFeasibleControlRollPitchDistsJacobian() { + using namespace aerial_robot_model; + const int rotor_num = getRotorNum(); const int joint_num = getJointNum(); const int ndof = 6 + joint_num; @@ -193,7 +198,7 @@ void HydrusRobotModel::updateRobotModelImpl(const KDL::JntArray& joint_positions void HydrusRobotModel::updateJacobians(const KDL::JntArray& joint_positions, bool update_model) { - aerial_robot_model::RobotModel::updateJacobians(joint_positions, update_model); + aerial_robot_model::transformable::RobotModel::updateJacobians(joint_positions, update_model); calcFeasibleControlRollPitchDistsJacobian(); } diff --git a/robots/hydrus/test/hydrus/numerical_jacobians.cpp b/robots/hydrus/test/hydrus/numerical_jacobians.cpp index 73c5dd4b7..bf2b815fd 100644 --- a/robots/hydrus/test/hydrus/numerical_jacobians.cpp +++ b/robots/hydrus/test/hydrus/numerical_jacobians.cpp @@ -1,6 +1,6 @@ #include -HydrusNumericalJacobian::HydrusNumericalJacobian(ros::NodeHandle nh, ros::NodeHandle nhp, std::unique_ptr robot_model): +HydrusNumericalJacobian::HydrusNumericalJacobian(ros::NodeHandle nh, ros::NodeHandle nhp, std::unique_ptr robot_model): aerial_robot_model::NumericalJacobian(nh, nhp, std::move(robot_model)) { nhp_.param("check_feasible_control_roll_pitch", check_feasible_control_roll_pitch_, true); diff --git a/robots/hydrus/test/hydrus/numerical_jacobians.h b/robots/hydrus/test/hydrus/numerical_jacobians.h index d85fea4bb..c0f8bb650 100644 --- a/robots/hydrus/test/hydrus/numerical_jacobians.h +++ b/robots/hydrus/test/hydrus/numerical_jacobians.h @@ -41,7 +41,7 @@ class HydrusNumericalJacobian : public aerial_robot_model::NumericalJacobian { public: - HydrusNumericalJacobian(ros::NodeHandle nh, ros::NodeHandle nhp, std::unique_ptr robot_model = std::make_unique(true)); + HydrusNumericalJacobian(ros::NodeHandle nh, ros::NodeHandle nhp, std::unique_ptr robot_model = std::make_unique(true)); virtual ~HydrusNumericalJacobian() = default; virtual bool checkJacobians() override; diff --git a/robots/hydrus_xi/include/hydrus_xi/hydrus_xi_fully_actuated_robot_model.h b/robots/hydrus_xi/include/hydrus_xi/hydrus_xi_fully_actuated_robot_model.h index 4f17b638e..c0385a538 100644 --- a/robots/hydrus_xi/include/hydrus_xi/hydrus_xi_fully_actuated_robot_model.h +++ b/robots/hydrus_xi/include/hydrus_xi/hydrus_xi_fully_actuated_robot_model.h @@ -35,14 +35,14 @@ #pragma once -#include +#include #include #include #include -using namespace aerial_robot_model; +using namespace aerial_robot_model::transformable; -class HydrusXiFullyActuatedRobotModel : public aerial_robot_model::RobotModel { +class HydrusXiFullyActuatedRobotModel : public aerial_robot_model::transformable::RobotModel { public: HydrusXiFullyActuatedRobotModel(bool init_with_rosparam = true, bool verbose = false,