From 3b4f51715c081edcb2b717202db9337f163349d3 Mon Sep 17 00:00:00 2001 From: sugihara Date: Sat, 9 Mar 2024 22:29:41 +0900 Subject: [PATCH 01/27] [dragon] pass build with dependency of pinocchio --- robots/dragon/CMakeLists.txt | 5 ++++- robots/dragon/package.xml | 2 ++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/robots/dragon/CMakeLists.txt b/robots/dragon/CMakeLists.txt index ddfa2b1f2..23f5547f2 100644 --- a/robots/dragon/CMakeLists.txt +++ b/robots/dragon/CMakeLists.txt @@ -11,7 +11,7 @@ find_package(catkin REQUIRED COMPONENTS mujoco_ros_control pluginlib roscpp -) + ) find_package(Eigen3 REQUIRED) if(NOT CMAKE_BUILD_TYPE) @@ -21,6 +21,9 @@ set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O3 -g -DNDEBUG") find_package(NLopt REQUIRED) +set(CMAKE_CXX_STANDARD 17) +find_package(pinocchio REQUIRED) + catkin_package( INCLUDE_DIRS include test LIBRARIES dragon_robot_model dragon_aerial_robot_controllib dragon_navigation dragon_numerical_jacobians diff --git a/robots/dragon/package.xml b/robots/dragon/package.xml index b490b9033..00e274943 100644 --- a/robots/dragon/package.xml +++ b/robots/dragon/package.xml @@ -17,6 +17,7 @@ roscpp hydrus mujoco_ros_control + pinocchio pluginlib aerial_robot_control @@ -27,6 +28,7 @@ aerial_robot_base roscpp hydrus + pinocchio pluginlib rostest From 2eefe8c7a9563bd64c851e43da416afca909a887 Mon Sep 17 00:00:00 2001 From: sugihara Date: Sat, 9 Mar 2024 22:58:56 +0900 Subject: [PATCH 02/27] [dragon] add pinocchio simple sample --- robots/dragon/CMakeLists.txt | 3 + robots/dragon/robots/quad/robot.urdf | 930 +++++++++++++++++++++ robots/dragon/src/model/pinocchio_test.cpp | 43 + 3 files changed, 976 insertions(+) create mode 100644 robots/dragon/robots/quad/robot.urdf create mode 100644 robots/dragon/src/model/pinocchio_test.cpp diff --git a/robots/dragon/CMakeLists.txt b/robots/dragon/CMakeLists.txt index 23f5547f2..e24ad6a4b 100644 --- a/robots/dragon/CMakeLists.txt +++ b/robots/dragon/CMakeLists.txt @@ -57,6 +57,9 @@ add_dependencies(dragon_aerial_robot_controllib aerial_robot_msgs_generate_messa add_library(dragon_navigation src/dragon_navigation.cpp) target_link_libraries(dragon_navigation ${catkin_LIBRARIES}) +add_executable(pinocchio_test src/model/pinocchio_test.cpp) +target_link_libraries(pinocchio_test ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${pinocchio_LIBRARIES}) + # test # pre-build test code diff --git a/robots/dragon/robots/quad/robot.urdf b/robots/dragon/robots/quad/robot.urdf new file mode 100644 index 000000000..57bad22b8 --- /dev/null +++ b/robots/dragon/robots/quad/robot.urdf @@ -0,0 +1,930 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.00 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.00 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.2 + 0.2 + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + + hardware_interface/EffortJointInterface + + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + + hardware_interface/EffortJointInterface + + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + RotorInterface + + + RotorInterface + 1 + + + + 0.4 + 0.4 + + + + + + + + + + + + + + + + + + + + + + + + + + 0.00 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.00 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.2 + 0.2 + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + + hardware_interface/EffortJointInterface + + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + + hardware_interface/EffortJointInterface + + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + RotorInterface + + + RotorInterface + 1 + + + + 0.4 + 0.4 + + + + + + + + + + + + + + + + + + + + + + + + + + 0.00 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.00 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.2 + 0.2 + + + transmission_interface/SimpleTransmission + + + hardware_interface/EffortJointInterface + + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + + hardware_interface/EffortJointInterface + + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + RotorInterface + + + RotorInterface + 1 + + + + 0.4 + 0.4 + + + + + + + + + + + + + + + + + + + + + + + + + + 0.00 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.00 + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + RotorInterface + + + RotorInterface + 1 + + + + 0.4 + 0.4 + + + diff --git a/robots/dragon/src/model/pinocchio_test.cpp b/robots/dragon/src/model/pinocchio_test.cpp new file mode 100644 index 000000000..5f0abbbbf --- /dev/null +++ b/robots/dragon/src/model/pinocchio_test.cpp @@ -0,0 +1,43 @@ +#include "pinocchio/fwd.hpp" + +#include "pinocchio/parsers/urdf.hpp" + +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/algorithm/kinematics.hpp" + +#include + +// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here. +#ifndef PINOCCHIO_MODEL_DIR +#define PINOCCHIO_MODEL_DIR "/home/sugihara/ros/jsk_aerial_robot_ws/src/jsk_aerial_robot/robots/dragon/robots" +#endif + +int main(int argc, char ** argv) +{ + // You should change here to set up your own URDF file or just pass it as an argument of this example. + const std::string urdf_filename = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/quad/robot.urdf") : argv[1]; + + // Load the urdf model + pinocchio::Model model; + pinocchio::urdf::buildModel(urdf_filename,model); + std::cout << "model name: " << model.name << std::endl; + + // Create data required by the algorithms + pinocchio:: Data data(model); + + // Sample a random configuration + Eigen::VectorXd q = randomConfiguration(model); + q = Eigen::VectorXd::Zero(q.rows()); + std::cout << "q: " << q.transpose() << std::endl; + + // Perform the forward kinematics over the kinematic tree + pinocchio::forwardKinematics(model,data,q); + + // Print out the placement of each joint of the kinematic tree + for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model.njoints; ++joint_id) + std::cout << std::setw(24) << std::left + << model.names[joint_id] << ": " + << std::fixed << std::setprecision(2) + << data.oMi[joint_id].translation().transpose() + << std::endl; +} From 141a060cecd4140062241a59744ad6c9a65c4397 Mon Sep 17 00:00:00 2001 From: sugihara Date: Mon, 11 Mar 2024 02:05:28 +0900 Subject: [PATCH 03/27] [dragon] add FK sample with casadi and pinocchio (hard coded) --- aerial_robot_noetic.rosinstall | 4 +- robots/dragon/CMakeLists.txt | 4 +- robots/dragon/package.xml | 4 + robots/dragon/src/model/pinocchio_test.cpp | 127 ++++++++++++++++++++- 4 files changed, 130 insertions(+), 9 deletions(-) diff --git a/aerial_robot_noetic.rosinstall b/aerial_robot_noetic.rosinstall index b07b161d4..c60683f37 100644 --- a/aerial_robot_noetic.rosinstall +++ b/aerial_robot_noetic.rosinstall @@ -1,8 +1,8 @@ # aerial robot third party - git: local-name: aerial_robot_3rdparty - uri: https://github.com/JSKAerialRobot/aerial_robot_3rdparty.git - version: b10f5bf + uri: https://github.com/sugikazu75/aerial_robot_3rdparty.git + version: 27caf90 # kalman filter - git: diff --git a/robots/dragon/CMakeLists.txt b/robots/dragon/CMakeLists.txt index e24ad6a4b..7e68accd7 100644 --- a/robots/dragon/CMakeLists.txt +++ b/robots/dragon/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(catkin REQUIRED COMPONENTS aerial_robot_control aerial_robot_model aerial_robot_msgs + casadi_eigen hydrus mujoco_ros_control pluginlib @@ -23,6 +24,7 @@ find_package(NLopt REQUIRED) set(CMAKE_CXX_STANDARD 17) find_package(pinocchio REQUIRED) +find_package(casadi REQUIRED) catkin_package( INCLUDE_DIRS include test @@ -58,7 +60,7 @@ add_library(dragon_navigation src/dragon_navigation.cpp) target_link_libraries(dragon_navigation ${catkin_LIBRARIES}) add_executable(pinocchio_test src/model/pinocchio_test.cpp) -target_link_libraries(pinocchio_test ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${pinocchio_LIBRARIES}) +target_link_libraries(pinocchio_test ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${pinocchio_LIBRARIES} casadi) # test diff --git a/robots/dragon/package.xml b/robots/dragon/package.xml index 00e274943..13200a31a 100644 --- a/robots/dragon/package.xml +++ b/robots/dragon/package.xml @@ -14,6 +14,8 @@ aerial_robot_control aerial_robot_model aerial_robot_msgs + casadi + casadi_eigen roscpp hydrus mujoco_ros_control @@ -26,6 +28,8 @@ aerial_robot_estimation aerial_robot_simulation aerial_robot_base + casadi + casadi_eigen roscpp hydrus pinocchio diff --git a/robots/dragon/src/model/pinocchio_test.cpp b/robots/dragon/src/model/pinocchio_test.cpp index 5f0abbbbf..4f8d28486 100644 --- a/robots/dragon/src/model/pinocchio_test.cpp +++ b/robots/dragon/src/model/pinocchio_test.cpp @@ -4,40 +4,155 @@ #include "pinocchio/algorithm/joint-configuration.hpp" #include "pinocchio/algorithm/kinematics.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/algorithm/kinematics-derivatives.hpp" +#include "pinocchio/algorithm/model.hxx" +#include "pinocchio/autodiff/casadi.hpp" +// #include +// #include + +#include +#include +#include +#include +#include #include // PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here. #ifndef PINOCCHIO_MODEL_DIR -#define PINOCCHIO_MODEL_DIR "/home/sugihara/ros/jsk_aerial_robot_ws/src/jsk_aerial_robot/robots/dragon/robots" +#define PINOCCHIO_MODEL_DIR "/home/sugihara/ros/test_ws/src/jsk_aerial_robot/robots/dragon/robots" #endif int main(int argc, char ** argv) { // You should change here to set up your own URDF file or just pass it as an argument of this example. const std::string urdf_filename = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/quad/robot.urdf") : argv[1]; - + // Load the urdf model pinocchio::Model model; pinocchio::urdf::buildModel(urdf_filename,model); std::cout << "model name: " << model.name << std::endl; - + // Create data required by the algorithms pinocchio:: Data data(model); - + + std::cout << "model.nq: " << model.nq << std::endl; + std::cout << "model.nv: " << model.nv << std::endl; + // Sample a random configuration Eigen::VectorXd q = randomConfiguration(model); q = Eigen::VectorXd::Zero(q.rows()); + // q(model.getJointId("joint1_yaw")) = ; + // q(model.getJointId("joint2_yaw")) = 1.57; + // q(model.getJointId("joint3_yaw")) = 1.57; std::cout << "q: " << q.transpose() << std::endl; + auto model_casadi = model.cast(); + pinocchio::DataTpl data_casadi(model_casadi); + Eigen::Matrix q_casadi; + q_casadi(0) = casadi::SX::sym("gimbal1_roll", 1); + q_casadi(1) = casadi::SX::sym("gimbal1_pitch", 1); + // q_casadi(2) = casadi::SX::sym("gimbal1_roll", 1); + // q_casadi(3) = casadi::SX::sym("gimbal1_roll", 1); + q_casadi(4) = casadi::SX::sym("joint1_pitch", 1); + q_casadi(5) = casadi::SX::sym("joint1_yaw", 1); + q_casadi(6) = casadi::SX::sym("gimbal2_roll", 1); + q_casadi(7) = casadi::SX::sym("gimbal2_pitch", 1); + // q_casadi(8) = casadi::SX::sym("gimbal1_roll", 1); + // q_casadi(9) = casadi::SX::sym("gimbal1_pitch", 1); + q_casadi(10) = casadi::SX::sym("joint2_pitch", 1); + q_casadi(11) = casadi::SX::sym("joint2_yaw", 1); + q_casadi(12) = casadi::SX::sym("gimbal3_roll", 1); + q_casadi(13) = casadi::SX::sym("gimbal3_pitch", 1); + // q_casadi(14) = casadi::SX::sym("gimbal1_roll", 1); + // q_casadi(15) = casadi::SX::sym("gimbal1_pitch", 1); + q_casadi(16) = casadi::SX::sym("joint3_pitch", 1); + q_casadi(17) = casadi::SX::sym("joint3_yaw", 1); + q_casadi(18) = casadi::SX::sym("gimbal4_roll", 1); + q_casadi(19) = casadi::SX::sym("gimbal4_pitch", 1); + // q_casadi(20) = casadi::SX::sym("gimbal1_roll", 1); + // q_casadi(21) = casadi::SX::sym("gimbal1_pitch", 1); + // Perform the forward kinematics over the kinematic tree - pinocchio::forwardKinematics(model,data,q); + pinocchio::forwardKinematics(model, data, q); + pinocchio::forwardKinematics(model_casadi, data_casadi, q_casadi); // Print out the placement of each joint of the kinematic tree for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model.njoints; ++joint_id) std::cout << std::setw(24) << std::left << model.names[joint_id] << ": " - << std::fixed << std::setprecision(2) + << std::setw(15) << std::left + << model.joints[joint_id].shortname() << ": " + << std::fixed << std::setprecision(5) << data.oMi[joint_id].translation().transpose() << std::endl; + + std::cout << std::endl; + std::cout << std::endl; + + std::cout << "q_casadi: " << q_casadi.transpose() << std::endl; + for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_casadi.njoints; ++joint_id) + std::cout << std::setw(24) << std::left + << model_casadi.names[joint_id] << ": " + << std::setw(15) << std::left + << model_casadi.joints[joint_id].shortname() << ": " + << std::fixed << std::setprecision(5) + << data_casadi.oMi[joint_id].translation().transpose() + << std::endl; + + std::cout << std::endl; + + // check the rotor pos (hard coded) + Eigen::Matrix pos = data_casadi.oMi[model_casadi.getJointId("gimbal1_pitch")].translation(); + std::cout << std::setw(20) << std::left + << "gimbal1_pitch pos" << ": " << pos.transpose() << std::endl; + std::cout << std::endl; + std::cout << std::endl; + + Eigen::Matrix pos_j; + for(int i = 0; i < pos.rows(); i++) + { + pos_j(i) = jacobian(pos(i), q_casadi(0)); + casadi::Function S = casadi::Function("S", {q_casadi(0)}, {pos_j(i)}); + } + std::cout << std::setw(20) << std::left + << "gimbal1_pitch pos'" << ": " << pos_j.transpose() << std::endl; + std::cout << std::endl; + std::cout << std::endl; + + + // check jacobian + pinocchio::computeJointJacobians(model, data, q); + std::cout << "data.J: \n" << data.J << std::endl; + std::cout << std::endl; + + pinocchio::computeJointJacobians(model_casadi, data_casadi, q_casadi); + std::cout << "data.J: \n" << data_casadi.J << std::endl; + std::cout << std::endl; + + // kinematics derivatives + Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); + Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); + + // Computes the kinematics derivatives for all the joints of the robot + pinocchio::computeForwardKinematicsDerivatives(model, data, q, v, a); + + // Retrieve the kinematics derivatives of a specific joint, expressed in the LOCAL frame of the joints. + pinocchio::JointIndex joint_id = (pinocchio::JointIndex)(model.njoints-1); + pinocchio::Data::Matrix6x v_partial_dq(6, model.nv), a_partial_dq(6, model.nv), a_partial_dv(6, model.nv), a_partial_da(6, model.nv); + v_partial_dq.setZero(); + a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero(); + pinocchio::getJointAccelerationDerivatives(model, data, joint_id, pinocchio::LOCAL, v_partial_dq, + a_partial_dq, a_partial_dv, a_partial_da); + + std::cout << "v partial dq: \n" << v_partial_dq << std::endl; + std::cout << std::endl; + // Remark: we are not directly computing the quantity v_partial_dv as it is also equal to a_partial_da. + + // But we can also expressed the same quantities in the frame centered on the end-effector joint, but expressed in the axis aligned with the world frame. + pinocchio::getJointAccelerationDerivatives(model,data, joint_id, pinocchio::WORLD, v_partial_dq, + a_partial_dq, a_partial_dv, a_partial_da); + std::cout << "v partial dq: \n" << v_partial_dq << std::endl; + } From 252b30676ed4550c7e374a4f570c5a06ce2cfdfd Mon Sep 17 00:00:00 2001 From: sugihara Date: Mon, 11 Mar 2024 17:38:16 +0900 Subject: [PATCH 04/27] [dragon] update pinocchio robot model class. implemented rotor origin and axis search process --- aerial_robot_melodic.rosinstall | 4 +- robots/dragon/src/model/pinocchio_test.cpp | 386 ++++++++++++++------- 2 files changed, 266 insertions(+), 124 deletions(-) diff --git a/aerial_robot_melodic.rosinstall b/aerial_robot_melodic.rosinstall index b07b161d4..c60683f37 100644 --- a/aerial_robot_melodic.rosinstall +++ b/aerial_robot_melodic.rosinstall @@ -1,8 +1,8 @@ # aerial robot third party - git: local-name: aerial_robot_3rdparty - uri: https://github.com/JSKAerialRobot/aerial_robot_3rdparty.git - version: b10f5bf + uri: https://github.com/sugikazu75/aerial_robot_3rdparty.git + version: 27caf90 # kalman filter - git: diff --git a/robots/dragon/src/model/pinocchio_test.cpp b/robots/dragon/src/model/pinocchio_test.cpp index 4f8d28486..b807b76c2 100644 --- a/robots/dragon/src/model/pinocchio_test.cpp +++ b/robots/dragon/src/model/pinocchio_test.cpp @@ -6,6 +6,7 @@ #include "pinocchio/algorithm/kinematics.hpp" #include "pinocchio/algorithm/joint-configuration.hpp" #include "pinocchio/algorithm/kinematics-derivatives.hpp" +#include "pinocchio/algorithm/center-of-mass.hxx" #include "pinocchio/algorithm/model.hxx" #include "pinocchio/autodiff/casadi.hpp" // #include @@ -21,138 +22,279 @@ // PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here. #ifndef PINOCCHIO_MODEL_DIR -#define PINOCCHIO_MODEL_DIR "/home/sugihara/ros/test_ws/src/jsk_aerial_robot/robots/dragon/robots" +#define PINOCCHIO_MODEL_DIR "/home/sugihara/ros/jsk_aerial_robot_ws/src/jsk_aerial_robot/robots/dragon/robots" #endif -int main(int argc, char ** argv) +class PinocchioRobotModel +{ + public: + PinocchioRobotModel(); + ~PinocchioRobotModel() = default; + + void modelInit(); + void kinematicsInit(); + void inertialInit(); + + pinocchio::Model model_dbl_; + pinocchio::Data data_dlb_; + pinocchio::ModelTpl model_; + pinocchio::DataTpl data_; + + std::vector rotor_origin_root_; + std::vector rotor_normal_root_; + + casadi::SX q_cs_; + Eigen::Matrix q_; + casadi::SX cog_pos_; + + std::map joint_index_map_; + int rotor_num_; + + double mass_; +}; + +PinocchioRobotModel::PinocchioRobotModel() +{ + modelInit(); + kinematicsInit(); + inertialInit(); +} + +void PinocchioRobotModel::modelInit() { // You should change here to set up your own URDF file or just pass it as an argument of this example. - const std::string urdf_filename = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/quad/robot.urdf") : argv[1]; + const std::string urdf_filename = PINOCCHIO_MODEL_DIR + std::string("/quad/robot.urdf"); // Load the urdf model - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename,model); - std::cout << "model name: " << model.name << std::endl; + pinocchio::urdf::buildModel(urdf_filename, model_dbl_); + std::cout << "model name: " << model_dbl_.name << std::endl; // Create data required by the algorithms - pinocchio:: Data data(model); - - std::cout << "model.nq: " << model.nq << std::endl; - std::cout << "model.nv: " << model.nv << std::endl; - - // Sample a random configuration - Eigen::VectorXd q = randomConfiguration(model); - q = Eigen::VectorXd::Zero(q.rows()); - // q(model.getJointId("joint1_yaw")) = ; - // q(model.getJointId("joint2_yaw")) = 1.57; - // q(model.getJointId("joint3_yaw")) = 1.57; - std::cout << "q: " << q.transpose() << std::endl; - - auto model_casadi = model.cast(); - pinocchio::DataTpl data_casadi(model_casadi); - Eigen::Matrix q_casadi; - q_casadi(0) = casadi::SX::sym("gimbal1_roll", 1); - q_casadi(1) = casadi::SX::sym("gimbal1_pitch", 1); - // q_casadi(2) = casadi::SX::sym("gimbal1_roll", 1); - // q_casadi(3) = casadi::SX::sym("gimbal1_roll", 1); - q_casadi(4) = casadi::SX::sym("joint1_pitch", 1); - q_casadi(5) = casadi::SX::sym("joint1_yaw", 1); - q_casadi(6) = casadi::SX::sym("gimbal2_roll", 1); - q_casadi(7) = casadi::SX::sym("gimbal2_pitch", 1); - // q_casadi(8) = casadi::SX::sym("gimbal1_roll", 1); - // q_casadi(9) = casadi::SX::sym("gimbal1_pitch", 1); - q_casadi(10) = casadi::SX::sym("joint2_pitch", 1); - q_casadi(11) = casadi::SX::sym("joint2_yaw", 1); - q_casadi(12) = casadi::SX::sym("gimbal3_roll", 1); - q_casadi(13) = casadi::SX::sym("gimbal3_pitch", 1); - // q_casadi(14) = casadi::SX::sym("gimbal1_roll", 1); - // q_casadi(15) = casadi::SX::sym("gimbal1_pitch", 1); - q_casadi(16) = casadi::SX::sym("joint3_pitch", 1); - q_casadi(17) = casadi::SX::sym("joint3_yaw", 1); - q_casadi(18) = casadi::SX::sym("gimbal4_roll", 1); - q_casadi(19) = casadi::SX::sym("gimbal4_pitch", 1); - // q_casadi(20) = casadi::SX::sym("gimbal1_roll", 1); - // q_casadi(21) = casadi::SX::sym("gimbal1_pitch", 1); - - // Perform the forward kinematics over the kinematic tree - pinocchio::forwardKinematics(model, data, q); - pinocchio::forwardKinematics(model_casadi, data_casadi, q_casadi); + pinocchio::Data data_dlb_(model_dbl_); - // Print out the placement of each joint of the kinematic tree - for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model.njoints; ++joint_id) - std::cout << std::setw(24) << std::left - << model.names[joint_id] << ": " - << std::setw(15) << std::left - << model.joints[joint_id].shortname() << ": " - << std::fixed << std::setprecision(5) - << data.oMi[joint_id].translation().transpose() - << std::endl; - - std::cout << std::endl; - std::cout << std::endl; - - std::cout << "q_casadi: " << q_casadi.transpose() << std::endl; - for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_casadi.njoints; ++joint_id) - std::cout << std::setw(24) << std::left - << model_casadi.names[joint_id] << ": " - << std::setw(15) << std::left - << model_casadi.joints[joint_id].shortname() << ": " - << std::fixed << std::setprecision(5) - << data_casadi.oMi[joint_id].translation().transpose() - << std::endl; - - std::cout << std::endl; - - // check the rotor pos (hard coded) - Eigen::Matrix pos = data_casadi.oMi[model_casadi.getJointId("gimbal1_pitch")].translation(); - std::cout << std::setw(20) << std::left - << "gimbal1_pitch pos" << ": " << pos.transpose() << std::endl; - std::cout << std::endl; - std::cout << std::endl; - - Eigen::Matrix pos_j; - for(int i = 0; i < pos.rows(); i++) + // declaraion of model and data with casadi + model_ = model_dbl_.cast(); + data_ = pinocchio::DataTpl(model_); + + std::cout << "model_.nq: " << model_.nq << std::endl; + std::cout << "model_.nv: " << model_.nv << std::endl; + + std::vector q_dims(model_.njoints); + for(int i = 0; i < model_.nv; i++) + { + std::string joint_type = model_.joints[i].shortname(); + if(joint_type == "JointModelRUBX" || joint_type == "JointModelRUBY" || joint_type == "JointModelRUBZ") + q_dims.at(i) = 2; + else + q_dims.at(i) = 1; + } + + int joint_index = 0; + rotor_num_ = 0; + // std::cout << "model njoints: " << model_.njoints << std::endl; + for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) + { + if(model_.names[joint_id] == "universe") + { + std::cout << "find joint named universe" << std::endl; + } + else + { + // std::cout << model_.names[joint_id] << std::endl; + joint_index_map_[model_.names[joint_id]] = joint_index; + joint_index += q_dims.at(joint_id); + + // special process for rotor + if(model_.names[joint_id].find("rotor") != std::string::npos) + { + // std::cout << "found rotor named " << model_.names[joint_id] << std::endl; + rotor_num_++; + } + } + } + // std::cout << "rotor num: " << rotor_num_ << std::endl; + + // check the joint index map + std::cout << "joint index map: \n"; + for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) + { + if(joint_index_map_.count(model_.names[joint_id])) + { + std::cout << model_.names[joint_id] << " :" << joint_index_map_.at(model_.names[joint_id]) << std::endl; + } + } +} + +void PinocchioRobotModel::kinematicsInit() +{ + // init q + q_cs_ = casadi::SX::sym("q", model_.nq); + q_.resize(model_.nq, 1); + for(int i = 0; i < model_.nq; i++) + { + q_(i) = q_cs_(i); + } + std::cout << "q: " << q_.transpose() << std::endl; + + // solve FK + pinocchio::forwardKinematics(model_, data_, q_); + + // setup rotor info + rotor_origin_root_.resize(rotor_num_); + rotor_normal_root_.resize(rotor_num_); + + for(int i = 0; i < rotor_num_; i++) + { + std::string rotor_name = "rotor" + std::to_string(i + 1); + int joint_id = model_.getJointId(rotor_name); + // std::cout << rotor_name << " " << joint_id << std::endl; + casadi::SX rotor_origin_root = casadi::SX::zeros(3); + for(int j = 0; j < 3; j++) + rotor_origin_root(j) = data_.oMi[joint_id].translation()(j); + rotor_origin_root_.at(i) = rotor_origin_root; + // std::cout << "rotor origin: " << i << " " << rotor_origin_root_.at(i) << std::endl; + + casadi::SX rotor_normal_root = casadi::SX::zeros(3); + int rotor_axis_type; + if(model_.joints[joint_id].shortname() == "JointModelRX" || model_.joints[joint_id].shortname() == "JointModelRUBX") + rotor_axis_type = 0; + else if(model_.joints[joint_id].shortname() == "JointModelRY" || model_.joints[joint_id].shortname() == "JointModelRUBY") + rotor_axis_type = 1; + else if(model_.joints[joint_id].shortname() == "JointModelRZ" || model_.joints[joint_id].shortname() == "JointModelRUBZ") + rotor_axis_type = 2; + + for(int j = 0; j < 3; j++) { - pos_j(i) = jacobian(pos(i), q_casadi(0)); - casadi::Function S = casadi::Function("S", {q_casadi(0)}, {pos_j(i)}); + rotor_normal_root(j) = data_.oMi[joint_id].rotation()(j, rotor_axis_type); } - std::cout << std::setw(20) << std::left - << "gimbal1_pitch pos'" << ": " << pos_j.transpose() << std::endl; - std::cout << std::endl; - std::cout << std::endl; - - - // check jacobian - pinocchio::computeJointJacobians(model, data, q); - std::cout << "data.J: \n" << data.J << std::endl; - std::cout << std::endl; - - pinocchio::computeJointJacobians(model_casadi, data_casadi, q_casadi); - std::cout << "data.J: \n" << data_casadi.J << std::endl; - std::cout << std::endl; - - // kinematics derivatives - Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); - Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); - - // Computes the kinematics derivatives for all the joints of the robot - pinocchio::computeForwardKinematicsDerivatives(model, data, q, v, a); - - // Retrieve the kinematics derivatives of a specific joint, expressed in the LOCAL frame of the joints. - pinocchio::JointIndex joint_id = (pinocchio::JointIndex)(model.njoints-1); - pinocchio::Data::Matrix6x v_partial_dq(6, model.nv), a_partial_dq(6, model.nv), a_partial_dv(6, model.nv), a_partial_da(6, model.nv); - v_partial_dq.setZero(); - a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero(); - pinocchio::getJointAccelerationDerivatives(model, data, joint_id, pinocchio::LOCAL, v_partial_dq, - a_partial_dq, a_partial_dv, a_partial_da); - - std::cout << "v partial dq: \n" << v_partial_dq << std::endl; - std::cout << std::endl; - // Remark: we are not directly computing the quantity v_partial_dv as it is also equal to a_partial_da. - - // But we can also expressed the same quantities in the frame centered on the end-effector joint, but expressed in the axis aligned with the world frame. - pinocchio::getJointAccelerationDerivatives(model,data, joint_id, pinocchio::WORLD, v_partial_dq, - a_partial_dq, a_partial_dv, a_partial_da); - std::cout << "v partial dq: \n" << v_partial_dq << std::endl; + // std::cout << "rotor rotation: \n" << data_.oMi[joint_id].rotation() << std::endl; + rotor_normal_root_.at(i) = rotor_normal_root; + // std::cout << "rotor axis for rotor" << i << ": " << rotor_axis_type << std::endl; + // std::cout << "rotor axis: " << i << " " << rotor_normal_root_.at(i) << std::endl; + // std::cout << std::endl; + } + + // Print out the placement of each joint of the kinematic tree + // for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) + // std::cout << std::setw(24) << std::left + // << model_.names[joint_id] << ": " + // << std::setw(15) << std::left + // << model_.joints[joint_id].shortname() << ": " + // << std::fixed << std::setprecision(5) + // << data_.oMi[joint_id].translation().transpose() + // << std::endl; + + + +} + +void PinocchioRobotModel::inertialInit() +{ + // https://github.com/stack-of-tasks/pinocchio/issues/1286 + // TODO: compute mass. But root link1 (fixed to root link) is ignored. + // casadi::SX m = 0.0; + // for(pinocchio::JointIndex i=0; i<(pinocchio::JointIndex)(model_.njoints); ++i) + // { + // std::cout << model_.names[i] << ": " << model_.inertias[i].mass() << std::endl; + // m += model_.inertias[i].mass(); + // } + // std::cout << m << std::endl; + // mass_ = pinocchio::computeTotalMass(model_); + // std::cout << "mass: " << mass_ << std::endl; + // cog_pos_ = pinocchio::centerOfMass(model_, data_, q_); + // std::cout << cog_pos_ << std::endl; +} + +int main(int argc, char ** argv) +{ + PinocchioRobotModel pinocchio_robot_model; + + // // zero configuration + // Eigen::VectorXd q = randomConfiguration(model); + // q = Eigen::VectorXd::Zero(q.rows()); + // std::cout << "q: " << q.transpose() << std::endl; + + // // joint space with casadi + // auto q_cs = casadi::SX::sym("q", model.nq); + // Eigen::Matrix q_casadi; + // q_casadi.resize(model.nq, 1); + // for(int i = 0; i < model.nq; i++) + // q_casadi(i) = q_cs(i); + + // // Perform the forward kinematics over the kinematic tree + // pinocchio::forwardKinematics(model, data, q); + + // // Print out the placement of each joint of the kinematic tree + // for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model.njoints; ++joint_id) + // std::cout << std::setw(24) << std::left + // << model.names[joint_id] << ": " + // << std::setw(15) << std::left + // << model.joints[joint_id].shortname() << ": " + // << std::fixed << std::setprecision(5) + // << data.oMi[joint_id].translation().transpose() + // << std::endl; + + // std::cout << std::endl; + // std::cout << std::endl; + + // std::cout << "q_casadi: " << q_casadi.transpose() << std::endl; + // for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_casadi.njoints; ++joint_id) + // std::cout << std::setw(24) << std::left + // << model_casadi.names[joint_id] << ": " + // << std::setw(15) << std::left + // << model_casadi.joints[joint_id].shortname() << ": " + // << std::fixed << std::setprecision(5) + // << data_casadi.oMi[joint_id].translation().transpose() + // << std::endl; + + // std::cout << std::endl; + + // // check the position of each joint + // casadi::DM casadi_dbl = casadi::DM({0, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}); + // std::cout << "casadi_dbl: " << casadi_dbl << std::endl; + // for(int i = 0; i < model.nv; i++) + // { + // for(int j = 0; j < 3; j++) + // { + // casadi::Function f = casadi::Function("f", {q_cs}, {data_casadi.oMi[i].translation()(j)}); + // casadi::DM pos_j = f(casadi_dbl); + // std::cout << pos_j << " "; + // } + // std::cout << std::endl; + // } + // std::cout << std::endl; + + // // check jacobian + // pinocchio::computeJointJacobians(model, data, q); + // std::cout << "data.J: \n" << data.J << std::endl; + // std::cout << std::endl; + + // pinocchio::computeJointJacobians(model_casadi, data_casadi, q_casadi); + // std::cout << "data.J: \n" << data_casadi.J << std::endl; + // std::cout << std::endl; + + // // kinematics derivatives + // Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); + // Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); + + // // Computes the kinematics derivatives for all the joints of the robot + // pinocchio::computeForwardKinematicsDerivatives(model, data, q, v, a); + + // // Retrieve the kinematics derivatives of a specific joint, expressed in the LOCAL frame of the joints. + // pinocchio::JointIndex joint_id = (pinocchio::JointIndex)(model.njoints-1); + // pinocchio::Data::Matrix6x v_partial_dq(6, model.nv), a_partial_dq(6, model.nv), a_partial_dv(6, model.nv), a_partial_da(6, model.nv); + // v_partial_dq.setZero(); + // a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero(); + // pinocchio::getJointAccelerationDerivatives(model, data, joint_id, pinocchio::LOCAL, v_partial_dq, + // a_partial_dq, a_partial_dv, a_partial_da); + + // std::cout << "v partial dq: \n" << v_partial_dq << std::endl; + // std::cout << std::endl; + // // Remark: we are not directly computing the quantity v_partial_dv as it is also equal to a_partial_da. + + // // But we can also expressed the same quantities in the frame centered on the end-effector joint, but expressed in the axis aligned with the world frame. + // pinocchio::getJointAccelerationDerivatives(model,data, joint_id, pinocchio::WORLD, v_partial_dq, + // a_partial_dq, a_partial_dv, a_partial_da); + // std::cout << "v partial dq: \n" << v_partial_dq << std::endl; } From 5551e58a1ae7ad5b7516b6de923a16698d7b7b6f Mon Sep 17 00:00:00 2001 From: sugihara Date: Tue, 12 Mar 2024 20:46:25 +0900 Subject: [PATCH 05/27] [dragon] update to use floating base system. add real value calculation function --- robots/dragon/src/model/pinocchio_test.cpp | 91 +++++++++++++++------- 1 file changed, 65 insertions(+), 26 deletions(-) diff --git a/robots/dragon/src/model/pinocchio_test.cpp b/robots/dragon/src/model/pinocchio_test.cpp index b807b76c2..a79831c30 100644 --- a/robots/dragon/src/model/pinocchio_test.cpp +++ b/robots/dragon/src/model/pinocchio_test.cpp @@ -9,8 +9,10 @@ #include "pinocchio/algorithm/center-of-mass.hxx" #include "pinocchio/algorithm/model.hxx" #include "pinocchio/autodiff/casadi.hpp" +#include +#include // #include -// #include +#include #include #include @@ -45,12 +47,13 @@ class PinocchioRobotModel casadi::SX q_cs_; Eigen::Matrix q_; + casadi::SX mass_; casadi::SX cog_pos_; + casadi::SX inertia_; std::map joint_index_map_; int rotor_num_; - double mass_; }; PinocchioRobotModel::PinocchioRobotModel() @@ -60,13 +63,43 @@ PinocchioRobotModel::PinocchioRobotModel() inertialInit(); } +Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, Eigen::VectorXd x_dbl) +{ + casadi::DM ret = casadi::DM(y.size1(), y.size2()); + for(int i = 0; i < y.size1(); i++) + { + for(int j = 0; j < y.size2(); j++) + { + casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); + casadi::DM y_dbl = f(eigenVectorToCasadiDm(x_dbl)); + ret(i, j) = y_dbl; + } + } + return casadiDmToEigenMatrix(ret); +} + +Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, casadi::DM x_dbl) +{ + casadi::DM ret = casadi::DM(y.size1(), y.size2()); + for(int i = 0; i < y.size1(); i++) + { + for(int j = 0; j < y.size2(); j++) + { + casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); + casadi::DM y_dbl = f(x_dbl); + ret(i, j) = y_dbl; + } + } + return casadiDmToEigenMatrix(ret); +} + void PinocchioRobotModel::modelInit() { // You should change here to set up your own URDF file or just pass it as an argument of this example. const std::string urdf_filename = PINOCCHIO_MODEL_DIR + std::string("/quad/robot.urdf"); // Load the urdf model - pinocchio::urdf::buildModel(urdf_filename, model_dbl_); + pinocchio::urdf::buildModel(urdf_filename, pinocchio::JointModelFreeFlyer(), model_dbl_); std::cout << "model name: " << model_dbl_.name << std::endl; // Create data required by the algorithms @@ -78,12 +111,17 @@ void PinocchioRobotModel::modelInit() std::cout << "model_.nq: " << model_.nq << std::endl; std::cout << "model_.nv: " << model_.nv << std::endl; + std::cout << "model_.njoints: " << model_.njoints << std::endl; + std::cout << std::endl; std::vector q_dims(model_.njoints); - for(int i = 0; i < model_.nv; i++) + for(int i = 0; i < model_.njoints; i++) { std::string joint_type = model_.joints[i].shortname(); - if(joint_type == "JointModelRUBX" || joint_type == "JointModelRUBY" || joint_type == "JointModelRUBZ") + // std::cout << model_.names[i] << " " << joint_type << std::endl; + if(joint_type == "JointModelFreeFlyer") + q_dims.at(i) = 7; + else if(joint_type == "JointModelRUBX" || joint_type == "JointModelRUBY" || joint_type == "JointModelRUBZ") q_dims.at(i) = 2; else q_dims.at(i) = 1; @@ -91,27 +129,27 @@ void PinocchioRobotModel::modelInit() int joint_index = 0; rotor_num_ = 0; - // std::cout << "model njoints: " << model_.njoints << std::endl; for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) { + std::cout << model_.names[joint_id] << std::endl; if(model_.names[joint_id] == "universe") { std::cout << "find joint named universe" << std::endl; } else { - // std::cout << model_.names[joint_id] << std::endl; joint_index_map_[model_.names[joint_id]] = joint_index; joint_index += q_dims.at(joint_id); // special process for rotor if(model_.names[joint_id].find("rotor") != std::string::npos) { - // std::cout << "found rotor named " << model_.names[joint_id] << std::endl; + std::cout << "found rotor named " << model_.names[joint_id] << std::endl; rotor_num_++; } } } + std::cout << std::endl; // std::cout << "rotor num: " << rotor_num_ << std::endl; // check the joint index map @@ -123,6 +161,7 @@ void PinocchioRobotModel::modelInit() std::cout << model_.names[joint_id] << " :" << joint_index_map_.at(model_.names[joint_id]) << std::endl; } } + std::cout << std::endl; } void PinocchioRobotModel::kinematicsInit() @@ -134,7 +173,7 @@ void PinocchioRobotModel::kinematicsInit() { q_(i) = q_cs_(i); } - std::cout << "q: " << q_.transpose() << std::endl; + // std::cout << "q: " << q_.transpose() << std::endl; // solve FK pinocchio::forwardKinematics(model_, data_, q_); @@ -178,31 +217,31 @@ void PinocchioRobotModel::kinematicsInit() // for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) // std::cout << std::setw(24) << std::left // << model_.names[joint_id] << ": " - // << std::setw(15) << std::left + // << std::setw(24) << std::left // << model_.joints[joint_id].shortname() << ": " // << std::fixed << std::setprecision(5) // << data_.oMi[joint_id].translation().transpose() // << std::endl; - - - } void PinocchioRobotModel::inertialInit() { - // https://github.com/stack-of-tasks/pinocchio/issues/1286 - // TODO: compute mass. But root link1 (fixed to root link) is ignored. - // casadi::SX m = 0.0; - // for(pinocchio::JointIndex i=0; i<(pinocchio::JointIndex)(model_.njoints); ++i) - // { - // std::cout << model_.names[i] << ": " << model_.inertias[i].mass() << std::endl; - // m += model_.inertias[i].mass(); - // } - // std::cout << m << std::endl; - // mass_ = pinocchio::computeTotalMass(model_); - // std::cout << "mass: " << mass_ << std::endl; - // cog_pos_ = pinocchio::centerOfMass(model_, data_, q_); - // std::cout << cog_pos_ << std::endl; + mass_ = pinocchio::computeTotalMass(model_); + std::cout << "mass: " << mass_ << std::endl; + + auto cog_pos = pinocchio::centerOfMass(model_, data_, q_, true); + pinocchio::casadi::copy(cog_pos, cog_pos_); + // std::cout << "cog_pos_: "cog_pos_ << std::endl; + std::cout << std::endl; + + casadi::DM q_test = casadi::DM(model_.nq, 1); + q_test(joint_index_map_["joint1_yaw"]) = 1.57; + q_test(joint_index_map_["joint2_yaw"]) = 1.57; + q_test(joint_index_map_["joint3_yaw"]) = 1.57; + std::cout << "q_test: " << q_test << std::endl; + std::cout << std::endl; + std::cout << "real cog: " << computeRealValue(cog_pos_, q_cs_, q_test).transpose() << std::endl; + std::cout << std::endl; } int main(int argc, char ** argv) From eeb72a37081a4e3be1db72457e73c33921a350f2 Mon Sep 17 00:00:00 2001 From: sugihara Date: Wed, 13 Mar 2024 22:30:51 +0900 Subject: [PATCH 06/27] [dragon] update pinocchio model. add ros interface, enable to calculate inertial of cog --- .../include/dragon/model/pinocchio_test.h | 129 ++++++++ robots/dragon/src/model/pinocchio_test.cpp | 283 ++++++++---------- 2 files changed, 257 insertions(+), 155 deletions(-) create mode 100644 robots/dragon/include/dragon/model/pinocchio_test.h diff --git a/robots/dragon/include/dragon/model/pinocchio_test.h b/robots/dragon/include/dragon/model/pinocchio_test.h new file mode 100644 index 000000000..467188fcb --- /dev/null +++ b/robots/dragon/include/dragon/model/pinocchio_test.h @@ -0,0 +1,129 @@ +#pragma once + +#include "pinocchio/fwd.hpp" + +#include "pinocchio/parsers/urdf.hpp" + +#include "pinocchio/algorithm/center-of-mass.hxx" +#include "pinocchio/algorithm/centroidal.hxx" +#include "pinocchio/algorithm/crba.hpp" +#include "pinocchio/algorithm/frames.hpp" +#include "pinocchio/algorithm/kinematics.hpp" +#include "pinocchio/algorithm/kinematics-derivatives.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/algorithm/model.hxx" +#include "pinocchio/autodiff/casadi.hpp" +#include +#include +#include +#include + + +#include +#include + +// #include +#include + +#include +#include +#include +#include +#include + +#include +#include + + +class PinocchioRobotModel +{ + public: + PinocchioRobotModel(ros::NodeHandle nh, ros::NodeHandle nhp); + ~PinocchioRobotModel() = default; + + std::vector getRotorsOriginFromCog() {return rotor_origin_cog_;} + std::vector getRotorsNormalFromCog() {return rotor_normal_cog_;} + std::map getJointIndexMap() {return joint_index_map_;} + + + private: + std::string getRobotModelXml(const std::string param, ros::NodeHandle nh = ros::NodeHandle()); + + void modelInit(); + void kinematicsInit(); + void inertialInit(); + void rotorInit(); + + pinocchio::Model model_dbl_; + pinocchio::Data data_dbl_; + pinocchio::ModelTpl model_; + pinocchio::DataTpl data_; + + std::vector rotor_origin_root_; + std::vector rotor_origin_cog_; + std::vector rotor_normal_root_; + std::vector rotor_normal_cog_; + + casadi::SX q_cs_; + Eigen::Matrix q_; + casadi::DM q_dbl_; + + casadi::SX mass_; + casadi::SX inertia_; + pinocchio::SE3Tpl oMcog_; + + std::map joint_index_map_; + + int rotor_num_; + std::string baselink_; + + ros::NodeHandle nh_; + ros::NodeHandle nhp_; + ros::Subscriber joint_state_sub_; + void jointStateCallback(const sensor_msgs::JointStateConstPtr msg); + +}; + + +Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, Eigen::VectorXd x_dbl) +{ + casadi::DM ret = casadi::DM(y.size1(), y.size2()); + for(int i = 0; i < y.size1(); i++) + { + for(int j = 0; j < y.size2(); j++) + { + casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); + casadi::DM y_dbl = f(eigenVectorToCasadiDm(x_dbl)); + ret(i, j) = y_dbl; + } + } + return casadiDmToEigenMatrix(ret); +} + +Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, casadi::DM x_dbl) +{ + casadi::DM ret = casadi::DM(y.size1(), y.size2()); + for(int i = 0; i < y.size1(); i++) + { + for(int j = 0; j < y.size2(); j++) + { + casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); + casadi::DM y_dbl = f(x_dbl); + ret(i, j) = y_dbl; + } + } + return casadiDmToEigenMatrix(ret); +} + +std::string PinocchioRobotModel::getRobotModelXml(const std::string param, ros::NodeHandle nh) +{ + std::string xml_string; + + 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_string; + } + nh.getParam(param, xml_string); + return xml_string; +} diff --git a/robots/dragon/src/model/pinocchio_test.cpp b/robots/dragon/src/model/pinocchio_test.cpp index a79831c30..3cf0cdd29 100644 --- a/robots/dragon/src/model/pinocchio_test.cpp +++ b/robots/dragon/src/model/pinocchio_test.cpp @@ -1,129 +1,55 @@ -#include "pinocchio/fwd.hpp" - -#include "pinocchio/parsers/urdf.hpp" - -#include "pinocchio/algorithm/joint-configuration.hpp" -#include "pinocchio/algorithm/kinematics.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" -#include "pinocchio/algorithm/kinematics-derivatives.hpp" -#include "pinocchio/algorithm/center-of-mass.hxx" -#include "pinocchio/algorithm/model.hxx" -#include "pinocchio/autodiff/casadi.hpp" -#include -#include -// #include -#include - -#include -#include -#include -#include -#include - -#include - -// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here. -#ifndef PINOCCHIO_MODEL_DIR -#define PINOCCHIO_MODEL_DIR "/home/sugihara/ros/jsk_aerial_robot_ws/src/jsk_aerial_robot/robots/dragon/robots" -#endif - -class PinocchioRobotModel -{ - public: - PinocchioRobotModel(); - ~PinocchioRobotModel() = default; - - void modelInit(); - void kinematicsInit(); - void inertialInit(); - - pinocchio::Model model_dbl_; - pinocchio::Data data_dlb_; - pinocchio::ModelTpl model_; - pinocchio::DataTpl data_; - - std::vector rotor_origin_root_; - std::vector rotor_normal_root_; - - casadi::SX q_cs_; - Eigen::Matrix q_; - casadi::SX mass_; - casadi::SX cog_pos_; - casadi::SX inertia_; - - std::map joint_index_map_; - int rotor_num_; +#include "dragon/model/pinocchio_test.h" -}; - -PinocchioRobotModel::PinocchioRobotModel() +PinocchioRobotModel::PinocchioRobotModel(ros::NodeHandle nh, ros::NodeHandle nhp): + nh_(nh), + nhp_(nhp) { modelInit(); kinematicsInit(); inertialInit(); -} + rotorInit(); -Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, Eigen::VectorXd x_dbl) -{ - casadi::DM ret = casadi::DM(y.size1(), y.size2()); - for(int i = 0; i < y.size1(); i++) - { - for(int j = 0; j < y.size2(); j++) - { - casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); - casadi::DM y_dbl = f(eigenVectorToCasadiDm(x_dbl)); - ret(i, j) = y_dbl; - } - } - return casadiDmToEigenMatrix(ret); + joint_state_sub_ = nh_.subscribe("joint_states", 1, &PinocchioRobotModel::jointStateCallback, this); } -Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, casadi::DM x_dbl) -{ - casadi::DM ret = casadi::DM(y.size1(), y.size2()); - for(int i = 0; i < y.size1(); i++) - { - for(int j = 0; j < y.size2(); j++) - { - casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); - casadi::DM y_dbl = f(x_dbl); - ret(i, j) = y_dbl; - } - } - return casadiDmToEigenMatrix(ret); -} void PinocchioRobotModel::modelInit() { - // You should change here to set up your own URDF file or just pass it as an argument of this example. - const std::string urdf_filename = PINOCCHIO_MODEL_DIR + std::string("/quad/robot.urdf"); - - // Load the urdf model - pinocchio::urdf::buildModel(urdf_filename, pinocchio::JointModelFreeFlyer(), model_dbl_); - std::cout << "model name: " << model_dbl_.name << std::endl; + // Load the urdf model from ros parameter server + std::string robot_model_string = getRobotModelXml("robot_description"); + pinocchio::urdf::buildModelFromXML(robot_model_string, pinocchio::JointModelFreeFlyer(), model_dbl_); + ROS_WARN_STREAM("[model][pinocchio] model name: " << model_dbl_.name); // Create data required by the algorithms - pinocchio::Data data_dlb_(model_dbl_); + pinocchio::Data data_dbl_(model_dbl_); // declaraion of model and data with casadi model_ = model_dbl_.cast(); data_ = pinocchio::DataTpl(model_); - std::cout << "model_.nq: " << model_.nq << std::endl; - std::cout << "model_.nv: " << model_.nv << std::endl; - std::cout << "model_.njoints: " << model_.njoints << std::endl; - std::cout << std::endl; + // get baselink name from urdf + TiXmlDocument robot_model_xml; + robot_model_xml.Parse(robot_model_string.c_str()); + TiXmlElement* baselink_attr = robot_model_xml.FirstChildElement("robot")->FirstChildElement("baselink"); + if(!baselink_attr) + ROS_DEBUG("Can not get baselink attribute from urdf model"); + else + baselink_ = std::string(baselink_attr->Attribute("name")); + + ROS_WARN_STREAM("[model][pinocchio] model_.nq: " << model_.nq); + ROS_WARN_STREAM("[model][pinocchio] model_.nv: " << model_.nv); + ROS_WARN_STREAM("[model][pinocchio] model_.njoints: " << model_.njoints); + // make map for joint position and index std::vector q_dims(model_.njoints); for(int i = 0; i < model_.njoints; i++) { std::string joint_type = model_.joints[i].shortname(); - // std::cout << model_.names[i] << " " << joint_type << std::endl; - if(joint_type == "JointModelFreeFlyer") + if(joint_type == "JointModelFreeFlyer") // floating joint is expressed by seven variables in joint position space (position and quaternion) q_dims.at(i) = 7; - else if(joint_type == "JointModelRUBX" || joint_type == "JointModelRUBY" || joint_type == "JointModelRUBZ") + else if(joint_type == "JointModelRUBX" || joint_type == "JointModelRUBY" || joint_type == "JointModelRUBZ") // continuous joint is expressed by two variables in joint position space (cos and sin) q_dims.at(i) = 2; - else + else // revolute joint is expressed by one variable in joint position space q_dims.at(i) = 1; } @@ -131,12 +57,7 @@ void PinocchioRobotModel::modelInit() rotor_num_ = 0; for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) { - std::cout << model_.names[joint_id] << std::endl; - if(model_.names[joint_id] == "universe") - { - std::cout << "find joint named universe" << std::endl; - } - else + if(model_.names[joint_id] != "universe") { joint_index_map_[model_.names[joint_id]] = joint_index; joint_index += q_dims.at(joint_id); @@ -144,56 +65,110 @@ void PinocchioRobotModel::modelInit() // special process for rotor if(model_.names[joint_id].find("rotor") != std::string::npos) { - std::cout << "found rotor named " << model_.names[joint_id] << std::endl; rotor_num_++; } } } - std::cout << std::endl; - // std::cout << "rotor num: " << rotor_num_ << std::endl; - - // check the joint index map - std::cout << "joint index map: \n"; - for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) - { - if(joint_index_map_.count(model_.names[joint_id])) - { - std::cout << model_.names[joint_id] << " :" << joint_index_map_.at(model_.names[joint_id]) << std::endl; - } - } - std::cout << std::endl; } + void PinocchioRobotModel::kinematicsInit() { // init q q_cs_ = casadi::SX::sym("q", model_.nq); + q_dbl_ = casadi::DM(model_.nq, 1); q_.resize(model_.nq, 1); for(int i = 0; i < model_.nq; i++) { + q_dbl_(i) = 0.0; q_(i) = q_cs_(i); } - // std::cout << "q: " << q_.transpose() << std::endl; // solve FK pinocchio::forwardKinematics(model_, data_, q_); + pinocchio::updateFramePlacements(model_, data_); +} + + +void PinocchioRobotModel::inertialInit() +{ + // setup cog info + oMcog_.translation() = pinocchio::centerOfMass(model_, data_, q_, true); + oMcog_.rotation() = data_.oMf[model_.getFrameId(baselink_)].rotation(); + + mass_ = pinocchio::computeTotalMass(model_); + ROS_WARN_STREAM("[model][pinocchio] robot mass: " << mass_); + + + // TODO: inertia https://github.com/stack-of-tasks/pinocchio/issues/980 + // for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) + // std::cout << std::setw(24) << std::left + // << model_.names[joint_id] << ": " + // << std::setw(24) << std::left + // << model_.joints[joint_id].shortname() << ": " + // << std::fixed << std::setprecision(5) + // << model_.inertias[joint_id] + // << std::endl; + + std::cout << std::endl; + // pinocchio::computeCentroidalMap(model_, data_, q_); + // std::cout << pinocchio::computeCentroidalMap(model_, data_, q_) << std::endl; + // std::cout << pinocchio::computeCentroidalMap(model_, data_, q_).rows() << " " << pinocchio::computeCentroidalMap(model_, data_, q_).cols() << std::endl; + + pinocchio::crba(model_, data_, q_); + data_.M.triangularView() = data_.M.transpose().triangularView(); + data_.Ycrb[0] = data_.liMi[1].act(data_.Ycrb[1]); + + pinocchio::InertiaTpl Ig(oMcog_.inverse().act(data_.Ycrb[0])); + pinocchio::Symmetric3Tpl I = Ig.inertia(); + // inertia_ = I.matrix(); + inertia_ = casadi::SX(3, 3); + for(int i = 0; i < 3; i++) + { + for(int j = 0; j < 3; j++) + { + inertia_(i, j) = I.matrix()(i, j); + } + } + + // std::cout << I.matrix() << std::endl; + // std::cout << I.matrix().rows() << " " << I.matrix().cols() << std::endl; + + // std::cout << data_.hg << std::endl; + // std::cout << data_.Ag << std::endl; + // std::cout << data_.Ig << std::endl; + + +} - // setup rotor info +void PinocchioRobotModel::rotorInit() +{ + // get rotor origin and normal from root and cog rotor_origin_root_.resize(rotor_num_); + rotor_origin_cog_.resize(rotor_num_); rotor_normal_root_.resize(rotor_num_); + rotor_normal_cog_.resize(rotor_num_); for(int i = 0; i < rotor_num_; i++) { std::string rotor_name = "rotor" + std::to_string(i + 1); int joint_id = model_.getJointId(rotor_name); - // std::cout << rotor_name << " " << joint_id << std::endl; + + // origin casadi::SX rotor_origin_root = casadi::SX::zeros(3); + casadi::SX rotor_origin_cog = casadi::SX::zeros(3); for(int j = 0; j < 3; j++) + { rotor_origin_root(j) = data_.oMi[joint_id].translation()(j); + rotor_origin_cog(j) = (oMcog_.inverse() * data_.oMi[joint_id]).translation()(j); + } + rotor_origin_root_.at(i) = rotor_origin_root; - // std::cout << "rotor origin: " << i << " " << rotor_origin_root_.at(i) << std::endl; + rotor_origin_cog_.at(i) = rotor_origin_cog; + // normal casadi::SX rotor_normal_root = casadi::SX::zeros(3); + casadi::SX rotor_normal_cog = casadi::SX::zeros(3); int rotor_axis_type; if(model_.joints[joint_id].shortname() == "JointModelRX" || model_.joints[joint_id].shortname() == "JointModelRUBX") rotor_axis_type = 0; @@ -205,48 +180,46 @@ void PinocchioRobotModel::kinematicsInit() for(int j = 0; j < 3; j++) { rotor_normal_root(j) = data_.oMi[joint_id].rotation()(j, rotor_axis_type); + rotor_normal_cog(j) = (oMcog_.inverse() * data_.oMi[joint_id]).rotation()(j, rotor_axis_type); } - // std::cout << "rotor rotation: \n" << data_.oMi[joint_id].rotation() << std::endl; rotor_normal_root_.at(i) = rotor_normal_root; - // std::cout << "rotor axis for rotor" << i << ": " << rotor_axis_type << std::endl; - // std::cout << "rotor axis: " << i << " " << rotor_normal_root_.at(i) << std::endl; - // std::cout << std::endl; + rotor_normal_cog_.at(i) = rotor_normal_cog; } - - // Print out the placement of each joint of the kinematic tree - // for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) - // std::cout << std::setw(24) << std::left - // << model_.names[joint_id] << ": " - // << std::setw(24) << std::left - // << model_.joints[joint_id].shortname() << ": " - // << std::fixed << std::setprecision(5) - // << data_.oMi[joint_id].translation().transpose() - // << std::endl; } -void PinocchioRobotModel::inertialInit() +void PinocchioRobotModel::jointStateCallback(const sensor_msgs::JointStateConstPtr msg) { - mass_ = pinocchio::computeTotalMass(model_); - std::cout << "mass: " << mass_ << std::endl; + std::vector joint_names = msg->name; + std::vector joint_positions = msg->position; - auto cog_pos = pinocchio::centerOfMass(model_, data_, q_, true); - pinocchio::casadi::copy(cog_pos, cog_pos_); - // std::cout << "cog_pos_: "cog_pos_ << std::endl; - std::cout << std::endl; + for(int i = 0; i < joint_names.size(); i++) + { + q_dbl_(joint_index_map_[joint_names.at(i)]) = joint_positions.at(i); + } - casadi::DM q_test = casadi::DM(model_.nq, 1); - q_test(joint_index_map_["joint1_yaw"]) = 1.57; - q_test(joint_index_map_["joint2_yaw"]) = 1.57; - q_test(joint_index_map_["joint3_yaw"]) = 1.57; - std::cout << "q_test: " << q_test << std::endl; - std::cout << std::endl; - std::cout << "real cog: " << computeRealValue(cog_pos_, q_cs_, q_test).transpose() << std::endl; - std::cout << std::endl; + // std::cout << "real cog: " << computeRealValue(cog_pos_, q_cs_, q_dbl_).transpose() << std::endl; + // for(int i = 0; i < rotor_num_; i++) + // { + // std::cout << "origin " << i + 1 << ": " << computeRealValue(rotor_origin_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; + // std::cout << "normal " << i + 1 << ": " << computeRealValue(rotor_normal_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; + // } + // std::cout << std::endl; + // std::cout << computeRealValue(inertia_, q_cs_, q_dbl_) << std::endl; + // std::cout << std::endl; } + int main(int argc, char ** argv) { - PinocchioRobotModel pinocchio_robot_model; + ros::init (argc, argv, "pinocchio_robot_model"); + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + + PinocchioRobotModel* pinocchio_robot_model = new PinocchioRobotModel(nh, nh_private); + ros::spin(); + + delete pinocchio_robot_model; + return 0; // // zero configuration // Eigen::VectorXd q = randomConfiguration(model); From 114cf232d8cd209ee059de4abf7647e3db0b2b35 Mon Sep 17 00:00:00 2001 From: sugihara Date: Wed, 13 Mar 2024 23:05:30 +0900 Subject: [PATCH 07/27] [dragon] refactor pinocchio robot model node --- .../include/dragon/model/pinocchio_test.h | 13 +- robots/dragon/src/model/pinocchio_test.cpp | 168 ++---------------- 2 files changed, 25 insertions(+), 156 deletions(-) diff --git a/robots/dragon/include/dragon/model/pinocchio_test.h b/robots/dragon/include/dragon/model/pinocchio_test.h index 467188fcb..bd383ee98 100644 --- a/robots/dragon/include/dragon/model/pinocchio_test.h +++ b/robots/dragon/include/dragon/model/pinocchio_test.h @@ -41,8 +41,8 @@ class PinocchioRobotModel PinocchioRobotModel(ros::NodeHandle nh, ros::NodeHandle nhp); ~PinocchioRobotModel() = default; - std::vector getRotorsOriginFromCog() {return rotor_origin_cog_;} - std::vector getRotorsNormalFromCog() {return rotor_normal_cog_;} + std::vector getRotorsOriginFromCog() {return rotors_origin_cog_;} + std::vector getRotorsNormalFromCog() {return rotors_normal_cog_;} std::map getJointIndexMap() {return joint_index_map_;} @@ -59,10 +59,10 @@ class PinocchioRobotModel pinocchio::ModelTpl model_; pinocchio::DataTpl data_; - std::vector rotor_origin_root_; - std::vector rotor_origin_cog_; - std::vector rotor_normal_root_; - std::vector rotor_normal_cog_; + std::vector rotors_origin_root_; + std::vector rotors_origin_cog_; + std::vector rotors_normal_root_; + std::vector rotors_normal_cog_; casadi::SX q_cs_; Eigen::Matrix q_; @@ -71,6 +71,7 @@ class PinocchioRobotModel casadi::SX mass_; casadi::SX inertia_; pinocchio::SE3Tpl oMcog_; + casadi::SX opcog_; std::map joint_index_map_; diff --git a/robots/dragon/src/model/pinocchio_test.cpp b/robots/dragon/src/model/pinocchio_test.cpp index 3cf0cdd29..23170cc90 100644 --- a/robots/dragon/src/model/pinocchio_test.cpp +++ b/robots/dragon/src/model/pinocchio_test.cpp @@ -92,62 +92,32 @@ void PinocchioRobotModel::kinematicsInit() void PinocchioRobotModel::inertialInit() { - // setup cog info + // set cog frame oMcog_.translation() = pinocchio::centerOfMass(model_, data_, q_, true); oMcog_.rotation() = data_.oMf[model_.getFrameId(baselink_)].rotation(); + pinocchio::casadi::copy(oMcog_.translation(), opcog_); + // get mass (caution: type of this variable is casadi::SX) mass_ = pinocchio::computeTotalMass(model_); ROS_WARN_STREAM("[model][pinocchio] robot mass: " << mass_); - - // TODO: inertia https://github.com/stack-of-tasks/pinocchio/issues/980 - // for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) - // std::cout << std::setw(24) << std::left - // << model_.names[joint_id] << ": " - // << std::setw(24) << std::left - // << model_.joints[joint_id].shortname() << ": " - // << std::fixed << std::setprecision(5) - // << model_.inertias[joint_id] - // << std::endl; - - std::cout << std::endl; - // pinocchio::computeCentroidalMap(model_, data_, q_); - // std::cout << pinocchio::computeCentroidalMap(model_, data_, q_) << std::endl; - // std::cout << pinocchio::computeCentroidalMap(model_, data_, q_).rows() << " " << pinocchio::computeCentroidalMap(model_, data_, q_).cols() << std::endl; - - pinocchio::crba(model_, data_, q_); + // get inertia matrix expressed in cog frame. (Hint: pinocchio/unittest/centroidal.cpp) + pinocchio::crba(model_, data_, q_); // Composite Rigid Body Algorithm data_.M.triangularView() = data_.M.transpose().triangularView(); data_.Ycrb[0] = data_.liMi[1].act(data_.Ycrb[1]); pinocchio::InertiaTpl Ig(oMcog_.inverse().act(data_.Ycrb[0])); pinocchio::Symmetric3Tpl I = Ig.inertia(); - // inertia_ = I.matrix(); - inertia_ = casadi::SX(3, 3); - for(int i = 0; i < 3; i++) - { - for(int j = 0; j < 3; j++) - { - inertia_(i, j) = I.matrix()(i, j); - } - } - - // std::cout << I.matrix() << std::endl; - // std::cout << I.matrix().rows() << " " << I.matrix().cols() << std::endl; - - // std::cout << data_.hg << std::endl; - // std::cout << data_.Ag << std::endl; - // std::cout << data_.Ig << std::endl; - - + pinocchio::casadi::copy(I.matrix(), inertia_); } void PinocchioRobotModel::rotorInit() { - // get rotor origin and normal from root and cog - rotor_origin_root_.resize(rotor_num_); - rotor_origin_cog_.resize(rotor_num_); - rotor_normal_root_.resize(rotor_num_); - rotor_normal_cog_.resize(rotor_num_); + // get rotor origin and normal from root and cog + rotors_origin_root_.resize(rotor_num_); + rotors_origin_cog_.resize(rotor_num_); + rotors_normal_root_.resize(rotor_num_); + rotors_normal_cog_.resize(rotor_num_); for(int i = 0; i < rotor_num_; i++) { @@ -155,16 +125,8 @@ void PinocchioRobotModel::rotorInit() int joint_id = model_.getJointId(rotor_name); // origin - casadi::SX rotor_origin_root = casadi::SX::zeros(3); - casadi::SX rotor_origin_cog = casadi::SX::zeros(3); - for(int j = 0; j < 3; j++) - { - rotor_origin_root(j) = data_.oMi[joint_id].translation()(j); - rotor_origin_cog(j) = (oMcog_.inverse() * data_.oMi[joint_id]).translation()(j); - } - - rotor_origin_root_.at(i) = rotor_origin_root; - rotor_origin_cog_.at(i) = rotor_origin_cog; + pinocchio::casadi::copy(data_.oMi[joint_id].translation(), rotors_origin_root_.at(i)); + pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).translation(), rotors_origin_cog_.at(i)); // normal casadi::SX rotor_normal_root = casadi::SX::zeros(3); @@ -177,13 +139,8 @@ void PinocchioRobotModel::rotorInit() else if(model_.joints[joint_id].shortname() == "JointModelRZ" || model_.joints[joint_id].shortname() == "JointModelRUBZ") rotor_axis_type = 2; - for(int j = 0; j < 3; j++) - { - rotor_normal_root(j) = data_.oMi[joint_id].rotation()(j, rotor_axis_type); - rotor_normal_cog(j) = (oMcog_.inverse() * data_.oMi[joint_id]).rotation()(j, rotor_axis_type); - } - rotor_normal_root_.at(i) = rotor_normal_root; - rotor_normal_cog_.at(i) = rotor_normal_cog; + pinocchio::casadi::copy(data_.oMi[joint_id].rotation().middleCols(rotor_axis_type, 1), rotors_normal_root_.at(i)); + pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).rotation().middleCols(rotor_axis_type, 1), rotors_normal_cog_.at(i)); } } @@ -197,11 +154,11 @@ void PinocchioRobotModel::jointStateCallback(const sensor_msgs::JointStateConstP q_dbl_(joint_index_map_[joint_names.at(i)]) = joint_positions.at(i); } - // std::cout << "real cog: " << computeRealValue(cog_pos_, q_cs_, q_dbl_).transpose() << std::endl; + // std::cout << "real cog: " << computeRealValue(opcog_, q_cs_, q_dbl_).transpose() << std::endl; // for(int i = 0; i < rotor_num_; i++) // { - // std::cout << "origin " << i + 1 << ": " << computeRealValue(rotor_origin_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; - // std::cout << "normal " << i + 1 << ": " << computeRealValue(rotor_normal_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; + // std::cout << "origin " << i + 1 << ": " << computeRealValue(rotors_origin_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; + // std::cout << "normal " << i + 1 << ": " << computeRealValue(rotors_normal_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; // } // std::cout << std::endl; // std::cout << computeRealValue(inertia_, q_cs_, q_dbl_) << std::endl; @@ -220,93 +177,4 @@ int main(int argc, char ** argv) delete pinocchio_robot_model; return 0; - - // // zero configuration - // Eigen::VectorXd q = randomConfiguration(model); - // q = Eigen::VectorXd::Zero(q.rows()); - // std::cout << "q: " << q.transpose() << std::endl; - - // // joint space with casadi - // auto q_cs = casadi::SX::sym("q", model.nq); - // Eigen::Matrix q_casadi; - // q_casadi.resize(model.nq, 1); - // for(int i = 0; i < model.nq; i++) - // q_casadi(i) = q_cs(i); - - // // Perform the forward kinematics over the kinematic tree - // pinocchio::forwardKinematics(model, data, q); - - // // Print out the placement of each joint of the kinematic tree - // for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model.njoints; ++joint_id) - // std::cout << std::setw(24) << std::left - // << model.names[joint_id] << ": " - // << std::setw(15) << std::left - // << model.joints[joint_id].shortname() << ": " - // << std::fixed << std::setprecision(5) - // << data.oMi[joint_id].translation().transpose() - // << std::endl; - - // std::cout << std::endl; - // std::cout << std::endl; - - // std::cout << "q_casadi: " << q_casadi.transpose() << std::endl; - // for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_casadi.njoints; ++joint_id) - // std::cout << std::setw(24) << std::left - // << model_casadi.names[joint_id] << ": " - // << std::setw(15) << std::left - // << model_casadi.joints[joint_id].shortname() << ": " - // << std::fixed << std::setprecision(5) - // << data_casadi.oMi[joint_id].translation().transpose() - // << std::endl; - - // std::cout << std::endl; - - // // check the position of each joint - // casadi::DM casadi_dbl = casadi::DM({0, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}); - // std::cout << "casadi_dbl: " << casadi_dbl << std::endl; - // for(int i = 0; i < model.nv; i++) - // { - // for(int j = 0; j < 3; j++) - // { - // casadi::Function f = casadi::Function("f", {q_cs}, {data_casadi.oMi[i].translation()(j)}); - // casadi::DM pos_j = f(casadi_dbl); - // std::cout << pos_j << " "; - // } - // std::cout << std::endl; - // } - // std::cout << std::endl; - - // // check jacobian - // pinocchio::computeJointJacobians(model, data, q); - // std::cout << "data.J: \n" << data.J << std::endl; - // std::cout << std::endl; - - // pinocchio::computeJointJacobians(model_casadi, data_casadi, q_casadi); - // std::cout << "data.J: \n" << data_casadi.J << std::endl; - // std::cout << std::endl; - - // // kinematics derivatives - // Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); - // Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); - - // // Computes the kinematics derivatives for all the joints of the robot - // pinocchio::computeForwardKinematicsDerivatives(model, data, q, v, a); - - // // Retrieve the kinematics derivatives of a specific joint, expressed in the LOCAL frame of the joints. - // pinocchio::JointIndex joint_id = (pinocchio::JointIndex)(model.njoints-1); - // pinocchio::Data::Matrix6x v_partial_dq(6, model.nv), a_partial_dq(6, model.nv), a_partial_dv(6, model.nv), a_partial_da(6, model.nv); - // v_partial_dq.setZero(); - // a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero(); - // pinocchio::getJointAccelerationDerivatives(model, data, joint_id, pinocchio::LOCAL, v_partial_dq, - // a_partial_dq, a_partial_dv, a_partial_da); - - // std::cout << "v partial dq: \n" << v_partial_dq << std::endl; - // std::cout << std::endl; - // // Remark: we are not directly computing the quantity v_partial_dv as it is also equal to a_partial_da. - - // // But we can also expressed the same quantities in the frame centered on the end-effector joint, but expressed in the axis aligned with the world frame. - // pinocchio::getJointAccelerationDerivatives(model,data, joint_id, pinocchio::WORLD, v_partial_dq, - // a_partial_dq, a_partial_dv, a_partial_da); - // std::cout << "v partial dq: \n" << v_partial_dq << std::endl; - } From 3365e128f5a85b81f23a0998df9bce4f148e9a30 Mon Sep 17 00:00:00 2001 From: sugihara Date: Thu, 14 Mar 2024 17:50:14 +0900 Subject: [PATCH 08/27] [aerial_robot_model] move pinocchio node to aerial_robot_model package --- aerial_robot_model/CMakeLists.txt | 8 + .../model/pinocchio_test.h | 0 aerial_robot_model/package.xml | 4 + .../src/model/base_model}/pinocchio_test.cpp | 2 +- robots/dragon/CMakeLists.txt | 8 - robots/dragon/package.xml | 4 - robots/dragon/robots/quad/robot.urdf | 930 ------------------ 7 files changed, 13 insertions(+), 943 deletions(-) rename {robots/dragon/include/dragon => aerial_robot_model/include/aerial_robot_model}/model/pinocchio_test.h (100%) rename {robots/dragon/src/model => aerial_robot_model/src/model/base_model}/pinocchio_test.cpp (99%) delete mode 100644 robots/dragon/robots/quad/robot.urdf diff --git a/aerial_robot_model/CMakeLists.txt b/aerial_robot_model/CMakeLists.txt index 43babfb41..31e2b2cab 100644 --- a/aerial_robot_model/CMakeLists.txt +++ b/aerial_robot_model/CMakeLists.txt @@ -7,6 +7,7 @@ add_compile_options(-std=c++17) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + casadi_eigen eigen_conversions geometry_msgs interactive_markers @@ -32,6 +33,10 @@ find_package(orocos_kdl REQUIRED) find_package(urdfdom_headers REQUIRED) find_package(Eigen3 REQUIRED) +set(CMAKE_CXX_STANDARD 17) +find_package(pinocchio REQUIRED) +find_package(casadi REQUIRED) + include_directories(${orocos_kdl_INCLUDE_DIRS} ${urdfdom_headers_INCLUDE_DIRS}) if(NOT CMAKE_BUILD_TYPE) @@ -92,6 +97,9 @@ add_library(robot_model_pluginlib src/model/plugin/underactuated_tilted_robot_model.cpp) target_link_libraries(robot_model_pluginlib ${catkin_LIBRARIES} aerial_robot_model) +add_executable(pinocchio_test src/model/base_model/pinocchio_test.cpp) +target_link_libraries(pinocchio_test ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${pinocchio_LIBRARIES} casadi) + add_library(servo_bridge src/servo_bridge/servo_bridge.cpp) target_link_libraries(servo_bridge ${catkin_LIBRARIES}) add_executable(servo_bridge_node src/servo_bridge/servo_bridge_node.cpp) diff --git a/robots/dragon/include/dragon/model/pinocchio_test.h b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_test.h similarity index 100% rename from robots/dragon/include/dragon/model/pinocchio_test.h rename to aerial_robot_model/include/aerial_robot_model/model/pinocchio_test.h diff --git a/aerial_robot_model/package.xml b/aerial_robot_model/package.xml index b8006e441..6fbb7f39f 100644 --- a/aerial_robot_model/package.xml +++ b/aerial_robot_model/package.xml @@ -12,6 +12,8 @@ catkin eigen_conversions + casadi + casadi_eigen geometry_msgs interactive_markers kalman_filter @@ -32,6 +34,8 @@ urdf visualization_msgs + casadi + casadi_eigen eigen_conversions geometry_msgs interactive_markers diff --git a/robots/dragon/src/model/pinocchio_test.cpp b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp similarity index 99% rename from robots/dragon/src/model/pinocchio_test.cpp rename to aerial_robot_model/src/model/base_model/pinocchio_test.cpp index 23170cc90..f2bed50a6 100644 --- a/robots/dragon/src/model/pinocchio_test.cpp +++ b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp @@ -1,4 +1,4 @@ -#include "dragon/model/pinocchio_test.h" +#include "aerial_robot_model/model/pinocchio_test.h" PinocchioRobotModel::PinocchioRobotModel(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh), diff --git a/robots/dragon/CMakeLists.txt b/robots/dragon/CMakeLists.txt index 7e68accd7..3a6239f8f 100644 --- a/robots/dragon/CMakeLists.txt +++ b/robots/dragon/CMakeLists.txt @@ -22,10 +22,6 @@ set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O3 -g -DNDEBUG") find_package(NLopt REQUIRED) -set(CMAKE_CXX_STANDARD 17) -find_package(pinocchio REQUIRED) -find_package(casadi REQUIRED) - catkin_package( INCLUDE_DIRS include test LIBRARIES dragon_robot_model dragon_aerial_robot_controllib dragon_navigation dragon_numerical_jacobians @@ -59,10 +55,6 @@ add_dependencies(dragon_aerial_robot_controllib aerial_robot_msgs_generate_messa add_library(dragon_navigation src/dragon_navigation.cpp) target_link_libraries(dragon_navigation ${catkin_LIBRARIES}) -add_executable(pinocchio_test src/model/pinocchio_test.cpp) -target_link_libraries(pinocchio_test ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${pinocchio_LIBRARIES} casadi) - - # test # pre-build test code add_library(dragon_numerical_jacobians test/dragon/numerical_jacobians.cpp) diff --git a/robots/dragon/package.xml b/robots/dragon/package.xml index 13200a31a..00e274943 100644 --- a/robots/dragon/package.xml +++ b/robots/dragon/package.xml @@ -14,8 +14,6 @@ aerial_robot_control aerial_robot_model aerial_robot_msgs - casadi - casadi_eigen roscpp hydrus mujoco_ros_control @@ -28,8 +26,6 @@ aerial_robot_estimation aerial_robot_simulation aerial_robot_base - casadi - casadi_eigen roscpp hydrus pinocchio diff --git a/robots/dragon/robots/quad/robot.urdf b/robots/dragon/robots/quad/robot.urdf deleted file mode 100644 index 57bad22b8..000000000 --- a/robots/dragon/robots/quad/robot.urdf +++ /dev/null @@ -1,930 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.00 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.00 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.2 - 0.2 - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - RotorInterface - - - RotorInterface - 1 - - - - 0.4 - 0.4 - - - - - - - - - - - - - - - - - - - - - - - - - - 0.00 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.00 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.2 - 0.2 - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - RotorInterface - - - RotorInterface - 1 - - - - 0.4 - 0.4 - - - - - - - - - - - - - - - - - - - - - - - - - - 0.00 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.00 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.2 - 0.2 - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - RotorInterface - - - RotorInterface - 1 - - - - 0.4 - 0.4 - - - - - - - - - - - - - - - - - - - - - - - - - - 0.00 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.00 - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - RotorInterface - - - RotorInterface - 1 - - - - 0.4 - 0.4 - - - From 1ffb60b686ac1f0ec550020fb81809d918f8a2c4 Mon Sep 17 00:00:00 2001 From: sugihara Date: Thu, 14 Mar 2024 17:56:38 +0900 Subject: [PATCH 09/27] [aerial_robot_model][pinocchio] fux only indent --- .../aerial_robot_model/model/pinocchio_test.h | 32 +++--- .../src/model/base_model/pinocchio_test.cpp | 108 +++++++++--------- 2 files changed, 69 insertions(+), 71 deletions(-) diff --git a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_test.h b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_test.h index bd383ee98..1a7f84dc8 100644 --- a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_test.h +++ b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_test.h @@ -37,7 +37,7 @@ class PinocchioRobotModel { - public: +public: PinocchioRobotModel(ros::NodeHandle nh, ros::NodeHandle nhp); ~PinocchioRobotModel() = default; @@ -45,8 +45,7 @@ class PinocchioRobotModel std::vector getRotorsNormalFromCog() {return rotors_normal_cog_;} std::map getJointIndexMap() {return joint_index_map_;} - - private: +private: std::string getRobotModelXml(const std::string param, ros::NodeHandle nh = ros::NodeHandle()); void modelInit(); @@ -82,7 +81,6 @@ class PinocchioRobotModel ros::NodeHandle nhp_; ros::Subscriber joint_state_sub_; void jointStateCallback(const sensor_msgs::JointStateConstPtr msg); - }; @@ -90,14 +88,14 @@ Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, Eigen::VectorXd x_d { casadi::DM ret = casadi::DM(y.size1(), y.size2()); for(int i = 0; i < y.size1(); i++) - { - for(int j = 0; j < y.size2(); j++) { - casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); - casadi::DM y_dbl = f(eigenVectorToCasadiDm(x_dbl)); - ret(i, j) = y_dbl; + for(int j = 0; j < y.size2(); j++) + { + casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); + casadi::DM y_dbl = f(eigenVectorToCasadiDm(x_dbl)); + ret(i, j) = y_dbl; + } } - } return casadiDmToEigenMatrix(ret); } @@ -105,14 +103,14 @@ Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, casadi::DM x_dbl) { casadi::DM ret = casadi::DM(y.size1(), y.size2()); for(int i = 0; i < y.size1(); i++) - { - for(int j = 0; j < y.size2(); j++) { - casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); - casadi::DM y_dbl = f(x_dbl); - ret(i, j) = y_dbl; + for(int j = 0; j < y.size2(); j++) + { + casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); + casadi::DM y_dbl = f(x_dbl); + ret(i, j) = y_dbl; + } } - } return casadiDmToEigenMatrix(ret); } @@ -120,7 +118,7 @@ std::string PinocchioRobotModel::getRobotModelXml(const std::string param, ros:: { std::string xml_string; - if (!nh.hasParam(param)) + 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_string; diff --git a/aerial_robot_model/src/model/base_model/pinocchio_test.cpp b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp index f2bed50a6..bb2fbc23b 100644 --- a/aerial_robot_model/src/model/base_model/pinocchio_test.cpp +++ b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp @@ -43,32 +43,32 @@ void PinocchioRobotModel::modelInit() // make map for joint position and index std::vector q_dims(model_.njoints); for(int i = 0; i < model_.njoints; i++) - { - std::string joint_type = model_.joints[i].shortname(); - if(joint_type == "JointModelFreeFlyer") // floating joint is expressed by seven variables in joint position space (position and quaternion) - q_dims.at(i) = 7; - else if(joint_type == "JointModelRUBX" || joint_type == "JointModelRUBY" || joint_type == "JointModelRUBZ") // continuous joint is expressed by two variables in joint position space (cos and sin) - q_dims.at(i) = 2; - else // revolute joint is expressed by one variable in joint position space - q_dims.at(i) = 1; - } + { + std::string joint_type = model_.joints[i].shortname(); + if(joint_type == "JointModelFreeFlyer") // floating joint is expressed by seven variables in joint position space (position and quaternion) + q_dims.at(i) = 7; + else if(joint_type == "JointModelRUBX" || joint_type == "JointModelRUBY" || joint_type == "JointModelRUBZ") // continuous joint is expressed by two variables in joint position space (cos and sin) + q_dims.at(i) = 2; + else // revolute joint is expressed by one variable in joint position space + q_dims.at(i) = 1; + } int joint_index = 0; rotor_num_ = 0; for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) - { - if(model_.names[joint_id] != "universe") { - joint_index_map_[model_.names[joint_id]] = joint_index; - joint_index += q_dims.at(joint_id); - - // special process for rotor - if(model_.names[joint_id].find("rotor") != std::string::npos) - { - rotor_num_++; - } + if(model_.names[joint_id] != "universe") + { + joint_index_map_[model_.names[joint_id]] = joint_index; + joint_index += q_dims.at(joint_id); + + // special process for rotor + if(model_.names[joint_id].find("rotor") != std::string::npos) + { + rotor_num_++; + } + } } - } } @@ -79,10 +79,10 @@ void PinocchioRobotModel::kinematicsInit() q_dbl_ = casadi::DM(model_.nq, 1); q_.resize(model_.nq, 1); for(int i = 0; i < model_.nq; i++) - { - q_dbl_(i) = 0.0; - q_(i) = q_cs_(i); - } + { + q_dbl_(i) = 0.0; + q_(i) = q_cs_(i); + } // solve FK pinocchio::forwardKinematics(model_, data_, q_); @@ -92,7 +92,7 @@ void PinocchioRobotModel::kinematicsInit() void PinocchioRobotModel::inertialInit() { - // set cog frame + // set cog frame oMcog_.translation() = pinocchio::centerOfMass(model_, data_, q_, true); oMcog_.rotation() = data_.oMf[model_.getFrameId(baselink_)].rotation(); pinocchio::casadi::copy(oMcog_.translation(), opcog_); @@ -120,28 +120,28 @@ void PinocchioRobotModel::rotorInit() rotors_normal_cog_.resize(rotor_num_); for(int i = 0; i < rotor_num_; i++) - { - std::string rotor_name = "rotor" + std::to_string(i + 1); - int joint_id = model_.getJointId(rotor_name); - - // origin - pinocchio::casadi::copy(data_.oMi[joint_id].translation(), rotors_origin_root_.at(i)); - pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).translation(), rotors_origin_cog_.at(i)); - - // normal - casadi::SX rotor_normal_root = casadi::SX::zeros(3); - casadi::SX rotor_normal_cog = casadi::SX::zeros(3); - int rotor_axis_type; - if(model_.joints[joint_id].shortname() == "JointModelRX" || model_.joints[joint_id].shortname() == "JointModelRUBX") - rotor_axis_type = 0; - else if(model_.joints[joint_id].shortname() == "JointModelRY" || model_.joints[joint_id].shortname() == "JointModelRUBY") - rotor_axis_type = 1; - else if(model_.joints[joint_id].shortname() == "JointModelRZ" || model_.joints[joint_id].shortname() == "JointModelRUBZ") - rotor_axis_type = 2; - - pinocchio::casadi::copy(data_.oMi[joint_id].rotation().middleCols(rotor_axis_type, 1), rotors_normal_root_.at(i)); - pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).rotation().middleCols(rotor_axis_type, 1), rotors_normal_cog_.at(i)); - } + { + std::string rotor_name = "rotor" + std::to_string(i + 1); + int joint_id = model_.getJointId(rotor_name); + + // origin + pinocchio::casadi::copy(data_.oMi[joint_id].translation(), rotors_origin_root_.at(i)); + pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).translation(), rotors_origin_cog_.at(i)); + + // normal + casadi::SX rotor_normal_root = casadi::SX::zeros(3); + casadi::SX rotor_normal_cog = casadi::SX::zeros(3); + int rotor_axis_type; + if(model_.joints[joint_id].shortname() == "JointModelRX" || model_.joints[joint_id].shortname() == "JointModelRUBX") + rotor_axis_type = 0; + else if(model_.joints[joint_id].shortname() == "JointModelRY" || model_.joints[joint_id].shortname() == "JointModelRUBY") + rotor_axis_type = 1; + else if(model_.joints[joint_id].shortname() == "JointModelRZ" || model_.joints[joint_id].shortname() == "JointModelRUBZ") + rotor_axis_type = 2; + + pinocchio::casadi::copy(data_.oMi[joint_id].rotation().middleCols(rotor_axis_type, 1), rotors_normal_root_.at(i)); + pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).rotation().middleCols(rotor_axis_type, 1), rotors_normal_cog_.at(i)); + } } void PinocchioRobotModel::jointStateCallback(const sensor_msgs::JointStateConstPtr msg) @@ -150,16 +150,16 @@ void PinocchioRobotModel::jointStateCallback(const sensor_msgs::JointStateConstP std::vector joint_positions = msg->position; for(int i = 0; i < joint_names.size(); i++) - { - q_dbl_(joint_index_map_[joint_names.at(i)]) = joint_positions.at(i); - } + { + q_dbl_(joint_index_map_[joint_names.at(i)]) = joint_positions.at(i); + } // std::cout << "real cog: " << computeRealValue(opcog_, q_cs_, q_dbl_).transpose() << std::endl; // for(int i = 0; i < rotor_num_; i++) - // { - // std::cout << "origin " << i + 1 << ": " << computeRealValue(rotors_origin_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; - // std::cout << "normal " << i + 1 << ": " << computeRealValue(rotors_normal_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; - // } + // { + // std::cout << "origin " << i + 1 << ": " << computeRealValue(rotors_origin_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; + // std::cout << "normal " << i + 1 << ": " << computeRealValue(rotors_normal_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; + // } // std::cout << std::endl; // std::cout << computeRealValue(inertia_, q_cs_, q_dbl_) << std::endl; // std::cout << std::endl; From 7084b4cef1c54f31903c033b63b77c5d35a4df7c Mon Sep 17 00:00:00 2001 From: sugihara Date: Thu, 14 Mar 2024 18:44:21 +0900 Subject: [PATCH 10/27] [aerial_robot_model][pinocchio] fix bug: add joint type for rotor JointModelRevoluteUnboundedUnaligned --- .../src/model/base_model/pinocchio_test.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/aerial_robot_model/src/model/base_model/pinocchio_test.cpp b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp index bb2fbc23b..2d77596c5 100644 --- a/aerial_robot_model/src/model/base_model/pinocchio_test.cpp +++ b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp @@ -47,7 +47,7 @@ void PinocchioRobotModel::modelInit() std::string joint_type = model_.joints[i].shortname(); if(joint_type == "JointModelFreeFlyer") // floating joint is expressed by seven variables in joint position space (position and quaternion) q_dims.at(i) = 7; - else if(joint_type == "JointModelRUBX" || joint_type == "JointModelRUBY" || joint_type == "JointModelRUBZ") // continuous joint is expressed by two variables in joint position space (cos and sin) + else if(joint_type == "JointModelRUBX" || joint_type == "JointModelRUBY" || joint_type == "JointModelRUBZ" || joint_type == "JointModelRevoluteUnboundedUnaligned") // continuous joint is expressed by two variables in joint position space (cos and sin) q_dims.at(i) = 2; else // revolute joint is expressed by one variable in joint position space q_dims.at(i) = 1; @@ -69,6 +69,9 @@ void PinocchioRobotModel::modelInit() } } } + + for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) + ROS_WARN_STREAM("[model][pinocchio] " << std::setw(24) << std::left << model_.names[joint_id] << ": " << std::setw(24) << std::left << model_.joints[joint_id].shortname()); } @@ -131,12 +134,12 @@ void PinocchioRobotModel::rotorInit() // normal casadi::SX rotor_normal_root = casadi::SX::zeros(3); casadi::SX rotor_normal_cog = casadi::SX::zeros(3); - int rotor_axis_type; + int rotor_axis_type = 0; if(model_.joints[joint_id].shortname() == "JointModelRX" || model_.joints[joint_id].shortname() == "JointModelRUBX") rotor_axis_type = 0; else if(model_.joints[joint_id].shortname() == "JointModelRY" || model_.joints[joint_id].shortname() == "JointModelRUBY") rotor_axis_type = 1; - else if(model_.joints[joint_id].shortname() == "JointModelRZ" || model_.joints[joint_id].shortname() == "JointModelRUBZ") + else if(model_.joints[joint_id].shortname() == "JointModelRZ" || model_.joints[joint_id].shortname() == "JointModelRUBZ" || model_.joints[joint_id].shortname() == "JointModelRevoluteUnboundedUnaligned") // hard coded for JointModelRevoluteUnboundedUnaligned rotor_axis_type = 2; pinocchio::casadi::copy(data_.oMi[joint_id].rotation().middleCols(rotor_axis_type, 1), rotors_normal_root_.at(i)); From 6696c24178d05992ed76be57137bb0ebbd8b2d1a Mon Sep 17 00:00:00 2001 From: sugihara Date: Thu, 14 Mar 2024 18:55:30 +0900 Subject: [PATCH 11/27] [aerial_robot_model][pinochio] rosout joint and frame info --- aerial_robot_model/src/model/base_model/pinocchio_test.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/aerial_robot_model/src/model/base_model/pinocchio_test.cpp b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp index 2d77596c5..04f1af7f9 100644 --- a/aerial_robot_model/src/model/base_model/pinocchio_test.cpp +++ b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp @@ -71,7 +71,9 @@ void PinocchioRobotModel::modelInit() } for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) - ROS_WARN_STREAM("[model][pinocchio] " << std::setw(24) << std::left << model_.names[joint_id] << ": " << std::setw(24) << std::left << model_.joints[joint_id].shortname()); + ROS_WARN_STREAM("[model][pinocchio] joint " << std::setw(4) << joint_id << ": " << std::setw(24) << std::left << model_.names[joint_id] << ": " << std::setw(24) << std::left << model_.joints[joint_id].shortname()); + for(int i = 0; i < model_.frames.size(); i++) + ROS_WARN_STREAM("[model][pinocchio] frame " << std::setw(4) << i << ": " << std::setw(24) << std::left << model_.frames[i].name); } From 2304e51c462d44f5592ddfa69f5a0c87ac1f0b92 Mon Sep 17 00:00:00 2001 From: sugihara Date: Thu, 14 Mar 2024 18:57:11 +0900 Subject: [PATCH 12/27] [aerial_robot_model][pinocchio] debug for CI --- aerial_robot_model/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aerial_robot_model/CMakeLists.txt b/aerial_robot_model/CMakeLists.txt index 31e2b2cab..9b9d1e060 100644 --- a/aerial_robot_model/CMakeLists.txt +++ b/aerial_robot_model/CMakeLists.txt @@ -123,7 +123,7 @@ install(TARGETS aerial_robot_model aerial_robot_model_ros robot_model_pluginlib DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) -install(TARGETS rotor_tf_publisher interactive_marker_tf_broadcaster servo_bridge_node +install(TARGETS rotor_tf_publisher interactive_marker_tf_broadcaster servo_bridge_node pinocchio_test DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) From 7554d11d7e98775225753d11acffe8ef4d35a895 Mon Sep 17 00:00:00 2001 From: sugihara Date: Thu, 14 Mar 2024 19:00:54 +0900 Subject: [PATCH 13/27] [aerial_robot_model][pinocchio] fix dependency --- aerial_robot_model/package.xml | 2 ++ robots/dragon/CMakeLists.txt | 1 - robots/dragon/package.xml | 2 -- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/aerial_robot_model/package.xml b/aerial_robot_model/package.xml index 6fbb7f39f..137c9f9fe 100644 --- a/aerial_robot_model/package.xml +++ b/aerial_robot_model/package.xml @@ -20,6 +20,7 @@ kdl_parser liburdfdom-headers-dev message_generation + pinocchio pluginlib sensor_msgs spinal @@ -43,6 +44,7 @@ kalman_filter kdl_parser message_runtime + pinocchio pluginlib robot_state_publisher rviz diff --git a/robots/dragon/CMakeLists.txt b/robots/dragon/CMakeLists.txt index 3a6239f8f..897e5a5f9 100644 --- a/robots/dragon/CMakeLists.txt +++ b/robots/dragon/CMakeLists.txt @@ -7,7 +7,6 @@ find_package(catkin REQUIRED COMPONENTS aerial_robot_control aerial_robot_model aerial_robot_msgs - casadi_eigen hydrus mujoco_ros_control pluginlib diff --git a/robots/dragon/package.xml b/robots/dragon/package.xml index 00e274943..b490b9033 100644 --- a/robots/dragon/package.xml +++ b/robots/dragon/package.xml @@ -17,7 +17,6 @@ roscpp hydrus mujoco_ros_control - pinocchio pluginlib aerial_robot_control @@ -28,7 +27,6 @@ aerial_robot_base roscpp hydrus - pinocchio pluginlib rostest From 8ea44af566ee40eec2d916210acd375ef02f5bb4 Mon Sep 17 00:00:00 2001 From: sugihara Date: Thu, 14 Mar 2024 19:08:34 +0900 Subject: [PATCH 14/27] [aerial_robot_model] fix package xml --- aerial_robot_model/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aerial_robot_model/package.xml b/aerial_robot_model/package.xml index 137c9f9fe..6a8ffeb84 100644 --- a/aerial_robot_model/package.xml +++ b/aerial_robot_model/package.xml @@ -20,7 +20,7 @@ kdl_parser liburdfdom-headers-dev message_generation - pinocchio + pinocchio pluginlib sensor_msgs spinal From b2b0207a4ca803effdba12a43dd0bae12108bd6d Mon Sep 17 00:00:00 2001 From: sugikazu75 Date: Sat, 16 Mar 2024 14:46:23 +0900 Subject: [PATCH 15/27] [aerial_robot_estimation] specify the namespace of array to avoid ambiguous error --- .../include/aerial_robot_estimation/state_estimation.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 a67a4ada8..fde5b9e7b 100644 --- a/aerial_robot_estimation/include/aerial_robot_estimation/state_estimation.h +++ b/aerial_robot_estimation/include/aerial_robot_estimation/state_estimation.h @@ -453,7 +453,7 @@ namespace aerial_robot_estimation std::string tf_prefix_; /* 9: x_w, y_w, z_w, roll_w, pitch_w, yaw_cog_w, x_b, y_b, yaw_board_w */ - array state_; + std::array state_; /* for calculate the sensor to baselink with the consideration of time delay */ int qu_size_; @@ -464,7 +464,7 @@ namespace aerial_robot_estimation /* sensor fusion */ boost::shared_ptr< pluginlib::ClassLoader > sensor_fusion_loader_ptr_; bool sensor_fusion_flag_; - array fuser_; //0: egomotion; 1: experiment + std::array fuser_; //0: egomotion; 1: experiment /* sensor (un)health level */ uint8_t unhealth_level_; From 6db783fa938d16ec565f32206abef4258d584df3 Mon Sep 17 00:00:00 2001 From: sugikazu75 Date: Sat, 16 Mar 2024 14:48:35 +0900 Subject: [PATCH 16/27] [aerial_robot_model] fix CmakeList: depend on casadi_eigen and separate pinocchio model library and execution node --- aerial_robot_model/CMakeLists.txt | 8 +- .../base_model/pinocchio_robot_model.cpp | 172 +++++++++++++++++ .../src/model/base_model/pinocchio_test.cpp | 173 +----------------- 3 files changed, 178 insertions(+), 175 deletions(-) create mode 100644 aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp diff --git a/aerial_robot_model/CMakeLists.txt b/aerial_robot_model/CMakeLists.txt index 9b9d1e060..2d233a646 100644 --- a/aerial_robot_model/CMakeLists.txt +++ b/aerial_robot_model/CMakeLists.txt @@ -57,8 +57,8 @@ generate_messages( catkin_package( INCLUDE_DIRS include test - 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 + LIBRARIES aerial_robot_model aerial_robot_model_ros numerical_jacobians pinocchio_robot_model robot_model_pluginlib + CATKIN_DEPENDS casadi_eigen 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 ) @@ -97,8 +97,10 @@ add_library(robot_model_pluginlib src/model/plugin/underactuated_tilted_robot_model.cpp) target_link_libraries(robot_model_pluginlib ${catkin_LIBRARIES} aerial_robot_model) +add_library(pinocchio_robot_model src/model/base_model/pinocchio_robot_model.cpp) +target_link_libraries(pinocchio_robot_model ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${pinocchio_LIBRARIES} casadi) add_executable(pinocchio_test src/model/base_model/pinocchio_test.cpp) -target_link_libraries(pinocchio_test ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${pinocchio_LIBRARIES} casadi) +target_link_libraries(pinocchio_test ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${pinocchio_LIBRARIES} casadi pinocchio_robot_model) add_library(servo_bridge src/servo_bridge/servo_bridge.cpp) target_link_libraries(servo_bridge ${catkin_LIBRARIES}) diff --git a/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp new file mode 100644 index 000000000..52631e672 --- /dev/null +++ b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp @@ -0,0 +1,172 @@ +#include + +PinocchioRobotModel::PinocchioRobotModel(ros::NodeHandle nh, ros::NodeHandle nhp): + nh_(nh), + nhp_(nhp) +{ + modelInit(); + kinematicsInit(); + inertialInit(); + rotorInit(); + + joint_state_sub_ = nh_.subscribe("joint_states", 1, &PinocchioRobotModel::jointStateCallback, this); +} + + +void PinocchioRobotModel::modelInit() +{ + // Load the urdf model from ros parameter server + std::string robot_model_string = getRobotModelXml("robot_description"); + pinocchio::urdf::buildModelFromXML(robot_model_string, pinocchio::JointModelFreeFlyer(), model_dbl_); + ROS_WARN_STREAM("[model][pinocchio] model name: " << model_dbl_.name); + + // Create data required by the algorithms + pinocchio::Data data_dbl_(model_dbl_); + + // declaraion of model and data with casadi + model_ = model_dbl_.cast(); + data_ = pinocchio::DataTpl(model_); + + // get baselink name from urdf + TiXmlDocument robot_model_xml; + robot_model_xml.Parse(robot_model_string.c_str()); + TiXmlElement* baselink_attr = robot_model_xml.FirstChildElement("robot")->FirstChildElement("baselink"); + if(!baselink_attr) + ROS_DEBUG("Can not get baselink attribute from urdf model"); + else + baselink_ = std::string(baselink_attr->Attribute("name")); + + ROS_WARN_STREAM("[model][pinocchio] model_.nq: " << model_.nq); + ROS_WARN_STREAM("[model][pinocchio] model_.nv: " << model_.nv); + ROS_WARN_STREAM("[model][pinocchio] model_.njoints: " << model_.njoints); + + // make map for joint position and index + std::vector q_dims(model_.njoints); + for(int i = 0; i < model_.njoints; i++) + { + std::string joint_type = model_.joints[i].shortname(); + if(joint_type == "JointModelFreeFlyer") // floating joint is expressed by seven variables in joint position space (position and quaternion) + q_dims.at(i) = 7; + else if(joint_type == "JointModelRUBX" || joint_type == "JointModelRUBY" || joint_type == "JointModelRUBZ" || joint_type == "JointModelRevoluteUnboundedUnaligned") // continuous joint is expressed by two variables in joint position space (cos and sin) + q_dims.at(i) = 2; + else // revolute joint is expressed by one variable in joint position space + q_dims.at(i) = 1; + } + + int joint_index = 0; + rotor_num_ = 0; + for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) + { + if(model_.names[joint_id] != "universe") + { + joint_index_map_[model_.names[joint_id]] = joint_index; + joint_index += q_dims.at(joint_id); + + // special process for rotor + if(model_.names[joint_id].find("rotor") != std::string::npos) + { + rotor_num_++; + } + } + } + + for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) + ROS_WARN_STREAM("[model][pinocchio] joint " << std::setw(4) << joint_id << ": " << std::setw(24) << std::left << model_.names[joint_id] << ": " << std::setw(24) << std::left << model_.joints[joint_id].shortname()); + for(int i = 0; i < model_.frames.size(); i++) + ROS_WARN_STREAM("[model][pinocchio] frame " << std::setw(4) << i << ": " << std::setw(24) << std::left << model_.frames[i].name); +} + + +void PinocchioRobotModel::kinematicsInit() +{ + // init q + q_cs_ = casadi::SX::sym("q", model_.nq); + q_dbl_ = casadi::DM(model_.nq, 1); + q_.resize(model_.nq, 1); + for(int i = 0; i < model_.nq; i++) + { + q_dbl_(i) = 0.0; + q_(i) = q_cs_(i); + } + + // solve FK + pinocchio::forwardKinematics(model_, data_, q_); + pinocchio::updateFramePlacements(model_, data_); +} + + +void PinocchioRobotModel::inertialInit() +{ + // set cog frame + oMcog_.translation() = pinocchio::centerOfMass(model_, data_, q_, true); + oMcog_.rotation() = data_.oMf[model_.getFrameId(baselink_)].rotation(); + pinocchio::casadi::copy(oMcog_.translation(), opcog_); + + // get mass (caution: type of this variable is casadi::SX) + mass_ = pinocchio::computeTotalMass(model_); + ROS_WARN_STREAM("[model][pinocchio] robot mass: " << mass_); + + // get inertia matrix expressed in cog frame. (Hint: pinocchio/unittest/centroidal.cpp) + pinocchio::crba(model_, data_, q_); // Composite Rigid Body Algorithm + data_.M.triangularView() = data_.M.transpose().triangularView(); + data_.Ycrb[0] = data_.liMi[1].act(data_.Ycrb[1]); + + pinocchio::InertiaTpl Ig(oMcog_.inverse().act(data_.Ycrb[0])); + pinocchio::Symmetric3Tpl I = Ig.inertia(); + pinocchio::casadi::copy(I.matrix(), inertia_); +} + +void PinocchioRobotModel::rotorInit() +{ + // get rotor origin and normal from root and cog + rotors_origin_root_.resize(rotor_num_); + rotors_origin_cog_.resize(rotor_num_); + rotors_normal_root_.resize(rotor_num_); + rotors_normal_cog_.resize(rotor_num_); + + for(int i = 0; i < rotor_num_; i++) + { + std::string rotor_name = "rotor" + std::to_string(i + 1); + int joint_id = model_.getJointId(rotor_name); + + // origin + pinocchio::casadi::copy(data_.oMi[joint_id].translation(), rotors_origin_root_.at(i)); + pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).translation(), rotors_origin_cog_.at(i)); + + // normal + casadi::SX rotor_normal_root = casadi::SX::zeros(3); + casadi::SX rotor_normal_cog = casadi::SX::zeros(3); + int rotor_axis_type = 0; + if(model_.joints[joint_id].shortname() == "JointModelRX" || model_.joints[joint_id].shortname() == "JointModelRUBX") + rotor_axis_type = 0; + else if(model_.joints[joint_id].shortname() == "JointModelRY" || model_.joints[joint_id].shortname() == "JointModelRUBY") + rotor_axis_type = 1; + else if(model_.joints[joint_id].shortname() == "JointModelRZ" || model_.joints[joint_id].shortname() == "JointModelRUBZ" || model_.joints[joint_id].shortname() == "JointModelRevoluteUnboundedUnaligned") // hard coded for JointModelRevoluteUnboundedUnaligned + rotor_axis_type = 2; + + pinocchio::casadi::copy(data_.oMi[joint_id].rotation().middleCols(rotor_axis_type, 1), rotors_normal_root_.at(i)); + pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).rotation().middleCols(rotor_axis_type, 1), rotors_normal_cog_.at(i)); + } +} + +void PinocchioRobotModel::jointStateCallback(const sensor_msgs::JointStateConstPtr msg) +{ + std::vector joint_names = msg->name; + std::vector joint_positions = msg->position; + + for(int i = 0; i < joint_names.size(); i++) + { + q_dbl_(joint_index_map_[joint_names.at(i)]) = joint_positions.at(i); + } + + // std::cout << "real cog: " << computeRealValue(opcog_, q_cs_, q_dbl_).transpose() << std::endl; + // for(int i = 0; i < rotor_num_; i++) + // { + // std::cout << "origin " << i + 1 << ": " << computeRealValue(rotors_origin_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; + // std::cout << "normal " << i + 1 << ": " << computeRealValue(rotors_normal_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; + // } + // std::cout << std::endl; + // std::cout << computeRealValue(inertia_, q_cs_, q_dbl_) << std::endl; + // std::cout << std::endl; +} + diff --git a/aerial_robot_model/src/model/base_model/pinocchio_test.cpp b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp index 04f1af7f9..128e1a710 100644 --- a/aerial_robot_model/src/model/base_model/pinocchio_test.cpp +++ b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp @@ -1,175 +1,4 @@ -#include "aerial_robot_model/model/pinocchio_test.h" - -PinocchioRobotModel::PinocchioRobotModel(ros::NodeHandle nh, ros::NodeHandle nhp): - nh_(nh), - nhp_(nhp) -{ - modelInit(); - kinematicsInit(); - inertialInit(); - rotorInit(); - - joint_state_sub_ = nh_.subscribe("joint_states", 1, &PinocchioRobotModel::jointStateCallback, this); -} - - -void PinocchioRobotModel::modelInit() -{ - // Load the urdf model from ros parameter server - std::string robot_model_string = getRobotModelXml("robot_description"); - pinocchio::urdf::buildModelFromXML(robot_model_string, pinocchio::JointModelFreeFlyer(), model_dbl_); - ROS_WARN_STREAM("[model][pinocchio] model name: " << model_dbl_.name); - - // Create data required by the algorithms - pinocchio::Data data_dbl_(model_dbl_); - - // declaraion of model and data with casadi - model_ = model_dbl_.cast(); - data_ = pinocchio::DataTpl(model_); - - // get baselink name from urdf - TiXmlDocument robot_model_xml; - robot_model_xml.Parse(robot_model_string.c_str()); - TiXmlElement* baselink_attr = robot_model_xml.FirstChildElement("robot")->FirstChildElement("baselink"); - if(!baselink_attr) - ROS_DEBUG("Can not get baselink attribute from urdf model"); - else - baselink_ = std::string(baselink_attr->Attribute("name")); - - ROS_WARN_STREAM("[model][pinocchio] model_.nq: " << model_.nq); - ROS_WARN_STREAM("[model][pinocchio] model_.nv: " << model_.nv); - ROS_WARN_STREAM("[model][pinocchio] model_.njoints: " << model_.njoints); - - // make map for joint position and index - std::vector q_dims(model_.njoints); - for(int i = 0; i < model_.njoints; i++) - { - std::string joint_type = model_.joints[i].shortname(); - if(joint_type == "JointModelFreeFlyer") // floating joint is expressed by seven variables in joint position space (position and quaternion) - q_dims.at(i) = 7; - else if(joint_type == "JointModelRUBX" || joint_type == "JointModelRUBY" || joint_type == "JointModelRUBZ" || joint_type == "JointModelRevoluteUnboundedUnaligned") // continuous joint is expressed by two variables in joint position space (cos and sin) - q_dims.at(i) = 2; - else // revolute joint is expressed by one variable in joint position space - q_dims.at(i) = 1; - } - - int joint_index = 0; - rotor_num_ = 0; - for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) - { - if(model_.names[joint_id] != "universe") - { - joint_index_map_[model_.names[joint_id]] = joint_index; - joint_index += q_dims.at(joint_id); - - // special process for rotor - if(model_.names[joint_id].find("rotor") != std::string::npos) - { - rotor_num_++; - } - } - } - - for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) - ROS_WARN_STREAM("[model][pinocchio] joint " << std::setw(4) << joint_id << ": " << std::setw(24) << std::left << model_.names[joint_id] << ": " << std::setw(24) << std::left << model_.joints[joint_id].shortname()); - for(int i = 0; i < model_.frames.size(); i++) - ROS_WARN_STREAM("[model][pinocchio] frame " << std::setw(4) << i << ": " << std::setw(24) << std::left << model_.frames[i].name); -} - - -void PinocchioRobotModel::kinematicsInit() -{ - // init q - q_cs_ = casadi::SX::sym("q", model_.nq); - q_dbl_ = casadi::DM(model_.nq, 1); - q_.resize(model_.nq, 1); - for(int i = 0; i < model_.nq; i++) - { - q_dbl_(i) = 0.0; - q_(i) = q_cs_(i); - } - - // solve FK - pinocchio::forwardKinematics(model_, data_, q_); - pinocchio::updateFramePlacements(model_, data_); -} - - -void PinocchioRobotModel::inertialInit() -{ - // set cog frame - oMcog_.translation() = pinocchio::centerOfMass(model_, data_, q_, true); - oMcog_.rotation() = data_.oMf[model_.getFrameId(baselink_)].rotation(); - pinocchio::casadi::copy(oMcog_.translation(), opcog_); - - // get mass (caution: type of this variable is casadi::SX) - mass_ = pinocchio::computeTotalMass(model_); - ROS_WARN_STREAM("[model][pinocchio] robot mass: " << mass_); - - // get inertia matrix expressed in cog frame. (Hint: pinocchio/unittest/centroidal.cpp) - pinocchio::crba(model_, data_, q_); // Composite Rigid Body Algorithm - data_.M.triangularView() = data_.M.transpose().triangularView(); - data_.Ycrb[0] = data_.liMi[1].act(data_.Ycrb[1]); - - pinocchio::InertiaTpl Ig(oMcog_.inverse().act(data_.Ycrb[0])); - pinocchio::Symmetric3Tpl I = Ig.inertia(); - pinocchio::casadi::copy(I.matrix(), inertia_); -} - -void PinocchioRobotModel::rotorInit() -{ - // get rotor origin and normal from root and cog - rotors_origin_root_.resize(rotor_num_); - rotors_origin_cog_.resize(rotor_num_); - rotors_normal_root_.resize(rotor_num_); - rotors_normal_cog_.resize(rotor_num_); - - for(int i = 0; i < rotor_num_; i++) - { - std::string rotor_name = "rotor" + std::to_string(i + 1); - int joint_id = model_.getJointId(rotor_name); - - // origin - pinocchio::casadi::copy(data_.oMi[joint_id].translation(), rotors_origin_root_.at(i)); - pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).translation(), rotors_origin_cog_.at(i)); - - // normal - casadi::SX rotor_normal_root = casadi::SX::zeros(3); - casadi::SX rotor_normal_cog = casadi::SX::zeros(3); - int rotor_axis_type = 0; - if(model_.joints[joint_id].shortname() == "JointModelRX" || model_.joints[joint_id].shortname() == "JointModelRUBX") - rotor_axis_type = 0; - else if(model_.joints[joint_id].shortname() == "JointModelRY" || model_.joints[joint_id].shortname() == "JointModelRUBY") - rotor_axis_type = 1; - else if(model_.joints[joint_id].shortname() == "JointModelRZ" || model_.joints[joint_id].shortname() == "JointModelRUBZ" || model_.joints[joint_id].shortname() == "JointModelRevoluteUnboundedUnaligned") // hard coded for JointModelRevoluteUnboundedUnaligned - rotor_axis_type = 2; - - pinocchio::casadi::copy(data_.oMi[joint_id].rotation().middleCols(rotor_axis_type, 1), rotors_normal_root_.at(i)); - pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).rotation().middleCols(rotor_axis_type, 1), rotors_normal_cog_.at(i)); - } -} - -void PinocchioRobotModel::jointStateCallback(const sensor_msgs::JointStateConstPtr msg) -{ - std::vector joint_names = msg->name; - std::vector joint_positions = msg->position; - - for(int i = 0; i < joint_names.size(); i++) - { - q_dbl_(joint_index_map_[joint_names.at(i)]) = joint_positions.at(i); - } - - // std::cout << "real cog: " << computeRealValue(opcog_, q_cs_, q_dbl_).transpose() << std::endl; - // for(int i = 0; i < rotor_num_; i++) - // { - // std::cout << "origin " << i + 1 << ": " << computeRealValue(rotors_origin_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; - // std::cout << "normal " << i + 1 << ": " << computeRealValue(rotors_normal_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; - // } - // std::cout << std::endl; - // std::cout << computeRealValue(inertia_, q_cs_, q_dbl_) << std::endl; - // std::cout << std::endl; -} - +#include int main(int argc, char ** argv) { From 0dd87eb17fd262686747fdf4190936255e7e14f8 Mon Sep 17 00:00:00 2001 From: sugihara Date: Sun, 17 Mar 2024 00:41:05 +0900 Subject: [PATCH 17/27] [aerial_robot_model][pinocchio] rename header file --- .../model/{pinocchio_test.h => pinocchio_robot_model.h} | 0 .../src/model/base_model/pinocchio_robot_model.cpp | 2 +- aerial_robot_model/src/model/base_model/pinocchio_test.cpp | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) rename aerial_robot_model/include/aerial_robot_model/model/{pinocchio_test.h => pinocchio_robot_model.h} (100%) diff --git a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_test.h b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h similarity index 100% rename from aerial_robot_model/include/aerial_robot_model/model/pinocchio_test.h rename to aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h diff --git a/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp index 52631e672..d8a9ed648 100644 --- a/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp +++ b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp @@ -1,4 +1,4 @@ -#include +#include PinocchioRobotModel::PinocchioRobotModel(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh), diff --git a/aerial_robot_model/src/model/base_model/pinocchio_test.cpp b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp index 128e1a710..6bafd2ce6 100644 --- a/aerial_robot_model/src/model/base_model/pinocchio_test.cpp +++ b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp @@ -1,4 +1,4 @@ -#include +#include int main(int argc, char ** argv) { From 41c97c29b2b650ccd8c63015f22810f3ee6c1734 Mon Sep 17 00:00:00 2001 From: sugihara Date: Sun, 17 Mar 2024 00:44:03 +0900 Subject: [PATCH 18/27] [dragon] fix Cmakelist --- robots/dragon/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/robots/dragon/CMakeLists.txt b/robots/dragon/CMakeLists.txt index 897e5a5f9..ddfa2b1f2 100644 --- a/robots/dragon/CMakeLists.txt +++ b/robots/dragon/CMakeLists.txt @@ -11,7 +11,7 @@ find_package(catkin REQUIRED COMPONENTS mujoco_ros_control pluginlib roscpp - ) +) find_package(Eigen3 REQUIRED) if(NOT CMAKE_BUILD_TYPE) @@ -54,6 +54,7 @@ add_dependencies(dragon_aerial_robot_controllib aerial_robot_msgs_generate_messa add_library(dragon_navigation src/dragon_navigation.cpp) target_link_libraries(dragon_navigation ${catkin_LIBRARIES}) + # test # pre-build test code add_library(dragon_numerical_jacobians test/dragon/numerical_jacobians.cpp) From cd0224c339de7ea5f55be279268ce580e7a2e749 Mon Sep 17 00:00:00 2001 From: sugihara Date: Sun, 17 Mar 2024 00:47:41 +0900 Subject: [PATCH 19/27] [aerial_robot_model] fix CmakeList for linter --- aerial_robot_model/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aerial_robot_model/CMakeLists.txt b/aerial_robot_model/CMakeLists.txt index 2d233a646..0684beb3d 100644 --- a/aerial_robot_model/CMakeLists.txt +++ b/aerial_robot_model/CMakeLists.txt @@ -121,7 +121,7 @@ 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 aerial_robot_model aerial_robot_model_ros robot_model_pluginlib servo_bridge numerical_jacobians +install(TARGETS aerial_robot_model aerial_robot_model_ros pinocchio_robot_model robot_model_pluginlib servo_bridge numerical_jacobians DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) From d2e4fc9feb88b17c304339da0e9fbb38af3f7a0b Mon Sep 17 00:00:00 2001 From: sugihara Date: Sun, 17 Mar 2024 11:10:11 +0900 Subject: [PATCH 20/27] [aerial_robot_model][pinocchio] separate pinocchio robot model class and ros wrapper which load base class as plugin --- aerial_robot_model/CMakeLists.txt | 8 +- .../model/pinocchio_robot_model.h | 106 +++--- .../model/pinocchio_robot_model_ros.h | 23 ++ .../base_model/pinocchio_robot_model.cpp | 350 +++++++++--------- .../base_model/pinocchio_robot_model_ros.cpp | 36 ++ .../src/model/base_model/pinocchio_test.cpp | 6 +- 6 files changed, 296 insertions(+), 233 deletions(-) create mode 100644 aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h create mode 100644 aerial_robot_model/src/model/base_model/pinocchio_robot_model_ros.cpp diff --git a/aerial_robot_model/CMakeLists.txt b/aerial_robot_model/CMakeLists.txt index 0684beb3d..8a70892e3 100644 --- a/aerial_robot_model/CMakeLists.txt +++ b/aerial_robot_model/CMakeLists.txt @@ -57,7 +57,7 @@ generate_messages( catkin_package( INCLUDE_DIRS include test - LIBRARIES aerial_robot_model aerial_robot_model_ros numerical_jacobians pinocchio_robot_model robot_model_pluginlib + LIBRARIES aerial_robot_model aerial_robot_model_ros numerical_jacobians pinocchio_robot_model pinocchio_robot_model_ros robot_model_pluginlib CATKIN_DEPENDS casadi_eigen 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 ) @@ -99,8 +99,10 @@ target_link_libraries(robot_model_pluginlib ${catkin_LIBRARIES} aerial_robot_mod add_library(pinocchio_robot_model src/model/base_model/pinocchio_robot_model.cpp) target_link_libraries(pinocchio_robot_model ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${pinocchio_LIBRARIES} casadi) +add_library(pinocchio_robot_model_ros src/model/base_model/pinocchio_robot_model_ros.cpp) +target_link_libraries(pinocchio_robot_model_ros ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${pinocchio_LIBRARIES} pinocchio_robot_model) add_executable(pinocchio_test src/model/base_model/pinocchio_test.cpp) -target_link_libraries(pinocchio_test ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${pinocchio_LIBRARIES} casadi pinocchio_robot_model) +target_link_libraries(pinocchio_test ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${pinocchio_LIBRARIES} pinocchio_robot_model_ros) add_library(servo_bridge src/servo_bridge/servo_bridge.cpp) target_link_libraries(servo_bridge ${catkin_LIBRARIES}) @@ -121,7 +123,7 @@ 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 aerial_robot_model aerial_robot_model_ros pinocchio_robot_model robot_model_pluginlib servo_bridge numerical_jacobians +install(TARGETS aerial_robot_model aerial_robot_model_ros pinocchio_robot_model pinocchio_robot_model_ros robot_model_pluginlib servo_bridge numerical_jacobians DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) diff --git a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h index 1a7f84dc8..fd9ececfa 100644 --- a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h +++ b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h @@ -35,53 +35,52 @@ #include -class PinocchioRobotModel -{ -public: - PinocchioRobotModel(ros::NodeHandle nh, ros::NodeHandle nhp); - ~PinocchioRobotModel() = default; - - std::vector getRotorsOriginFromCog() {return rotors_origin_cog_;} - std::vector getRotorsNormalFromCog() {return rotors_normal_cog_;} - std::map getJointIndexMap() {return joint_index_map_;} - -private: - std::string getRobotModelXml(const std::string param, ros::NodeHandle nh = ros::NodeHandle()); - - void modelInit(); - void kinematicsInit(); - void inertialInit(); - void rotorInit(); - - pinocchio::Model model_dbl_; - pinocchio::Data data_dbl_; - pinocchio::ModelTpl model_; - pinocchio::DataTpl data_; - - std::vector rotors_origin_root_; - std::vector rotors_origin_cog_; - std::vector rotors_normal_root_; - std::vector rotors_normal_cog_; - - casadi::SX q_cs_; - Eigen::Matrix q_; - casadi::DM q_dbl_; - - casadi::SX mass_; - casadi::SX inertia_; - pinocchio::SE3Tpl oMcog_; - casadi::SX opcog_; - - std::map joint_index_map_; - - int rotor_num_; - std::string baselink_; - - ros::NodeHandle nh_; - ros::NodeHandle nhp_; - ros::Subscriber joint_state_sub_; - void jointStateCallback(const sensor_msgs::JointStateConstPtr msg); -}; +namespace aerial_robot_model { + class PinocchioRobotModel + { + public: + PinocchioRobotModel(); + ~PinocchioRobotModel() = default; + + std::vector getRotorsOriginFromCog() {return rotors_origin_cog_;} + std::vector getRotorsNormalFromCog() {return rotors_normal_cog_;} + std::map getJointIndexMap() {return joint_index_map_;} + void updateRobotModel(const sensor_msgs::JointState& state); + void updateRobotModelImpl(const sensor_msgs::JointState& state); + + private: + std::string getRobotModelXml(const std::string param, ros::NodeHandle nh = ros::NodeHandle()); + + void modelInit(); + void kinematicsInit(); + void inertialInit(); + void rotorInit(); + + pinocchio::Model model_dbl_; + pinocchio::Data data_dbl_; + pinocchio::ModelTpl model_; + pinocchio::DataTpl data_; + + std::vector rotors_origin_root_; + std::vector rotors_origin_cog_; + std::vector rotors_normal_root_; + std::vector rotors_normal_cog_; + + casadi::SX q_cs_; + Eigen::Matrix q_; + casadi::DM q_dbl_; + + casadi::SX mass_; + casadi::SX inertia_; + pinocchio::SE3Tpl oMcog_; + casadi::SX opcog_; + + std::map joint_index_map_; + + int rotor_num_; + std::string baselink_; + }; +} Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, Eigen::VectorXd x_dbl) @@ -113,16 +112,3 @@ Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, casadi::DM x_dbl) } return casadiDmToEigenMatrix(ret); } - -std::string PinocchioRobotModel::getRobotModelXml(const std::string param, ros::NodeHandle nh) -{ - std::string xml_string; - - 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_string; - } - nh.getParam(param, xml_string); - return xml_string; -} diff --git a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h new file mode 100644 index 000000000..fb6853dc3 --- /dev/null +++ b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h @@ -0,0 +1,23 @@ +#pragma once + +#include +#include + +namespace aerial_robot_model { + class PinocchioRobotModelRos { + public: + PinocchioRobotModelRos(ros::NodeHandle nh, ros::NodeHandle nhp); + virtual ~PinocchioRobotModelRos() = default; + + private: + ros::NodeHandle nh_; + ros::NodeHandle nhp_; + ros::Subscriber joint_state_sub_; + + pluginlib::ClassLoader pinocchio_robot_model_loader_; + boost::shared_ptr pinocchio_robot_model_; + + void jointStateCallback(const sensor_msgs::JointStateConstPtr& msg); + }; + +} diff --git a/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp index d8a9ed648..ba54bf482 100644 --- a/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp +++ b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp @@ -1,172 +1,188 @@ #include -PinocchioRobotModel::PinocchioRobotModel(ros::NodeHandle nh, ros::NodeHandle nhp): - nh_(nh), - nhp_(nhp) -{ - modelInit(); - kinematicsInit(); - inertialInit(); - rotorInit(); - - joint_state_sub_ = nh_.subscribe("joint_states", 1, &PinocchioRobotModel::jointStateCallback, this); -} - - -void PinocchioRobotModel::modelInit() -{ - // Load the urdf model from ros parameter server - std::string robot_model_string = getRobotModelXml("robot_description"); - pinocchio::urdf::buildModelFromXML(robot_model_string, pinocchio::JointModelFreeFlyer(), model_dbl_); - ROS_WARN_STREAM("[model][pinocchio] model name: " << model_dbl_.name); - - // Create data required by the algorithms - pinocchio::Data data_dbl_(model_dbl_); - - // declaraion of model and data with casadi - model_ = model_dbl_.cast(); - data_ = pinocchio::DataTpl(model_); - - // get baselink name from urdf - TiXmlDocument robot_model_xml; - robot_model_xml.Parse(robot_model_string.c_str()); - TiXmlElement* baselink_attr = robot_model_xml.FirstChildElement("robot")->FirstChildElement("baselink"); - if(!baselink_attr) - ROS_DEBUG("Can not get baselink attribute from urdf model"); - else - baselink_ = std::string(baselink_attr->Attribute("name")); - - ROS_WARN_STREAM("[model][pinocchio] model_.nq: " << model_.nq); - ROS_WARN_STREAM("[model][pinocchio] model_.nv: " << model_.nv); - ROS_WARN_STREAM("[model][pinocchio] model_.njoints: " << model_.njoints); - - // make map for joint position and index - std::vector q_dims(model_.njoints); - for(int i = 0; i < model_.njoints; i++) - { - std::string joint_type = model_.joints[i].shortname(); - if(joint_type == "JointModelFreeFlyer") // floating joint is expressed by seven variables in joint position space (position and quaternion) - q_dims.at(i) = 7; - else if(joint_type == "JointModelRUBX" || joint_type == "JointModelRUBY" || joint_type == "JointModelRUBZ" || joint_type == "JointModelRevoluteUnboundedUnaligned") // continuous joint is expressed by two variables in joint position space (cos and sin) - q_dims.at(i) = 2; - else // revolute joint is expressed by one variable in joint position space - q_dims.at(i) = 1; - } - - int joint_index = 0; - rotor_num_ = 0; - for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) - { - if(model_.names[joint_id] != "universe") - { - joint_index_map_[model_.names[joint_id]] = joint_index; - joint_index += q_dims.at(joint_id); - - // special process for rotor - if(model_.names[joint_id].find("rotor") != std::string::npos) - { - rotor_num_++; - } - } - } - - for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) - ROS_WARN_STREAM("[model][pinocchio] joint " << std::setw(4) << joint_id << ": " << std::setw(24) << std::left << model_.names[joint_id] << ": " << std::setw(24) << std::left << model_.joints[joint_id].shortname()); - for(int i = 0; i < model_.frames.size(); i++) - ROS_WARN_STREAM("[model][pinocchio] frame " << std::setw(4) << i << ": " << std::setw(24) << std::left << model_.frames[i].name); -} - - -void PinocchioRobotModel::kinematicsInit() -{ - // init q - q_cs_ = casadi::SX::sym("q", model_.nq); - q_dbl_ = casadi::DM(model_.nq, 1); - q_.resize(model_.nq, 1); - for(int i = 0; i < model_.nq; i++) +namespace aerial_robot_model { + PinocchioRobotModel::PinocchioRobotModel() + { + modelInit(); + kinematicsInit(); + inertialInit(); + rotorInit(); + } + + + void PinocchioRobotModel::modelInit() + { + // Load the urdf model from ros parameter server + std::string robot_model_string = getRobotModelXml("robot_description"); + pinocchio::urdf::buildModelFromXML(robot_model_string, pinocchio::JointModelFreeFlyer(), model_dbl_); + ROS_WARN_STREAM("[model][pinocchio] model name: " << model_dbl_.name); + + // Create data required by the algorithms + pinocchio::Data data_dbl_(model_dbl_); + + // declaraion of model and data with casadi + model_ = model_dbl_.cast(); + data_ = pinocchio::DataTpl(model_); + + // get baselink name from urdf + TiXmlDocument robot_model_xml; + robot_model_xml.Parse(robot_model_string.c_str()); + TiXmlElement* baselink_attr = robot_model_xml.FirstChildElement("robot")->FirstChildElement("baselink"); + if(!baselink_attr) + ROS_DEBUG("Can not get baselink attribute from urdf model"); + else + baselink_ = std::string(baselink_attr->Attribute("name")); + + ROS_WARN_STREAM("[model][pinocchio] model_.nq: " << model_.nq); + ROS_WARN_STREAM("[model][pinocchio] model_.nv: " << model_.nv); + ROS_WARN_STREAM("[model][pinocchio] model_.njoints: " << model_.njoints); + + // make map for joint position and index + std::vector q_dims(model_.njoints); + for(int i = 0; i < model_.njoints; i++) + { + std::string joint_type = model_.joints[i].shortname(); + if(joint_type == "JointModelFreeFlyer") // floating joint is expressed by seven variables in joint position space (position and quaternion) + q_dims.at(i) = 7; + else if(joint_type == "JointModelRUBX" || joint_type == "JointModelRUBY" || joint_type == "JointModelRUBZ" || joint_type == "JointModelRevoluteUnboundedUnaligned") // continuous joint is expressed by two variables in joint position space (cos and sin) + q_dims.at(i) = 2; + else // revolute joint is expressed by one variable in joint position space + q_dims.at(i) = 1; + } + + int joint_index = 0; + rotor_num_ = 0; + for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) + { + if(model_.names[joint_id] != "universe") + { + joint_index_map_[model_.names[joint_id]] = joint_index; + joint_index += q_dims.at(joint_id); + + // special process for rotor + if(model_.names[joint_id].find("rotor") != std::string::npos) + { + rotor_num_++; + } + } + } + + for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) + ROS_WARN_STREAM("[model][pinocchio] joint " << std::setw(4) << joint_id << ": " << std::setw(24) << std::left << model_.names[joint_id] << ": " << std::setw(24) << std::left << model_.joints[joint_id].shortname()); + for(int i = 0; i < model_.frames.size(); i++) + ROS_WARN_STREAM("[model][pinocchio] frame " << std::setw(4) << i << ": " << std::setw(24) << std::left << model_.frames[i].name); + } + + + void PinocchioRobotModel::kinematicsInit() + { + // init q + q_cs_ = casadi::SX::sym("q", model_.nq); + q_dbl_ = casadi::DM(model_.nq, 1); + q_.resize(model_.nq, 1); + for(int i = 0; i < model_.nq; i++) + { + q_dbl_(i) = 0.0; + q_(i) = q_cs_(i); + } + + // solve FK + pinocchio::forwardKinematics(model_, data_, q_); + pinocchio::updateFramePlacements(model_, data_); + } + + + void PinocchioRobotModel::inertialInit() + { + // set cog frame + oMcog_.translation() = pinocchio::centerOfMass(model_, data_, q_, true); + oMcog_.rotation() = data_.oMf[model_.getFrameId(baselink_)].rotation(); + pinocchio::casadi::copy(oMcog_.translation(), opcog_); + + // get mass (caution: type of this variable is casadi::SX) + mass_ = pinocchio::computeTotalMass(model_); + ROS_WARN_STREAM("[model][pinocchio] robot mass: " << mass_); + + // get inertia matrix expressed in cog frame. (Hint: pinocchio/unittest/centroidal.cpp) + pinocchio::crba(model_, data_, q_); // Composite Rigid Body Algorithm + data_.M.triangularView() = data_.M.transpose().triangularView(); + data_.Ycrb[0] = data_.liMi[1].act(data_.Ycrb[1]); + + pinocchio::InertiaTpl Ig(oMcog_.inverse().act(data_.Ycrb[0])); + pinocchio::Symmetric3Tpl I = Ig.inertia(); + pinocchio::casadi::copy(I.matrix(), inertia_); + } + + void PinocchioRobotModel::rotorInit() + { + // get rotor origin and normal from root and cog + rotors_origin_root_.resize(rotor_num_); + rotors_origin_cog_.resize(rotor_num_); + rotors_normal_root_.resize(rotor_num_); + rotors_normal_cog_.resize(rotor_num_); + + for(int i = 0; i < rotor_num_; i++) + { + std::string rotor_name = "rotor" + std::to_string(i + 1); + int joint_id = model_.getJointId(rotor_name); + + // origin + pinocchio::casadi::copy(data_.oMi[joint_id].translation(), rotors_origin_root_.at(i)); + pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).translation(), rotors_origin_cog_.at(i)); + + // normal + casadi::SX rotor_normal_root = casadi::SX::zeros(3); + casadi::SX rotor_normal_cog = casadi::SX::zeros(3); + int rotor_axis_type = 0; + if(model_.joints[joint_id].shortname() == "JointModelRX" || model_.joints[joint_id].shortname() == "JointModelRUBX") + rotor_axis_type = 0; + else if(model_.joints[joint_id].shortname() == "JointModelRY" || model_.joints[joint_id].shortname() == "JointModelRUBY") + rotor_axis_type = 1; + else if(model_.joints[joint_id].shortname() == "JointModelRZ" || model_.joints[joint_id].shortname() == "JointModelRUBZ" || model_.joints[joint_id].shortname() == "JointModelRevoluteUnboundedUnaligned") // hard coded for JointModelRevoluteUnboundedUnaligned + rotor_axis_type = 2; + + pinocchio::casadi::copy(data_.oMi[joint_id].rotation().middleCols(rotor_axis_type, 1), rotors_normal_root_.at(i)); + pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).rotation().middleCols(rotor_axis_type, 1), rotors_normal_cog_.at(i)); + } + } + + + void PinocchioRobotModel::updateRobotModel(const sensor_msgs::JointState& state) + { + updateRobotModelImpl(state); + } + + void PinocchioRobotModel::updateRobotModelImpl(const sensor_msgs::JointState& state) + { + std::vector joint_names = state.name; + std::vector joint_positions = state.position; + + for(int i = 0; i < joint_names.size(); i++) + { + q_dbl_(joint_index_map_[joint_names.at(i)]) = joint_positions.at(i); + } + + // std::cout << "real cog: " << computeRealValue(opcog_, q_cs_, q_dbl_).transpose() << std::endl; + // for(int i = 0; i < rotor_num_; i++) + // { + // std::cout << "origin " << i + 1 << ": " << computeRealValue(rotors_origin_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; + // std::cout << "normal " << i + 1 << ": " << computeRealValue(rotors_normal_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; + // } + // std::cout << std::endl; + // std::cout << computeRealValue(inertia_, q_cs_, q_dbl_) << std::endl; + // std::cout << std::endl; + } + + std::string PinocchioRobotModel::getRobotModelXml(const std::string param, ros::NodeHandle nh) + { + std::string xml_string; + + if(!nh.hasParam(param)) { - q_dbl_(i) = 0.0; - q_(i) = q_cs_(i); + ROS_ERROR("Could not find parameter %s on parameter server with namespace '%s'", param.c_str(), nh.getNamespace().c_str()); + return xml_string; } - - // solve FK - pinocchio::forwardKinematics(model_, data_, q_); - pinocchio::updateFramePlacements(model_, data_); + nh.getParam(param, xml_string); + return xml_string; + } } - - -void PinocchioRobotModel::inertialInit() -{ - // set cog frame - oMcog_.translation() = pinocchio::centerOfMass(model_, data_, q_, true); - oMcog_.rotation() = data_.oMf[model_.getFrameId(baselink_)].rotation(); - pinocchio::casadi::copy(oMcog_.translation(), opcog_); - - // get mass (caution: type of this variable is casadi::SX) - mass_ = pinocchio::computeTotalMass(model_); - ROS_WARN_STREAM("[model][pinocchio] robot mass: " << mass_); - - // get inertia matrix expressed in cog frame. (Hint: pinocchio/unittest/centroidal.cpp) - pinocchio::crba(model_, data_, q_); // Composite Rigid Body Algorithm - data_.M.triangularView() = data_.M.transpose().triangularView(); - data_.Ycrb[0] = data_.liMi[1].act(data_.Ycrb[1]); - - pinocchio::InertiaTpl Ig(oMcog_.inverse().act(data_.Ycrb[0])); - pinocchio::Symmetric3Tpl I = Ig.inertia(); - pinocchio::casadi::copy(I.matrix(), inertia_); -} - -void PinocchioRobotModel::rotorInit() -{ - // get rotor origin and normal from root and cog - rotors_origin_root_.resize(rotor_num_); - rotors_origin_cog_.resize(rotor_num_); - rotors_normal_root_.resize(rotor_num_); - rotors_normal_cog_.resize(rotor_num_); - - for(int i = 0; i < rotor_num_; i++) - { - std::string rotor_name = "rotor" + std::to_string(i + 1); - int joint_id = model_.getJointId(rotor_name); - - // origin - pinocchio::casadi::copy(data_.oMi[joint_id].translation(), rotors_origin_root_.at(i)); - pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).translation(), rotors_origin_cog_.at(i)); - - // normal - casadi::SX rotor_normal_root = casadi::SX::zeros(3); - casadi::SX rotor_normal_cog = casadi::SX::zeros(3); - int rotor_axis_type = 0; - if(model_.joints[joint_id].shortname() == "JointModelRX" || model_.joints[joint_id].shortname() == "JointModelRUBX") - rotor_axis_type = 0; - else if(model_.joints[joint_id].shortname() == "JointModelRY" || model_.joints[joint_id].shortname() == "JointModelRUBY") - rotor_axis_type = 1; - else if(model_.joints[joint_id].shortname() == "JointModelRZ" || model_.joints[joint_id].shortname() == "JointModelRUBZ" || model_.joints[joint_id].shortname() == "JointModelRevoluteUnboundedUnaligned") // hard coded for JointModelRevoluteUnboundedUnaligned - rotor_axis_type = 2; - - pinocchio::casadi::copy(data_.oMi[joint_id].rotation().middleCols(rotor_axis_type, 1), rotors_normal_root_.at(i)); - pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).rotation().middleCols(rotor_axis_type, 1), rotors_normal_cog_.at(i)); - } -} - -void PinocchioRobotModel::jointStateCallback(const sensor_msgs::JointStateConstPtr msg) -{ - std::vector joint_names = msg->name; - std::vector joint_positions = msg->position; - - for(int i = 0; i < joint_names.size(); i++) - { - q_dbl_(joint_index_map_[joint_names.at(i)]) = joint_positions.at(i); - } - - // std::cout << "real cog: " << computeRealValue(opcog_, q_cs_, q_dbl_).transpose() << std::endl; - // for(int i = 0; i < rotor_num_; i++) - // { - // std::cout << "origin " << i + 1 << ": " << computeRealValue(rotors_origin_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; - // std::cout << "normal " << i + 1 << ": " << computeRealValue(rotors_normal_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; - // } - // std::cout << std::endl; - // std::cout << computeRealValue(inertia_, q_cs_, q_dbl_) << std::endl; - // std::cout << std::endl; -} - diff --git a/aerial_robot_model/src/model/base_model/pinocchio_robot_model_ros.cpp b/aerial_robot_model/src/model/base_model/pinocchio_robot_model_ros.cpp new file mode 100644 index 000000000..cc1d071a6 --- /dev/null +++ b/aerial_robot_model/src/model/base_model/pinocchio_robot_model_ros.cpp @@ -0,0 +1,36 @@ +#include + +namespace aerial_robot_model { + PinocchioRobotModelRos::PinocchioRobotModelRos(ros::NodeHandle nh, ros::NodeHandle nhp): + nh_(nh), + nhp_(nhp), + pinocchio_robot_model_loader_("aerial_robot_model", "aerial_robot_model::PinocchioRobotModel") + { + // load robot model plugin + std::string plugin_name; + if(nh_.getParam("pinocchio_robot_model_plugin_name", plugin_name)) + { + try + { + pinocchio_robot_model_ = pinocchio_robot_model_loader_.createInstance(plugin_name); + } + catch(pluginlib::PluginlibException& ex) + { + ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what()); + } + } + else + { + ROS_ERROR("can not find plugin rosparameter for robot model, use default class: aerial_robot_model::PinocchioRobotModel"); + pinocchio_robot_model_ = boost::make_shared(); + } + + + joint_state_sub_ = nh_.subscribe("joint_states", 1, &PinocchioRobotModelRos::jointStateCallback, this); + } + + void PinocchioRobotModelRos::jointStateCallback(const sensor_msgs::JointStateConstPtr& msg) + { + pinocchio_robot_model_->updateRobotModel(*msg); + } +} diff --git a/aerial_robot_model/src/model/base_model/pinocchio_test.cpp b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp index 6bafd2ce6..221bb16ca 100644 --- a/aerial_robot_model/src/model/base_model/pinocchio_test.cpp +++ b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp @@ -1,4 +1,4 @@ -#include +#include int main(int argc, char ** argv) { @@ -6,9 +6,9 @@ int main(int argc, char ** argv) ros::NodeHandle nh; ros::NodeHandle nh_private("~"); - PinocchioRobotModel* pinocchio_robot_model = new PinocchioRobotModel(nh, nh_private); + aerial_robot_model::PinocchioRobotModelRos* pinocchio_robot_model_ros = new aerial_robot_model::PinocchioRobotModelRos(nh, nh_private); ros::spin(); - delete pinocchio_robot_model; + delete pinocchio_robot_model_ros; return 0; } From e05fee3b4f897cab29ef565f8db311a25e03f787 Mon Sep 17 00:00:00 2001 From: sugihara Date: Sun, 17 Mar 2024 13:03:54 +0900 Subject: [PATCH 21/27] update 3rd party repository version and update cmake version in CI for melodic and kinetic --- .travis.sh | 22 ++++++++++++++++++++++ aerial_robot_kinetic.rosinstall | 4 ++-- aerial_robot_melodic.rosinstall | 2 +- aerial_robot_noetic.rosinstall | 2 +- 4 files changed, 26 insertions(+), 4 deletions(-) diff --git a/.travis.sh b/.travis.sh index df8f561a0..69c23ecfb 100644 --- a/.travis.sh +++ b/.travis.sh @@ -69,6 +69,28 @@ if [[ "$ROS_DISTRO" = "kinetic" ]]; then export CC='gcc-7' fi +# use latest cmake +# https://qiita.com/sunrise_lover/items/810977fede4b979c382b +if [[ "$ROS_DISTRO" = "kinetic" ]]; then + cmake --version + wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /etc/apt/trusted.gpg.d/kitware.gpg >/dev/null + sudo apt-get install -y software-properties-common + sudo apt-get install -y apt-transport-https + sudo apt-add-repository "deb https://apt.kitware.com/ubuntu/ $(lsb_release -cs) main" + sudo apt update + sudo apt install -y cmake + cmake --version +fi +if [[ "$ROS_DISTRO" = "melodic" ]]; then + cmake --version + wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /etc/apt/trusted.gpg.d/kitware.gpg >/dev/null + sudo apt-get install -y software-properties-common + sudo apt-add-repository "deb https://apt.kitware.com/ubuntu/ $(lsb_release -cs) main" + sudo apt update + sudo apt install -y cmake + cmake --version +fi + # Build catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON catkin build -p1 -j1 --no-status diff --git a/aerial_robot_kinetic.rosinstall b/aerial_robot_kinetic.rosinstall index fc7b02212..ea109b3c6 100644 --- a/aerial_robot_kinetic.rosinstall +++ b/aerial_robot_kinetic.rosinstall @@ -1,8 +1,8 @@ # aerial robot third party - git: local-name: aerial_robot_3rdparty - uri: https://github.com/JSKAerialRobot/aerial_robot_3rdparty.git - version: b10f5bf + uri: https://github.com/sugikazu75/aerial_robot_3rdparty.git + version: develop/casadi/sample # kalman filter - git: diff --git a/aerial_robot_melodic.rosinstall b/aerial_robot_melodic.rosinstall index c60683f37..370c8b871 100644 --- a/aerial_robot_melodic.rosinstall +++ b/aerial_robot_melodic.rosinstall @@ -2,7 +2,7 @@ - git: local-name: aerial_robot_3rdparty uri: https://github.com/sugikazu75/aerial_robot_3rdparty.git - version: 27caf90 + version: develop/casadi/sample # kalman filter - git: diff --git a/aerial_robot_noetic.rosinstall b/aerial_robot_noetic.rosinstall index c60683f37..370c8b871 100644 --- a/aerial_robot_noetic.rosinstall +++ b/aerial_robot_noetic.rosinstall @@ -2,7 +2,7 @@ - git: local-name: aerial_robot_3rdparty uri: https://github.com/sugikazu75/aerial_robot_3rdparty.git - version: 27caf90 + version: develop/casadi/sample # kalman filter - git: From d08ee19f35ea1fcd545921a27ab820ac56521a4c Mon Sep 17 00:00:00 2001 From: sugihara Date: Sun, 17 Mar 2024 19:42:37 +0900 Subject: [PATCH 22/27] [aerial_robot_model][pinocchio] add accessors for private variables in pinocchio robot model class --- .../model/pinocchio_robot_model.h | 44 +++++++++++++------ .../model/pinocchio_robot_model_ros.h | 3 ++ 2 files changed, 34 insertions(+), 13 deletions(-) diff --git a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h index fd9ececfa..e778594c3 100644 --- a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h +++ b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h @@ -1,18 +1,18 @@ #pragma once -#include "pinocchio/fwd.hpp" - -#include "pinocchio/parsers/urdf.hpp" - -#include "pinocchio/algorithm/center-of-mass.hxx" -#include "pinocchio/algorithm/centroidal.hxx" -#include "pinocchio/algorithm/crba.hpp" -#include "pinocchio/algorithm/frames.hpp" -#include "pinocchio/algorithm/kinematics.hpp" -#include "pinocchio/algorithm/kinematics-derivatives.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" -#include "pinocchio/algorithm/model.hxx" -#include "pinocchio/autodiff/casadi.hpp" +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -42,9 +42,27 @@ namespace aerial_robot_model { PinocchioRobotModel(); ~PinocchioRobotModel() = default; + pinocchio::Model getModelDbl() {return model_dbl_;} + pinocchio::Data getDataDbl() {return data_dbl_;} + pinocchio::ModelTpl getModel() {return model_;} + pinocchio::DataTpl getData() {return data_;} + + std::vector getRotorsOriginFromRoot() {return rotors_origin_root_;} std::vector getRotorsOriginFromCog() {return rotors_origin_cog_;} + std::vector getRotorsNormalFromRoot() {return rotors_normal_root_;} std::vector getRotorsNormalFromCog() {return rotors_normal_cog_;} + + casadi::SX getQCs() {return q_cs_;} + Eigen::Matrix getQ() {return q_;} + casadi::DM getQDbl() {return q_dbl_;} + + casadi::SX getMass() {return mass_;} + casadi::SX getInertia() {return inertia_;} + pinocchio::SE3Tpl getocog() {return oMcog_;} + casadi::SX getoPcog() {return opcog_;} + std::map getJointIndexMap() {return joint_index_map_;} + void updateRobotModel(const sensor_msgs::JointState& state); void updateRobotModelImpl(const sensor_msgs::JointState& state); diff --git a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h index fb6853dc3..a2b8a664d 100644 --- a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h +++ b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h @@ -2,12 +2,15 @@ #include #include +#include +#include namespace aerial_robot_model { class PinocchioRobotModelRos { public: PinocchioRobotModelRos(ros::NodeHandle nh, ros::NodeHandle nhp); virtual ~PinocchioRobotModelRos() = default; + boost::shared_ptr getPinocchioRobotModel() {return pinocchio_robot_model_;} private: ros::NodeHandle nh_; From 138f3c2b35e6595d61341542a3132b2e15f7a77e Mon Sep 17 00:00:00 2001 From: sugihara Date: Mon, 25 Mar 2024 22:10:40 +0900 Subject: [PATCH 23/27] [aerial_robot_model] separate desinition and implementation of common function --- .../model/pinocchio_robot_model.h | 31 ++----------------- .../base_model/pinocchio_robot_model.cpp | 30 ++++++++++++++++++ 2 files changed, 32 insertions(+), 29 deletions(-) diff --git a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h index e778594c3..93c1c08b9 100644 --- a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h +++ b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h @@ -101,32 +101,5 @@ namespace aerial_robot_model { } -Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, Eigen::VectorXd x_dbl) -{ - casadi::DM ret = casadi::DM(y.size1(), y.size2()); - for(int i = 0; i < y.size1(); i++) - { - for(int j = 0; j < y.size2(); j++) - { - casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); - casadi::DM y_dbl = f(eigenVectorToCasadiDm(x_dbl)); - ret(i, j) = y_dbl; - } - } - return casadiDmToEigenMatrix(ret); -} - -Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, casadi::DM x_dbl) -{ - casadi::DM ret = casadi::DM(y.size1(), y.size2()); - for(int i = 0; i < y.size1(); i++) - { - for(int j = 0; j < y.size2(); j++) - { - casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); - casadi::DM y_dbl = f(x_dbl); - ret(i, j) = y_dbl; - } - } - return casadiDmToEigenMatrix(ret); -} +Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, Eigen::VectorXd x_dbl); +Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, casadi::DM x_dbl); diff --git a/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp index ba54bf482..20c16bc66 100644 --- a/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp +++ b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp @@ -186,3 +186,33 @@ namespace aerial_robot_model { return xml_string; } } + +Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, Eigen::VectorXd x_dbl) +{ + casadi::DM ret = casadi::DM(y.size1(), y.size2()); + for(int i = 0; i < y.size1(); i++) + { + for(int j = 0; j < y.size2(); j++) + { + casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); + casadi::DM y_dbl = f(eigenVectorToCasadiDm(x_dbl)); + ret(i, j) = y_dbl; + } + } + return casadiDmToEigenMatrix(ret); +} + +Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, casadi::DM x_dbl) +{ + casadi::DM ret = casadi::DM(y.size1(), y.size2()); + for(int i = 0; i < y.size1(); i++) + { + for(int j = 0; j < y.size2(); j++) + { + casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); + casadi::DM y_dbl = f(x_dbl); + ret(i, j) = y_dbl; + } + } + return casadiDmToEigenMatrix(ret); +} From 4313edf79611f62becb946da355b454a4cfcba49 Mon Sep 17 00:00:00 2001 From: sugihara Date: Tue, 26 Mar 2024 18:15:35 +0900 Subject: [PATCH 24/27] [aerial_robot_model][pinocchio] enable to update robot model with variable and remove joint state allback --- .../model/pinocchio_robot_model.h | 6 +- .../model/pinocchio_robot_model_ros.h | 10 +-- .../base_model/pinocchio_robot_model.cpp | 82 ++++++++----------- .../base_model/pinocchio_robot_model_ros.cpp | 11 +-- 4 files changed, 46 insertions(+), 63 deletions(-) diff --git a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h index 93c1c08b9..a943d717d 100644 --- a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h +++ b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h @@ -63,8 +63,8 @@ namespace aerial_robot_model { std::map getJointIndexMap() {return joint_index_map_;} - void updateRobotModel(const sensor_msgs::JointState& state); - void updateRobotModelImpl(const sensor_msgs::JointState& state); + void updateRobotModel(); + void updateRobotModelImpl(Eigen::Matrix q); private: std::string getRobotModelXml(const std::string param, ros::NodeHandle nh = ros::NodeHandle()); @@ -72,7 +72,9 @@ namespace aerial_robot_model { void modelInit(); void kinematicsInit(); void inertialInit(); + void inertialUpdate(); void rotorInit(); + void rotorUpdate(); pinocchio::Model model_dbl_; pinocchio::Data data_dbl_; diff --git a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h index a2b8a664d..14934f2fa 100644 --- a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h +++ b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h @@ -5,8 +5,10 @@ #include #include -namespace aerial_robot_model { - class PinocchioRobotModelRos { +namespace aerial_robot_model +{ + class PinocchioRobotModelRos + { public: PinocchioRobotModelRos(ros::NodeHandle nh, ros::NodeHandle nhp); virtual ~PinocchioRobotModelRos() = default; @@ -15,12 +17,8 @@ namespace aerial_robot_model { private: ros::NodeHandle nh_; ros::NodeHandle nhp_; - ros::Subscriber joint_state_sub_; pluginlib::ClassLoader pinocchio_robot_model_loader_; boost::shared_ptr pinocchio_robot_model_; - - void jointStateCallback(const sensor_msgs::JointStateConstPtr& msg); }; - } diff --git a/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp index 20c16bc66..716f205a3 100644 --- a/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp +++ b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp @@ -7,6 +7,8 @@ namespace aerial_robot_model { kinematicsInit(); inertialInit(); rotorInit(); + + updateRobotModel(); } @@ -78,6 +80,9 @@ namespace aerial_robot_model { { // init q q_cs_ = casadi::SX::sym("q", model_.nq); + for(int i = 0; i < 7; i++) // set 0 to free flyer joint + q_cs_(i) = 0; + q_dbl_ = casadi::DM(model_.nq, 1); q_.resize(model_.nq, 1); for(int i = 0; i < model_.nq; i++) @@ -85,24 +90,30 @@ namespace aerial_robot_model { q_dbl_(i) = 0.0; q_(i) = q_cs_(i); } + } - // solve FK - pinocchio::forwardKinematics(model_, data_, q_); - pinocchio::updateFramePlacements(model_, data_); + void PinocchioRobotModel::inertialInit() + { + // get mass (caution: type of this variable is casadi::SX) + mass_ = pinocchio::computeTotalMass(model_); + ROS_WARN_STREAM("[model][pinocchio] robot mass: " << mass_); } + void PinocchioRobotModel::rotorInit() + { + rotors_origin_root_.resize(rotor_num_); + rotors_origin_cog_.resize(rotor_num_); + rotors_normal_root_.resize(rotor_num_); + rotors_normal_cog_.resize(rotor_num_); + } - void PinocchioRobotModel::inertialInit() + void PinocchioRobotModel::inertialUpdate() { // set cog frame oMcog_.translation() = pinocchio::centerOfMass(model_, data_, q_, true); oMcog_.rotation() = data_.oMf[model_.getFrameId(baselink_)].rotation(); pinocchio::casadi::copy(oMcog_.translation(), opcog_); - // get mass (caution: type of this variable is casadi::SX) - mass_ = pinocchio::computeTotalMass(model_); - ROS_WARN_STREAM("[model][pinocchio] robot mass: " << mass_); - // get inertia matrix expressed in cog frame. (Hint: pinocchio/unittest/centroidal.cpp) pinocchio::crba(model_, data_, q_); // Composite Rigid Body Algorithm data_.M.triangularView() = data_.M.transpose().triangularView(); @@ -113,14 +124,9 @@ namespace aerial_robot_model { pinocchio::casadi::copy(I.matrix(), inertia_); } - void PinocchioRobotModel::rotorInit() + void PinocchioRobotModel::rotorUpdate() { // get rotor origin and normal from root and cog - rotors_origin_root_.resize(rotor_num_); - rotors_origin_cog_.resize(rotor_num_); - rotors_normal_root_.resize(rotor_num_); - rotors_normal_cog_.resize(rotor_num_); - for(int i = 0; i < rotor_num_; i++) { std::string rotor_name = "rotor" + std::to_string(i + 1); @@ -130,47 +136,31 @@ namespace aerial_robot_model { pinocchio::casadi::copy(data_.oMi[joint_id].translation(), rotors_origin_root_.at(i)); pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).translation(), rotors_origin_cog_.at(i)); - // normal - casadi::SX rotor_normal_root = casadi::SX::zeros(3); - casadi::SX rotor_normal_cog = casadi::SX::zeros(3); - int rotor_axis_type = 0; - if(model_.joints[joint_id].shortname() == "JointModelRX" || model_.joints[joint_id].shortname() == "JointModelRUBX") - rotor_axis_type = 0; - else if(model_.joints[joint_id].shortname() == "JointModelRY" || model_.joints[joint_id].shortname() == "JointModelRUBY") - rotor_axis_type = 1; - else if(model_.joints[joint_id].shortname() == "JointModelRZ" || model_.joints[joint_id].shortname() == "JointModelRUBZ" || model_.joints[joint_id].shortname() == "JointModelRevoluteUnboundedUnaligned") // hard coded for JointModelRevoluteUnboundedUnaligned - rotor_axis_type = 2; - - pinocchio::casadi::copy(data_.oMi[joint_id].rotation().middleCols(rotor_axis_type, 1), rotors_normal_root_.at(i)); - pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).rotation().middleCols(rotor_axis_type, 1), rotors_normal_cog_.at(i)); + // normal (assume rotational axis is corresponding to z axis) + pinocchio::casadi::copy(data_.oMi[joint_id].rotation().middleCols(2, 1), rotors_normal_root_.at(i)); + pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).rotation().middleCols(2, 1), rotors_normal_cog_.at(i)); } } - - void PinocchioRobotModel::updateRobotModel(const sensor_msgs::JointState& state) + void PinocchioRobotModel::updateRobotModel() { - updateRobotModelImpl(state); + Eigen::Matrix q; + q.resize(model_.nq, 1); + pinocchio::casadi::copy(q_cs_, q); + + updateRobotModelImpl(q); } - void PinocchioRobotModel::updateRobotModelImpl(const sensor_msgs::JointState& state) + void PinocchioRobotModel::updateRobotModelImpl(Eigen::Matrix q) { - std::vector joint_names = state.name; - std::vector joint_positions = state.position; + q_ = q; - for(int i = 0; i < joint_names.size(); i++) - { - q_dbl_(joint_index_map_[joint_names.at(i)]) = joint_positions.at(i); - } + // solve FK + pinocchio::forwardKinematics(model_, data_, q_); + pinocchio::updateFramePlacements(model_, data_); - // std::cout << "real cog: " << computeRealValue(opcog_, q_cs_, q_dbl_).transpose() << std::endl; - // for(int i = 0; i < rotor_num_; i++) - // { - // std::cout << "origin " << i + 1 << ": " << computeRealValue(rotors_origin_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; - // std::cout << "normal " << i + 1 << ": " << computeRealValue(rotors_normal_cog_.at(i), q_cs_, q_dbl_).transpose() << std::endl; - // } - // std::cout << std::endl; - // std::cout << computeRealValue(inertia_, q_cs_, q_dbl_) << std::endl; - // std::cout << std::endl; + inertialUpdate(); + rotorUpdate(); } std::string PinocchioRobotModel::getRobotModelXml(const std::string param, ros::NodeHandle nh) diff --git a/aerial_robot_model/src/model/base_model/pinocchio_robot_model_ros.cpp b/aerial_robot_model/src/model/base_model/pinocchio_robot_model_ros.cpp index cc1d071a6..dcb551d7e 100644 --- a/aerial_robot_model/src/model/base_model/pinocchio_robot_model_ros.cpp +++ b/aerial_robot_model/src/model/base_model/pinocchio_robot_model_ros.cpp @@ -1,6 +1,7 @@ #include -namespace aerial_robot_model { +namespace aerial_robot_model +{ PinocchioRobotModelRos::PinocchioRobotModelRos(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh), nhp_(nhp), @@ -24,13 +25,5 @@ namespace aerial_robot_model { ROS_ERROR("can not find plugin rosparameter for robot model, use default class: aerial_robot_model::PinocchioRobotModel"); pinocchio_robot_model_ = boost::make_shared(); } - - - joint_state_sub_ = nh_.subscribe("joint_states", 1, &PinocchioRobotModelRos::jointStateCallback, this); - } - - void PinocchioRobotModelRos::jointStateCallback(const sensor_msgs::JointStateConstPtr& msg) - { - pinocchio_robot_model_->updateRobotModel(*msg); } } From 75a5394880e6db25f6604df8cc38c0457181657e Mon Sep 17 00:00:00 2001 From: sugikazu75 Date: Tue, 26 Mar 2024 19:05:22 +0900 Subject: [PATCH 25/27] [aerial_robot_model][pinocchio] add updateRobotModel function with some type of variable --- .../model/pinocchio_robot_model.h | 2 ++ .../model/base_model/pinocchio_robot_model.cpp | 18 ++++++++++++++++++ 2 files changed, 20 insertions(+) diff --git a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h index a943d717d..45d831ce8 100644 --- a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h +++ b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h @@ -64,6 +64,8 @@ namespace aerial_robot_model { std::map getJointIndexMap() {return joint_index_map_;} void updateRobotModel(); + void updateRobotModel(casadi::SX q_cs); + void updateRobotModel(Eigen::Matrix q); void updateRobotModelImpl(Eigen::Matrix q); private: diff --git a/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp index 716f205a3..1786da18e 100644 --- a/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp +++ b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp @@ -148,6 +148,24 @@ namespace aerial_robot_model { q.resize(model_.nq, 1); pinocchio::casadi::copy(q_cs_, q); + updateRobotModel(q); + } + + void PinocchioRobotModel::updateRobotModel(casadi::SX q_cs) + { + assert((q_cs.size1() == model_.nq) || (q_cs.size2() == model_.nq)); + + Eigen::Matrix q; + q.resize(model_.nq, 1); + pinocchio::casadi::copy(q_cs, q); + + updateRobotModel(q); + } + + void PinocchioRobotModel::updateRobotModel(Eigen::Matrix q) + { + assert(q.size() == model_.nq); + updateRobotModelImpl(q); } From 855d516a921570c22d2f7ff0d632bc02db915b2c Mon Sep 17 00:00:00 2001 From: sugikazu75 Date: Wed, 3 Apr 2024 16:30:34 +0900 Subject: [PATCH 26/27] [aerial_robot_model][pinocchio] add mutex to access private variables --- .../model/pinocchio_robot_model.h | 67 ++++++++++++++----- .../base_model/pinocchio_robot_model.cpp | 59 +++++----------- 2 files changed, 69 insertions(+), 57 deletions(-) diff --git a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h index 45d831ce8..b380cffa2 100644 --- a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h +++ b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h @@ -47,10 +47,10 @@ namespace aerial_robot_model { pinocchio::ModelTpl getModel() {return model_;} pinocchio::DataTpl getData() {return data_;} - std::vector getRotorsOriginFromRoot() {return rotors_origin_root_;} - std::vector getRotorsOriginFromCog() {return rotors_origin_cog_;} - std::vector getRotorsNormalFromRoot() {return rotors_normal_root_;} - std::vector getRotorsNormalFromCog() {return rotors_normal_cog_;} + std::vector getRotorsOriginFromRoot() {return rotors_origin_from_root_;} + std::vector getRotorsOriginFromCog() {return rotors_origin_from_cog_;} + std::vector getRotorsNormalFromRoot() {return rotors_normal_from_root_;} + std::vector getRotorsNormalFromCog() {return rotors_normal_from_cog_;} casadi::SX getQCs() {return q_cs_;} Eigen::Matrix getQ() {return q_;} @@ -58,8 +58,7 @@ namespace aerial_robot_model { casadi::SX getMass() {return mass_;} casadi::SX getInertia() {return inertia_;} - pinocchio::SE3Tpl getocog() {return oMcog_;} - casadi::SX getoPcog() {return opcog_;} + pinocchio::SE3Tpl getoMcog() {return oMcog_;} std::map getJointIndexMap() {return joint_index_map_;} @@ -83,10 +82,16 @@ namespace aerial_robot_model { pinocchio::ModelTpl model_; pinocchio::DataTpl data_; - std::vector rotors_origin_root_; - std::vector rotors_origin_cog_; - std::vector rotors_normal_root_; - std::vector rotors_normal_cog_; + std::mutex mutex_cog_pos_; + std::mutex mutex_cog_rot_; + std::mutex mutex_inertia_; + std::mutex mutex_rotor_origin_; + std::mutex mutex_rotor_normal_; + + std::vector rotors_origin_from_root_; + std::vector rotors_origin_from_cog_; + std::vector rotors_normal_from_root_; + std::vector rotors_normal_from_cog_; casadi::SX q_cs_; Eigen::Matrix q_; @@ -95,15 +100,47 @@ namespace aerial_robot_model { casadi::SX mass_; casadi::SX inertia_; pinocchio::SE3Tpl oMcog_; - casadi::SX opcog_; std::map joint_index_map_; int rotor_num_; std::string baselink_; + + protected: + void setCogPos(const Eigen::Matrix cog_pos) + { + std::lock_guard lock(mutex_cog_pos_); + oMcog_.translation() = cog_pos; + } + void setCogRot(const Eigen::Matrix cog_rot) + { + std::lock_guard lock(mutex_cog_rot_); + oMcog_.rotation() = cog_rot; + } + void setInertia(const Eigen::Matrix inertia) + { + std::lock_guard lock(mutex_inertia_); + pinocchio::casadi::copy(inertia, inertia_); + } + void setRotorsOriginFromRoot(const std::vector rotors_origin_from_root) + { + std::lock_guard lock(mutex_rotor_origin_); + rotors_origin_from_root_ = rotors_origin_from_root; + } + void setRotorsOriginFromCog(const std::vector rotors_origin_from_cog) + { + std::lock_guard lock(mutex_rotor_origin_); + rotors_origin_from_cog_ = rotors_origin_from_cog; + } + void setRotorsNormalFromRoot(const std::vector rotors_normal_from_root) + { + std::lock_guard lock(mutex_rotor_normal_); + rotors_normal_from_root_ = rotors_normal_from_root; + } + void setRotorsNormalFromCog(const std::vector rotors_normal_from_cog) + { + std::lock_guard lock(mutex_rotor_normal_); + rotors_normal_from_cog_ = rotors_normal_from_cog; + } }; } - - -Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, Eigen::VectorXd x_dbl); -Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, casadi::DM x_dbl); diff --git a/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp index 1786da18e..685bd7e77 100644 --- a/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp +++ b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp @@ -101,18 +101,17 @@ namespace aerial_robot_model { void PinocchioRobotModel::rotorInit() { - rotors_origin_root_.resize(rotor_num_); - rotors_origin_cog_.resize(rotor_num_); - rotors_normal_root_.resize(rotor_num_); - rotors_normal_cog_.resize(rotor_num_); + rotors_origin_from_root_.resize(rotor_num_); + rotors_origin_from_cog_.resize(rotor_num_); + rotors_normal_from_root_.resize(rotor_num_); + rotors_normal_from_cog_.resize(rotor_num_); } void PinocchioRobotModel::inertialUpdate() { // set cog frame - oMcog_.translation() = pinocchio::centerOfMass(model_, data_, q_, true); - oMcog_.rotation() = data_.oMf[model_.getFrameId(baselink_)].rotation(); - pinocchio::casadi::copy(oMcog_.translation(), opcog_); + setCogPos(pinocchio::centerOfMass(model_, data_, q_, true)); + setCogRot(data_.oMf[model_.getFrameId(baselink_)].rotation()); // get inertia matrix expressed in cog frame. (Hint: pinocchio/unittest/centroidal.cpp) pinocchio::crba(model_, data_, q_); // Composite Rigid Body Algorithm @@ -121,11 +120,12 @@ namespace aerial_robot_model { pinocchio::InertiaTpl Ig(oMcog_.inverse().act(data_.Ycrb[0])); pinocchio::Symmetric3Tpl I = Ig.inertia(); - pinocchio::casadi::copy(I.matrix(), inertia_); + setInertia(I.matrix()); } void PinocchioRobotModel::rotorUpdate() { + std::vector rotors_origin_from_root(rotor_num_), rotors_origin_from_cog(rotor_num_), rotors_normal_from_root(rotor_num_), rotors_normal_from_cog(rotor_num_); // get rotor origin and normal from root and cog for(int i = 0; i < rotor_num_; i++) { @@ -133,13 +133,18 @@ namespace aerial_robot_model { int joint_id = model_.getJointId(rotor_name); // origin - pinocchio::casadi::copy(data_.oMi[joint_id].translation(), rotors_origin_root_.at(i)); - pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).translation(), rotors_origin_cog_.at(i)); + pinocchio::casadi::copy(data_.oMi[joint_id].translation(), rotors_origin_from_root.at(i)); + pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).translation(), rotors_origin_from_cog.at(i)); // normal (assume rotational axis is corresponding to z axis) - pinocchio::casadi::copy(data_.oMi[joint_id].rotation().middleCols(2, 1), rotors_normal_root_.at(i)); - pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).rotation().middleCols(2, 1), rotors_normal_cog_.at(i)); + pinocchio::casadi::copy(data_.oMi[joint_id].rotation().middleCols(2, 1), rotors_normal_from_root.at(i)); + pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).rotation().middleCols(2, 1), rotors_normal_from_cog.at(i)); } + + setRotorsOriginFromRoot(rotors_origin_from_root); + setRotorsOriginFromCog(rotors_origin_from_cog); + setRotorsNormalFromRoot(rotors_normal_from_root); + setRotorsNormalFromCog(rotors_normal_from_cog); } void PinocchioRobotModel::updateRobotModel() @@ -194,33 +199,3 @@ namespace aerial_robot_model { return xml_string; } } - -Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, Eigen::VectorXd x_dbl) -{ - casadi::DM ret = casadi::DM(y.size1(), y.size2()); - for(int i = 0; i < y.size1(); i++) - { - for(int j = 0; j < y.size2(); j++) - { - casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); - casadi::DM y_dbl = f(eigenVectorToCasadiDm(x_dbl)); - ret(i, j) = y_dbl; - } - } - return casadiDmToEigenMatrix(ret); -} - -Eigen::MatrixXd computeRealValue(casadi::SX y, casadi::SX x, casadi::DM x_dbl) -{ - casadi::DM ret = casadi::DM(y.size1(), y.size2()); - for(int i = 0; i < y.size1(); i++) - { - for(int j = 0; j < y.size2(); j++) - { - casadi::Function f = casadi::Function("f", {x}, {y(i, j)}); - casadi::DM y_dbl = f(x_dbl); - ret(i, j) = y_dbl; - } - } - return casadiDmToEigenMatrix(ret); -} From 14332e131b0ed0ddc5b4eb094a0608b6e046004d Mon Sep 17 00:00:00 2001 From: sugikazu75 Date: Wed, 3 Apr 2024 17:21:19 +0900 Subject: [PATCH 27/27] [aerial_robot_model][pinocchio] include mutex --- .../include/aerial_robot_model/model/pinocchio_robot_model.h | 1 + 1 file changed, 1 insertion(+) diff --git a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h index b380cffa2..2151b1990 100644 --- a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h +++ b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h @@ -32,6 +32,7 @@ #include #include +#include #include