├── CMakeLists.txt ├── README.md ├── config └── impedance_modulation_settings.yaml ├── include └── ImpedanceModulation │ ├── ImpedanceModulationManager.h │ └── utils │ ├── KDLhelper.h │ ├── algebra.h │ ├── logger.h │ └── utilities.h ├── launch └── impedance_modulation.launch.py ├── msg ├── ImpedanceMsg.msg └── TaskMsg.msg ├── package.xml ├── presentation └── ROSCON23-Impedance-BERTONI-PDF.pdf ├── src ├── ImpedanceModulation.cpp └── ImpedanceModulationManager.cpp ├── urdf └── inail2arm.urdf └── utils ├── KDLhelper.cpp ├── algebra.cpp ├── logger.cpp └── utilities.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(rim) 3 | 4 | # c++ compiler 5 | if(NOT CMAKE_CXX_STANDARD) #Default to C++14 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | # ros2 10 | find_package(ament_cmake REQUIRED) 11 | find_package(rclcpp REQUIRED) 12 | 13 | # custom messages 14 | find_package(rosidl_default_generators REQUIRED) 15 | 16 | # external 17 | find_package(orocos_kdl REQUIRED) 18 | find_package(kdl_parser REQUIRED) 19 | find_package(Eigen3 REQUIRED) 20 | find_package(matlogger2 REQUIRED) 21 | 22 | # to generate custom messages 23 | rosidl_generate_interfaces(${PROJECT_NAME} 24 | "msg/TaskMsg.msg" 25 | "msg/ImpedanceMsg.msg" 26 | ) 27 | 28 | include_directories( 29 | include 30 | ${EIGEN3_INCLUDE_DIRS} 31 | ) 32 | 33 | add_library(Logger 34 | utils/logger.cpp 35 | ) 36 | 37 | add_library(Algebra 38 | utils/algebra.cpp 39 | ) 40 | 41 | add_library(Utilities 42 | utils/utilities.cpp 43 | ) 44 | 45 | add_library(KDLHelper 46 | utils/KDLhelper.cpp 47 | ) 48 | 49 | add_library(ImpedanceModulationManager 50 | src/ImpedanceModulationManager.cpp 51 | ) 52 | 53 | set(dependencies 54 | "rclcpp") 55 | 56 | ###################################################################################################### 57 | add_executable(ImpedanceModulation src/ImpedanceModulation.cpp) 58 | ament_target_dependencies(ImpedanceModulation ${dependencies}) 59 | 60 | target_link_libraries(Logger 61 | matlogger2::matlogger2 62 | ) 63 | 64 | target_link_libraries(Utilities 65 | Algebra 66 | ) 67 | 68 | rosidl_get_typesupport_target(cpp_typesupport_target 69 | ${PROJECT_NAME} rosidl_typesupport_cpp) 70 | 71 | # VIM plugin settings 72 | target_link_libraries(ImpedanceModulationManager 73 | "${cpp_typesupport_target}" 74 | KDLHelper 75 | Algebra 76 | Utilities 77 | Logger 78 | ) 79 | 80 | target_link_libraries(ImpedanceModulation 81 | ImpedanceModulationManager 82 | ) 83 | 84 | ament_target_dependencies(KDLHelper kdl_parser) 85 | ament_target_dependencies(ImpedanceModulationManager ${dependencies}) 86 | ament_export_dependencies(rosidl_default_runtime) 87 | 88 | # to allow to ros2 to find the executable 89 | install(TARGETS 90 | ImpedanceModulation 91 | DESTINATION lib/${PROJECT_NAME}) 92 | 93 | # Install launch files. 94 | install(DIRECTORY 95 | launch 96 | DESTINATION share/${PROJECT_NAME} 97 | ) 98 | 99 | # To make config visible 100 | install(DIRECTORY 101 | config 102 | DESTINATION share/${PROJECT_NAME} 103 | ) 104 | 105 | ament_package() -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ➤ Robot Impedance Modulation 2 | 3 | ## Introduction 4 | **Robot Impedance Modulation** is a **ROS2** based package aimed to help the community in calculating a _variable_ impedance of a robot. 5 | Considering a robot having SEA motors, typically used for collaborative robots (Cobots), or a general robot controlled with the following control law 6 | 7 | $$ 8 | \tau = K \\, (q^{ref} - q) - D \dot q + \tau^{ff}, 9 | $$ 10 | 11 | the package provides the required **stiffness KK** and **damping DD** (diagonal matrices) to be set in the motors or equivalently used to compute such a control law. By defining the 12 | required force and admissible error crucial for task execution, the robot impedance is accordingly designed. For further details on the algorithm, you can have a look at the related article 13 | available [**_here_**](https://ieeexplore.ieee.org/abstract/document/10000215). 14 | 15 | 16 | ## Outline 17 | * [Introduction](#introduction) 18 | * [Outline](#outline) 19 | * [How to Use It](#how-to-use-it) 20 | * [How to Configure It](#how-to-use-it) 21 | * [Installation](#installation) 22 | * [Execution](#execution) 23 | * [Dependences](#dependences) 24 | * [Maintaner](#maintaner) 25 | 26 | ## How to Use It 27 | 28 | Here, an explanation of how to use the package is given. We start by providing the rationale behind it, and then we detail the practical usage. 29 | 30 | As mentioned in the introduction, this package allows you to directly obtain the impedance parameters of a robot, i.e. the values of 31 | stiffness and damping for each joint _"j"_ of the robot, kjk_j and djd_j, respectively. These values can be used either to set the motor parameters or to calculate the above control law. 32 | Classical approaches apply fixed values to design these parameters by a manual procedure of trials and errors. For real-time applications in dynamic environments, this procedure can not 33 | properly generate suitable robot reactions based on occurring changes maybe leading to the failure of the task execution. 34 | To overcome these drawbacks, here, the impedance modulation is intended to be designed in such a way as to _better_ fit with the task to accomplish. 35 | For this purpose, by specifying the required force to exert and the precision demanded by the task, the package allows automatically computing a resulting 36 | **time-varying** and **_task_-varying** robot impedance. 37 | 38 | Based on that, we assume that the task to execute is described with 39 | - **force to execute**: expressed as a wrench at the end-effector of the robot, 3D force vector, and 3D moment vector. 40 | - **precision to satisfy**: expressed as a pose error vector, 3D position error vector, and 3D orientation error vector (Euler angles RPY). 41 | - **joints position reference vector**: refers to the references of the joints position. 42 | - **joints position vector**: refers to the measurements of the joints position. 43 | 44 | For real-time applications, these quantities are expected to be time-varying, at least the joints position vector. Otherwise, the robot is expected to be fixed, and the calculation could not fit the task if it is executed. 45 | 46 | **In order to properly use the package**, it is required to provide: 47 | 1. the above information 48 | 2. the model of the robot via standard URDF file. 49 | 50 | How to do that? how to provide this information, It is specified below. 51 | 52 | The task force, precision, joints positions reference, and measurements are assumed to be sent via topic by a publisher using the custom message provided in the package and available in the _msg folder_. The name of the message is _TaskMsg.msg_ and we report here its content 53 | 54 | ``` 55 | bool cartesian_space 56 | float64[] joints_position 57 | float64[] joints_position_reference 58 | float64[] task_pose_reference 59 | float64[] task_wrench 60 | float64[] task_precision 61 | ``` 62 | 63 | By filling these fields, and cyclically sending them on a topic, the package computes the required stiffness and damping to be used. As you can notice, we give the possibility to specify the 64 | desired robot configuration in terms of the Cartesian pose of the end-effector. By putting true to the boolean cartesian_space, the package will ignore the field `joints_position_reference` and it will use `task_pose_reference` expressed as translation and orientation. The orientation is required to be expressed in Euler angles RPY and in radiants. 65 | 66 | 67 | [ATTENTION] Due to the technical details of the implemented algorithm, the package will further provide a torque to be sent to the motors as well. This is due to two reasons, one is related to the control theory and the second is due to practical reasons. Specifically, to determine a robot impedance that satisfies task requirements, the algorithm computes a full stiffness matrix. To realize the computed stiffness thus is required to send to the motors this additional torque. 68 | 69 | The result of the calculated impedance is sent via message on a topic. The message sent is a custom message and it is provided in the package as well. It is named _ImpedanceMsg.msg_ and it is in the _msg folder_. Its content is 70 | 71 | ``` 72 | float64[] robot_stiffness 73 | float64[] robot_damping 74 | float64[] robot_feedforward_torque 75 | ``` 76 | 77 | So, by subscribing to the message you can use these values to control your robot by setting these values in the motors. 78 | 79 | ## How to Configure It 80 | 81 | As we have seen, to use the package, it is necessary to 82 | 83 | 1. Have a URDF file of the robot. 84 | 2. Send a message to give the task information. 85 | 3. Retrieve the result by listening to a topic. 86 | 87 | But, to make everything effectively working we need to properly configure the package before using it. In particular, it is required to 88 | 89 | 1. provide the URDF file by specifying the path where the file is. 90 | 2. set the names of the topics. 91 | 3. set the initial configuration of the robot, defined in terms of joint position at the initial instant time. 92 | 4. set the maximum stiffness and damping available by the motors. 93 | 5. set the preset stiffness and damping configured in the robot. 94 | 6. set the initial force and precision to set on the robot. 95 | 7. specify the base frame of the kinematic chain intended to control (the same as in the URDF file). 96 | 8. specify the tip frame of the kinematic chain intended to control (the same as in the URDF file). 97 | 9. set the transition time (seconds) during which the impedance of the robot is adjusted from its preset values to the one intended to start the task. This is to ensure the smoothness of the impedance profiles. Messages related to this phase will be sent on the topic and must be actuated. 98 | 10. set the rate of the plugin. 99 | 11. set the path where you would like to save the logs (the package provides them). 100 | 12. set the verbose mode if you like. 101 | 102 | This can be simply done just by inserting the proper values in the configuration file. The file is named _impedance_modulation_settings.yaml_ present in the _config folder_. The ROS node will start by loading these parameters. Here, we report its content 103 | 104 | ```yaml 105 | /robot/impedance_settings: 106 | ros__parameters: 107 | robot_initial_config: [0.0,-1.56,0.9,0.2,-0.5,1.12] 108 | stiffness_preset: [100.0,100.0,100.0,100.0,100.0,100.0] 109 | stiffness_constant: [200.0,200.0,200.0,200.0,200.0,200.0] 110 | stiffness_maximum: [2000.0,2000.0,2000.0,2000.0,2000.0,2000.0] 111 | damping_preset: [100.0,100.0,100.0,100.0,100.0,100.0] 112 | damping_maximum: [50.0,50.0,50.0,50.0,50.0,50.0] 113 | wrench_initial: [0.0,0.0,0.0,0.0,0.0,0.0] 114 | precision_initial: [0.1,0.1,0.1,0.1,0.1,0.1] 115 | robot_urdf_model_path: "/home/liana/ros2_humble/src/ROS2UtilityNodes/urdf/inail2arm.urdf" 116 | robot_base_frame_name: "base_link" 117 | robot_tip_frame_name: "arm1_6" 118 | topic_subscriber_name: "/robot/task_planning" 119 | topic_publisher_name: "impedance_planning" 120 | transition_time: 10.0 121 | rate: 1000 122 | log_path: "/tmp/impedance_planner" 123 | verbose: true 124 | ``` 125 | 126 | The values here are just an example. You have to make the proper changes. Please, do not touch any names (the green ones), just modify the fields (the blue ones). 127 | 128 | ## Installation 129 | **To Install** Variable Impedance Modulation Package run 130 | ```console 131 | git clone https://github.com/ADVRHumanoids/RobotImpedanceModulation.git 132 | ``` 133 | inside the _**src folder**_ of the workspace run 134 | ```console 135 | colcon build 136 | ``` 137 | 138 | ## Execution 139 | To execute the code, just run: 140 | ```console 141 | ros2 launch rim impedance_modulation.launch.py 142 | ``` 143 | 144 | Remember to source `install/setup.sh`, by simply 145 | ```console 146 | source install/setup.sh 147 | ``` 148 | inside the principal folder. If everything has been properly set, you should have an output similar to 149 | ```console 150 | liana:~/ros2_humble$ ros2 launch rim impedance_modulation.launch.py 151 | [INFO] [launch]: All log files can be found below /home/liana/.ros/log/2023-07-08-15-45-00-723821-liana-MS-7820-20325 152 | [INFO] [launch]: Default logging verbosity is set to INFO 153 | [INFO] [ImpedanceModulation-1]: process started with pid [20326] 154 | [ImpedanceModulation-1] [INFO] [1688823900.952604282] [robot.impedance_settings]: I am initializing the ROS node... 155 | [ImpedanceModulation-1] 156 | [ImpedanceModulation-1] Worked for 0.00 sec (0.0 MB flushed)....average load is -nan 157 | [ImpedanceModulation-1] [INFO] [1688823900.983917845] [robot.impedance_settings]: I am loading the ROS node params... 158 | [ImpedanceModulation-1] 159 | [ImpedanceModulation-1] [INFO] [1688823900.990769844] [robot.impedance_settings]: I am initializing the robot impedance... 160 | [ImpedanceModulation-1] 161 | [ImpedanceModulation-1] Created variable 'EEwrench' (20 blocks, 500 elem each) 162 | [ImpedanceModulation-1] Created variable 'EETaskwrench' (20 blocks, 500 elem each) 163 | [ImpedanceModulation-1] Created variable 'TaskErrorDesired' (20 blocks, 500 elem each) 164 | [ImpedanceModulation-1] Created variable 'CartesianStiffness' (20 blocks, 500 elem each) 165 | [ImpedanceModulation-1] Created variable 'JointStiffness' (20 blocks, 500 elem each) 166 | [ImpedanceModulation-1] Created variable 'JointDamping' (20 blocks, 500 elem each) 167 | [ImpedanceModulation-1] Created variable 'FeedforwardTorque' (20 blocks, 500 elem each) 168 | [ImpedanceModulation-1] Created variable 'JointsPosition' (20 blocks, 500 elem each) 169 | [ImpedanceModulation-1] Created variable 'JointsPositionReference' (20 blocks, 500 elem each) 170 | [ImpedanceModulation-1] Worked for 0.02 sec (2.6 MB flushed)....average load is 0.00 171 | [ImpedanceModulation-1] ------------------------------------------------------------------------------ 172 | [ImpedanceModulation-1] Robot Impedance Modulation started! Ready to accept task planning... 173 | ``` 174 | 175 | If yes, then your package is listening to messages and ready to compute the impedance. 176 | 177 | ## Dependences 178 | The Robot Impedance Modulation requires the following dependencies: 179 | * [Eigen](https://eigen.tuxfamily.org/index.php?title=Main_Page): to handle with basic algebra; 180 | * [MatLogger2](https://github.com/ADVRHumanoids/MatLogger2): to log data; 181 | * [KDL](https://www.orocos.org/kdl.html): to handle robot quantities. 182 | 183 | ## Maintaner 184 | 185 | || 186 | |:-------------:| 187 | |Liana Bertoni| 188 | |liana.bertoni at iit.it| 189 | 190 | -------------------------------------------------------------------------------- /config/impedance_modulation_settings.yaml: -------------------------------------------------------------------------------- 1 | /robot/impedance_settings: 2 | ros__parameters: 3 | robot_initial_config: [0.0,-1.56,0.9,0.2,-0.5,1.12] 4 | stiffness_preset: [100.0,100.0,100.0,100.0,100.0,100.0] 5 | stiffness_constant: [200.0,200.0,200.0,200.0,200.0,200.0] 6 | stiffness_maximum: [2000.0,2000.0,2000.0,2000.0,2000.0,2000.0] 7 | damping_preset: [100.0,100.0,100.0,100.0,100.0,100.0] 8 | damping_maximum: [50.0,50.0,50.0,50.0,50.0,50.0] 9 | wrench_initial: [0.0,0.0,0.0,0.0,0.0,0.0] 10 | precision_initial: [0.1,0.1,0.1,0.1,0.1,0.1] 11 | robot_urdf_model_path: "/home/liana/ros2_humble/src/ROS2UtilityNodes/urdf/inail2arm.urdf" 12 | robot_base_frame_name: "base_link" 13 | robot_tip_frame_name: "arm1_6" 14 | topic_subscriber_name: "/robot/task_planning" 15 | topic_publisher_name: "impedance_planning" 16 | transition_time: 10.0 17 | rate: 1000 18 | log_path: "/tmp/impedance_planner" 19 | verbose: true -------------------------------------------------------------------------------- /include/ImpedanceModulation/ImpedanceModulationManager.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2023 IIT-HHCM 3 | * Author: Liana Bertoni 4 | * email: liana.bertoni@iit.it 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #ifndef IMPEDANCE_MODULATION_ 19 | #define IMPEDANCE_MODULATION_ 20 | 21 | // ROS 22 | #include 23 | #include "rclcpp/rclcpp.hpp" 24 | #include "rim/msg/task_msg.hpp" 25 | #include "rim/msg/impedance_msg.hpp" 26 | 27 | // utils 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | using std::placeholders::_1; 34 | using namespace std::chrono_literals; 35 | /** 36 | * @brief ImpedanceModulation class is a inherited ControlPlugin Class 37 | * implements joint impedance modulation. 38 | */ 39 | class ImpedanceModulationManager 40 | { 41 | 42 | public: 43 | 44 | /////////////// COSTRUCTOR //////////////////// 45 | /* costructor of the class */ 46 | ImpedanceModulationManager( std::string ns = "" ); 47 | /////////// COMPUTATION functions //////////// 48 | /** 49 | * callback of the ROS Node 50 | * @return void 51 | */ 52 | void timer_callback(); 53 | /** 54 | * spin function 55 | * @return void 56 | */ 57 | void spin(); 58 | /////////////// DISTRUCTOR //////////////////// 59 | /* distructor of the class */ 60 | ~ImpedanceModulationManager(); 61 | 62 | private: 63 | 64 | /** 65 | * init ROS Node 66 | * @return void 67 | */ 68 | void initROSNode(); 69 | /** 70 | * load params 71 | * @return void 72 | */ 73 | void loadParam(); 74 | /** 75 | * init vars 76 | * @return void 77 | */ 78 | void initVars(); 79 | /** 80 | * init robot impedance stiffness and damping 81 | * @return void 82 | */ 83 | void initRobotImpedance(); 84 | /** 85 | * compute robot impedance plus torques [stiffness,damping,torques] 86 | * @return void 87 | */ 88 | void computeRobotImpedanceModulation(); 89 | /** 90 | * @return void 91 | */ 92 | void checkLimits(); 93 | /** 94 | * subscribe task params 95 | * @return void 96 | */ 97 | void TaskParamSubscriberCallback(const rim::msg::TaskMsg::SharedPtr msg); 98 | /** 99 | * create robot message 100 | * @return void 101 | */ 102 | void composeRobotMsg(); 103 | /** 104 | * compute jacobian 105 | */ 106 | Eigen::MatrixXd getJacobian(); 107 | /** 108 | * compute gravity wrench 109 | */ 110 | Eigen::VectorXd getGravityWrench(); 111 | /** 112 | * compute gravity torque 113 | */ 114 | Eigen::VectorXd getGravityCompensation(); 115 | /** 116 | * log data 117 | * @return void 118 | */ 119 | void log(); 120 | 121 | // ROS ---------------------------------------------------------------- 122 | rclcpp::Node::SharedPtr _nh; /* ROSE node handle */ 123 | rclcpp::TimerBase::SharedPtr _timer; 124 | rclcpp::Subscription::SharedPtr _taskSubscriber; /* ROS topic object model subscriber */ 125 | rclcpp::Publisher::SharedPtr _robotPublisher; /* ROS topic torques publisher */ 126 | std::string _topicSub; 127 | std::string _topicPub; 128 | double _rate; 129 | 130 | // Utilities ---------------------------------------------------------- 131 | Algebra _algebra; // algebra functions container 132 | Utilities _utils; // utils 133 | LoggerL _loggerL; // data logger 134 | std::string _logPath; 135 | 136 | 137 | KDLHelper _kdl; // kdl 138 | std::string _robotURDFModelPath; 139 | std::string _base; 140 | std::string _endEffector; 141 | 142 | // Data ---------------------------------------------------------------- 143 | // Cartesian Space 144 | int _nc = 6; 145 | Eigen::VectorXd _w_ee_d; 146 | Eigen::VectorXd _w_ee_td; 147 | Eigen::VectorXd _w_ee_g; 148 | Eigen::VectorXd _w_ee_d_initial; 149 | 150 | Eigen::VectorXd _delta_x_ee_td; 151 | Eigen::VectorXd _delta_x_ee_initial; 152 | 153 | Eigen::VectorXd _k_c_d; 154 | Eigen::MatrixXd _K_C_d; 155 | 156 | // Joint Space 157 | int _nj; 158 | double _csi = 0.7; 159 | Eigen::MatrixXd _K_0; 160 | Eigen::MatrixXd _K_J; 161 | Eigen::MatrixXd _D_J; 162 | Eigen::MatrixXd _K_J_off; 163 | 164 | Eigen::VectorXd _k; 165 | Eigen::VectorXd _k_0; 166 | Eigen::VectorXd _k_preset; 167 | Eigen::VectorXd _k_max; 168 | 169 | Eigen::VectorXd _d; 170 | Eigen::VectorXd _d_preset; 171 | Eigen::VectorXd _d_max; 172 | 173 | Eigen::VectorXd _tau_ff; 174 | Eigen::VectorXd _tau_g; 175 | Eigen::VectorXd _tau_ext; 176 | Eigen::VectorXd _tau_Koff; 177 | 178 | Eigen::MatrixXd _J; // Jacobian 179 | Eigen::VectorXd _q,_qref,_qerr; // joints variable 180 | 181 | double _ref_time; 182 | 183 | rim::msg::ImpedanceMsg _robotMsg; 184 | 185 | // IM status -------------------------------------------------------- 186 | bool _subscribed; 187 | bool _verbose; 188 | }; 189 | 190 | #endif // JOINT_IMPEDANCE_CONTROL_H 191 | -------------------------------------------------------------------------------- /include/ImpedanceModulation/utils/KDLhelper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2023 IIT-HHCM 3 | * Author: Liana Bertoni 4 | * email: liana.bertoni@iit.it 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #ifndef KDL_HELPER_ 19 | #define KDL_HELPER_ 20 | 21 | // std library 22 | #include 23 | #include 24 | 25 | // Eigen 26 | #include 27 | #include 28 | 29 | // kdl 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | // #include 42 | 43 | /** 44 | * @brief Class KDLHelper 45 | * 46 | */ 47 | class KDLHelper 48 | { 49 | // 50 | public: 51 | /////////////// COSTRUCTOR //////////////////// 52 | /* costructor of the class */ 53 | KDLHelper(); 54 | 55 | // init 56 | void init(std::string fileURDFpath, std::string base_frame, std::string tip_frame); 57 | 58 | // set joints position 59 | void setJointsPositions(Eigen::VectorXd q); 60 | 61 | // set frame pose 62 | void setPose(Eigen::VectorXd pose); 63 | 64 | // compute fk 65 | void FK(); 66 | 67 | // compute ik 68 | void IK(); 69 | 70 | // get frame pose 71 | Eigen::VectorXd getPose(); 72 | 73 | // get joints positions 74 | Eigen::VectorXd getJointsPositions(); 75 | 76 | // get jacobian 77 | Eigen::MatrixXd getJacobian(); 78 | 79 | // get gravity torque 80 | Eigen::VectorXd getGravity(); 81 | 82 | /////////////// DISTRUCTOR //////////////////// 83 | /* distructor of the class */ 84 | ~KDLHelper(); 85 | 86 | private: 87 | 88 | // Robot data ----------------------------------------------------- 89 | unsigned int _nc; 90 | unsigned int _nj; 91 | KDL::Tree _tree; 92 | KDL::Chain _chain; 93 | KDL::Frame _pose; 94 | KDL::JntArray _q; 95 | 96 | // Solvers --------------------------------------------------------- 97 | std::unique_ptr _fksolver; 98 | std::unique_ptr _iksolver1v;//Inverse velocity solver 99 | std::unique_ptr _iksolver; 100 | 101 | std::unique_ptr _jnt_to_jac_solver; 102 | KDL::Jacobian _jacobian; 103 | 104 | std::unique_ptr _dyn; 105 | 106 | }; 107 | 108 | #endif // KDL_HELPER_ -------------------------------------------------------------------------------- /include/ImpedanceModulation/utils/algebra.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2023 IIT-HHCM 3 | * Author: Liana Bertoni 4 | * email: liana.bertoni@iit.it 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #ifndef ALGEBRA_ 19 | #define ALGEBRA_ 20 | 21 | // Eigen 22 | #include 23 | #include 24 | 25 | /** 26 | * @brief Class Algebra 27 | * 28 | */ 29 | class Algebra 30 | { 31 | // 32 | public: 33 | /////////////// COSTRUCTOR //////////////////// 34 | /* costructor of the class */ 35 | Algebra(); 36 | 37 | // pseudo inverse 38 | Eigen::MatrixXd pseudoInverse(Eigen::MatrixXd M_); 39 | 40 | // get skew symmetric matrix 41 | void getSkewMatrix(Eigen::VectorXd p, Eigen::MatrixXd& Skew); 42 | 43 | // quat to euler angles in degrees 44 | Eigen::VectorXd quatToEulerDeg(Eigen::VectorXd quat); 45 | // quat to euler angles in rads 46 | Eigen::VectorXd quatToEulerRad(Eigen::VectorXd quat); 47 | // quat to euler 48 | Eigen::VectorXd quatToEuler(Eigen::VectorXd quat, bool areDegrees); 49 | // eulers to quats - rad 50 | Eigen::VectorXd eulersToQuatsRad(Eigen::VectorXd eulerAngleOrients); 51 | // eulers to quats - deg 52 | Eigen::VectorXd eulersToQuatsDeg(Eigen::VectorXd eulerAngleOrients); 53 | // eulers to quats 54 | Eigen::VectorXd eulersToQuats(Eigen::VectorXd eulerAngleOrients, bool areDegrees); 55 | // euler to quat - return vec type 56 | Eigen::VectorXd EulerToQuatVec(Eigen::VectorXd eulerAngles); 57 | // euler to quat - return quat type 58 | Eigen::Quaternionf EulerToQuat(Eigen::VectorXd eulerAngles); 59 | // rot to quat x y z w 60 | Eigen::VectorXd RotToQuat(Eigen::Matrix3d rot); 61 | 62 | // get tranform 63 | void getTransformationRotTrasl(Eigen::Affine3d T, Eigen::Matrix3d& rot, Eigen::VectorXd& trasl); 64 | 65 | // min to T 66 | void minToT(Eigen::VectorXd position, Eigen::VectorXd orientation, Eigen::Affine3d& T, bool areDegree); 67 | // T to min 68 | void TToMin(Eigen::Affine3d T, Eigen::VectorXd& t); 69 | // T to min 70 | void TToMinDeg(Eigen::Affine3d T, Eigen::VectorXd& t); 71 | // T to min 72 | void TToMinQuat(Eigen::Affine3d T, Eigen::VectorXd& t); 73 | 74 | // deg To rad 75 | void degTorad(Eigen::VectorXd deg, Eigen::VectorXd& rad); 76 | // rad to deg 77 | Eigen::VectorXd radTodeg(Eigen::VectorXd vecRad); 78 | 79 | // T to min 80 | Eigen::VectorXd TToMin(Eigen::Affine3d T); 81 | // T to min 82 | Eigen::VectorXd TToMinDeg(Eigen::Affine3d T); 83 | // T to min 84 | Eigen::VectorXd TToMinQuat(Eigen::Affine3d T); 85 | 86 | /////////////// DISTRUCTOR //////////////////// 87 | /* distructor of the class */ 88 | ~Algebra(); 89 | }; 90 | 91 | #endif // ALGEBRA_ -------------------------------------------------------------------------------- /include/ImpedanceModulation/utils/logger.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2023 IIT-HHCM 3 | * Author: Liana Bertoni 4 | * email: liana.bertoni@iit.it 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #ifndef LOGGER_ 19 | #define LOGGER_ 20 | 21 | // matlogger 22 | #include 23 | #include 24 | // Eigen 25 | #include 26 | 27 | /** 28 | * @brief Class Data Logger 29 | * 30 | */ 31 | class LoggerL 32 | { 33 | // Logger --------------------------------------- 34 | XBot::MatLogger2::Ptr _logger; /* mt logger */ 35 | XBot::MatAppender::Ptr _appender; /* mt appender */ 36 | 37 | public: 38 | /* costructor of the class */ 39 | LoggerL(); 40 | /** 41 | * init plannerLogger 42 | * @param logFilesPath: string of the path file 43 | * @return void 44 | */ 45 | void initLogger(std::string logFilesPath); 46 | /** 47 | * set logger mode 48 | * @param setMode: true: fixed buffer false: unlimited 49 | * @return void 50 | */ 51 | void setLogger(bool setMode); 52 | /** 53 | * log data eigen vector data 54 | * @return void 55 | */ 56 | bool logData(std::string dataName, Eigen::VectorXd data); 57 | /** 58 | * log data double data 59 | * @return void 60 | */ 61 | bool logData(std::string dataName, double data); 62 | 63 | /* distructor of the class */ 64 | ~LoggerL(); 65 | 66 | }; 67 | 68 | #endif // LOGGER_ -------------------------------------------------------------------------------- /include/ImpedanceModulation/utils/utilities.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2023 IIT-HHCM 3 | * Author: Liana Bertoni 4 | * email: liana.bertoni@iit.it 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #ifndef UTILITIES_ 19 | #define UTILITIES_ 20 | 21 | // utils 22 | #include 23 | 24 | // vector 25 | #include 26 | 27 | /** 28 | * @brief Class Utils 29 | * 30 | */ 31 | class Utilities 32 | { 33 | // utils --------------------------------------------------- 34 | Algebra _algebra; 35 | 36 | public: 37 | 38 | /////////////// COSTRUCTOR //////////////////// 39 | /* costructor of the class */ 40 | Utilities(); 41 | 42 | // std to eigen 43 | Eigen::VectorXd toEigen(std::vector vec); 44 | // eigen to std 45 | std::vector toStdVector(Eigen::VectorXd vecEigen); 46 | 47 | // min repre to quat repr for a pose 48 | std::vector toQuaternionPose(std::vector poses, bool areDegrees); 49 | 50 | /////////////// DISTRUCTOR //////////////////// 51 | /* distructor of the class */ 52 | ~Utilities(); 53 | }; 54 | 55 | #endif // UTILITIES_ -------------------------------------------------------------------------------- /launch/impedance_modulation.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | from launch import LaunchDescription 6 | from launch_ros.actions import Node 7 | 8 | def generate_launch_description(): 9 | config = os.path.join( 10 | get_package_share_directory('impedance_modulation'), 11 | 'config', 12 | 'impedance_modulation_settings.yaml' 13 | ) 14 | return LaunchDescription([ 15 | Node( 16 | package='impedance_modulation', 17 | executable='ImpedanceModulation', 18 | namespace='robot', 19 | name='impedance_settings', 20 | parameters=[config], 21 | output="screen", 22 | emulate_tty=True 23 | ) 24 | ]) -------------------------------------------------------------------------------- /msg/ImpedanceMsg.msg: -------------------------------------------------------------------------------- 1 | # Robot message is given by 2 | # 3 | # - robot_stiffness R^nj - k 4 | # - robot_damping R^nj - d 5 | # - robot_feedforward_torque R^nj - tauff 6 | # 7 | # 8 | # where nj represents the number of joints considered in the chain. 9 | # These joints are considered as actuated joints. 10 | # The elements above constitute the following robot control law 11 | # 12 | # K = diag{k}; 13 | # D = diag{d}; 14 | # tau = K * ( qdes - q ) - D * qdot + tauff 15 | 16 | 17 | float64[] robot_stiffness 18 | float64[] robot_damping 19 | float64[] robot_feedforward_torque 20 | -------------------------------------------------------------------------------- /msg/TaskMsg.msg: -------------------------------------------------------------------------------- 1 | # Task message is given by 2 | # 3 | # - task pose R^6 [x,y,z,roll,pitch,yaw] {meters,radiants} 4 | # - task wrench R^6 [fx,fy,fz,mx,my,mz] 5 | # - task precision R^6 [x,y,z,roll,pitch,yaw] {meters,radiants} 6 | # 7 | # These quantities should come from a task planner. 8 | # > If the task wrench is not known, you can insert zero, 9 | # > If the task precision is not known, you CAN NOT insert zero 10 | # Please specify a value for your application even if it is a rough approssimation. 11 | 12 | bool cartesian_space 13 | float64[] joints_position 14 | float64[] joints_position_reference 15 | float64[] task_pose_reference 16 | float64[] task_wrench 17 | float64[] task_precision 18 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | rim 7 | 0.0.0 8 | robot impedance modulation 9 | liana 10 | Apache-2.0 11 | 12 | ament_cmake 13 | 14 | rosidl_default_generators 15 | rosidl_default_runtime 16 | rosidl_interface_packages 17 | 18 | ros2launch 19 | 20 | rclcpp 21 | eigen 22 | 23 | ament_lint_auto 24 | ament_lint_common 25 | rosidl_default_generators 26 | rosidl_default_runtime 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /presentation/ROSCON23-Impedance-BERTONI-PDF.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/RobotImpedanceModulation/ce560ac698204ddc2e35c1efebf6307f3ef3150f/presentation/ROSCON23-Impedance-BERTONI-PDF.pdf -------------------------------------------------------------------------------- /src/ImpedanceModulation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2023 IIT-HHCM 3 | * Author: Liana Bertoni 4 | * email: liana.bertoni@iit.it 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #include 19 | #include "rclcpp/rclcpp.hpp" 20 | #include 21 | 22 | int main(int argc, char **argv) 23 | { 24 | 25 | rclcpp::init(argc, argv); 26 | ImpedanceModulationManager manager("impedance_modulation"); 27 | manager.spin(); 28 | 29 | return 0; 30 | } 31 | -------------------------------------------------------------------------------- /src/ImpedanceModulationManager.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2023 IIT-HHCM 3 | * Author: Liana Bertoni 4 | * email: liana.bertoni@iit.it 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #include 19 | 20 | ImpedanceModulationManager::ImpedanceModulationManager ( std::string ns ) 21 | { 22 | // ros2 'init' 23 | _nh = rclcpp::Node::make_shared(ns); 24 | 25 | //initialization ROS node 26 | RCLCPP_INFO(_nh->get_logger(), "%s\n","I am initializing the ROS node..."); 27 | initROSNode(); 28 | 29 | //initialization ROS node 30 | RCLCPP_INFO(_nh->get_logger(), "%s\n","I am loading the ROS node params..."); 31 | loadParam(); 32 | 33 | //initialization ROS node 34 | RCLCPP_INFO(_nh->get_logger(), "%s\n","I am initializing the robot impedance..."); 35 | initRobotImpedance(); 36 | 37 | } 38 | 39 | void ImpedanceModulationManager::initROSNode() 40 | { 41 | // init ROS node 42 | int rate; 43 | _nh->declare_parameter("rate", 1000); 44 | _nh->get_parameter("rate", rate); 45 | _rate = rate; 46 | _timer = _nh->create_wall_timer( std::chrono::milliseconds(rate),std::bind(&ImpedanceModulationManager::timer_callback, this)); 47 | 48 | _nh->declare_parameter("topic_subscriber_name","task_planner"); 49 | _nh->declare_parameter("topic_publisher_name","robot_IM_planner"); 50 | _nh->get_parameter("topic_subscriber_name",_topicSub); 51 | _nh->get_parameter("topic_publisher_name",_topicPub); 52 | _taskSubscriber = _nh->create_subscription(_topicSub,1, std::bind(&ImpedanceModulationManager::TaskParamSubscriberCallback, this, _1)); 53 | _robotPublisher = _nh->create_publisher(_topicPub,1000); 54 | 55 | // logger 56 | _nh->declare_parameter("log_path","/tmp/"); 57 | _nh->get_parameter("log_path",_logPath); 58 | _loggerL.initLogger(_logPath); 59 | _loggerL.setLogger(false); // false:unlimited true:limited buffer 60 | } 61 | 62 | void ImpedanceModulationManager::loadParam() 63 | { 64 | std::vector q0; 65 | std::vector k_preset; 66 | std::vector k_0; 67 | std::vector k_max; 68 | std::vector d_preset; 69 | std::vector d_max; 70 | std::vector w_ee_d_initial; 71 | std::vector delta_x_ee_initial; 72 | 73 | // declaring params with default values 74 | _nh->declare_parameter("verbose",false); 75 | _nh->declare_parameter("stiffness_preset",k_preset); 76 | _nh->declare_parameter("stiffness_constant",k_0); 77 | _nh->declare_parameter("stiffness_maximum",k_max); 78 | 79 | _nh->declare_parameter("damping_preset",d_preset); 80 | _nh->declare_parameter("damping_maximum",d_max); 81 | 82 | _nh->declare_parameter("wrench_initial",w_ee_d_initial); 83 | _nh->declare_parameter("precision_initial",delta_x_ee_initial); 84 | 85 | _nh->declare_parameter("robot_initial_config",q0); 86 | _nh->declare_parameter("robot_urdf_model_path","/tmp/robot.urdf"); 87 | _nh->declare_parameter("robot_base_frame_name","base_link"); 88 | _nh->declare_parameter("robot_tip_frame_name","end_effector"); 89 | 90 | _nh->declare_parameter("transition_time",5.0); 91 | 92 | // getting params with their proper values 93 | _nh->get_parameter("verbose",_verbose); 94 | _nh->get_parameter("stiffness_preset",k_preset); 95 | _nh->get_parameter("stiffness_constant",k_0); 96 | _nh->get_parameter("stiffness_maximum",k_max); 97 | 98 | _nh->get_parameter("damping_preset",d_preset); 99 | _nh->get_parameter("damping_maximum",d_max); 100 | 101 | _nh->get_parameter("wrench_initial",w_ee_d_initial); 102 | _nh->get_parameter("precision_initial",delta_x_ee_initial); 103 | 104 | _nh->get_parameter("robot_initial_config",q0); 105 | _nh->get_parameter("robot_urdf_model_path",_robotURDFModelPath); 106 | _nh->get_parameter("robot_base_frame_name",_base); 107 | _nh->get_parameter("robot_tip_frame_name",_endEffector); 108 | 109 | _nh->get_parameter("transition_time",_ref_time); 110 | 111 | _k_preset = _utils.toEigen(k_preset); 112 | _k_0 = _utils.toEigen(k_0); 113 | _k_max = _utils.toEigen(k_max); 114 | _d_preset = _utils.toEigen(d_preset); 115 | _d_max = _utils.toEigen(d_max); 116 | _w_ee_d_initial = _utils.toEigen(w_ee_d_initial); 117 | _delta_x_ee_initial = _utils.toEigen(delta_x_ee_initial); 118 | _q = _utils.toEigen(q0); 119 | 120 | _kdl.init(_robotURDFModelPath,_base,_endEffector); 121 | _kdl.setJointsPositions(_q); 122 | 123 | initVars(); 124 | } 125 | 126 | void ImpedanceModulationManager::initVars() 127 | { 128 | // cartesian space 129 | _w_ee_d = Eigen::VectorXd::Zero(_nc); 130 | _w_ee_td = Eigen::VectorXd::Zero(_nc); 131 | _w_ee_g = Eigen::VectorXd::Zero(_nc); 132 | _delta_x_ee_td = Eigen::VectorXd::Zero(_nc); 133 | _k_c_d = Eigen::VectorXd::Zero(_nc); 134 | _K_C_d = Eigen::MatrixXd::Identity(_nc,_nc); 135 | 136 | // joint space 137 | _nj = _k_0.size(); 138 | _qref = Eigen::VectorXd::Zero(_nj); 139 | _qerr = Eigen::VectorXd::Zero(_nj); 140 | 141 | _k = Eigen::VectorXd::Zero(_nj); 142 | _d = Eigen::VectorXd::Zero(_nj); 143 | _K_0 = _k_0.asDiagonal(); 144 | _K_J = Eigen::MatrixXd::Identity(_nj,_nj); 145 | _D_J = Eigen::MatrixXd::Identity(_nj,_nj); 146 | _K_J_off = Eigen::MatrixXd::Identity(_nj,_nj); 147 | 148 | _tau_g = Eigen::VectorXd::Zero(_nj); 149 | _tau_ext = Eigen::VectorXd::Zero(_nj); 150 | _tau_Koff= Eigen::VectorXd::Zero(_nj); 151 | _tau_ff= Eigen::VectorXd::Zero(_nj); 152 | 153 | // state 154 | _subscribed = false; 155 | } 156 | 157 | void ImpedanceModulationManager::initRobotImpedance() 158 | { 159 | // smoothing 160 | double t = 0.0; 161 | double time = 0.0; 162 | double it_time = 0.0; 163 | 164 | Eigen::VectorXd taug; 165 | taug = getGravityCompensation(); 166 | Eigen::MatrixXd _J_invT; 167 | _J = getJacobian(); 168 | 169 | _J_invT = _algebra.pseudoInverse(_J.transpose()); 170 | 171 | Eigen::VectorXd k; 172 | Eigen::VectorXd d; 173 | 174 | // smoothing stiffness and damping and torque 175 | while( it_time < 1.0 ) 176 | { 177 | it_time = time/_ref_time; 178 | t = ((6*it_time - 15)*it_time + 10)*it_time*it_time*it_time; 179 | 180 | _tau_g = ( 1 - t ) * Eigen::VectorXd::Zero(_nj) + t * taug; 181 | _w_ee_td = ( 1 - t ) * Eigen::VectorXd::Zero(_nj) + t * _w_ee_d_initial; 182 | _w_ee_g = _J_invT * _tau_g; 183 | 184 | _w_ee_d = _w_ee_g + _w_ee_td; 185 | _w_ee_d = _w_ee_d.cwiseAbs(); 186 | _k_c_d = _w_ee_d.cwiseProduct(_delta_x_ee_initial.cwiseInverse()); 187 | _K_C_d = _k_c_d.asDiagonal(); 188 | _K_J = _K_0 + _J.transpose() * _K_C_d * _J; 189 | 190 | k = _K_J.diagonal(); 191 | d = 2*_csi*_k.cwiseAbs().cwiseSqrt(); 192 | 193 | Eigen::MatrixXd Kapp; 194 | Kapp = k.asDiagonal(); 195 | _K_J_off = _K_J - Kapp; 196 | 197 | _tau_Koff = ( 1 - t ) * Eigen::VectorXd::Zero(_nj) + t * _K_J_off * _qerr; 198 | _tau_ext = _J.transpose() * _w_ee_td; 199 | 200 | _k = ( 1 - t ) * _k_preset + t * k; 201 | _d = ( 1 - t ) * _d_preset + t * d; 202 | _tau_ff = _tau_g + _tau_Koff + _tau_ext; 203 | 204 | checkLimits(); 205 | 206 | // compose robot msg 207 | composeRobotMsg(); 208 | _robotPublisher->publish(_robotMsg); 209 | 210 | // iteration step 211 | time += 1/_rate; 212 | log(); 213 | } 214 | } 215 | 216 | void ImpedanceModulationManager::computeRobotImpedanceModulation() 217 | { 218 | // cartesian stiffness calculations 219 | _w_ee_g = getGravityWrench(); 220 | _w_ee_d = _w_ee_g + _w_ee_td; 221 | _w_ee_d = _w_ee_d.cwiseAbs(); 222 | _k_c_d = _w_ee_d.cwiseProduct(_delta_x_ee_td.cwiseInverse()); 223 | 224 | // joints stiffness calculations 225 | _J = getJacobian(); 226 | _K_C_d = _k_c_d.asDiagonal(); 227 | _K_J = _K_0 + _J.transpose() * _K_C_d * _J; 228 | _k = _K_J.diagonal(); 229 | 230 | Eigen::MatrixXd Kapp; 231 | Kapp = _k.asDiagonal(); 232 | _K_J_off = _K_J - Kapp; 233 | 234 | // joints damping calculations 235 | _d = 2*_csi*_k.cwiseAbs().cwiseSqrt(); 236 | _D_J.diagonal() = _d; 237 | 238 | // torques calculations 239 | _qerr = _qref - _q; 240 | _tau_g = getGravityCompensation(); 241 | _tau_Koff = _K_J_off * _qerr; 242 | _tau_ext = _J.transpose() * _w_ee_td; 243 | _tau_ff =_tau_g + _tau_Koff + _tau_ext; 244 | 245 | // check limits for motors 246 | checkLimits(); 247 | } 248 | 249 | void ImpedanceModulationManager::checkLimits() 250 | { 251 | for (int i = 0; i < _nj; ++i) 252 | { 253 | if( _k(i) > _k_max(i) ) 254 | _k(i) = _k_max(i); 255 | 256 | if( _d(i) > _d_max(i) ) 257 | _d(i) = _d_max(i); 258 | } 259 | } 260 | 261 | void ImpedanceModulationManager::composeRobotMsg() 262 | { 263 | // compose message 264 | _robotMsg.robot_stiffness = _utils.toStdVector(_k); 265 | _robotMsg.robot_damping = _utils.toStdVector(_d); 266 | _robotMsg.robot_feedforward_torque = _utils.toStdVector(_tau_ff); 267 | } 268 | 269 | void ImpedanceModulationManager::timer_callback() 270 | { 271 | // control loop variable impedance modulation 272 | if (_subscribed) 273 | { 274 | if(_verbose) 275 | { 276 | std::cout << "Subscribed data..." << std::endl; 277 | std::cout << "Joints state: " << _q.transpose() << std::endl; 278 | std::cout << "Joints reference: " << _qref.transpose() << std::endl; 279 | std::cout << "Wrench reference: " << _w_ee_td.transpose() << std::endl; 280 | std::cout << "Precision reference: " << _delta_x_ee_td.transpose() << std::endl; 281 | } 282 | 283 | // iteration for the planning 284 | computeRobotImpedanceModulation(); 285 | composeRobotMsg(); 286 | 287 | // pub 288 | _robotPublisher->publish(_robotMsg); 289 | 290 | // log 291 | log(); 292 | 293 | _subscribed = false; 294 | } 295 | } 296 | 297 | void ImpedanceModulationManager::spin() 298 | { 299 | std::cout << "------------------------------------------------------------------------------" << std::endl; 300 | std::cout << "Robot Impedance Modulation started! Ready to accept task planning..." << std::endl; 301 | rclcpp::spin(_nh); 302 | } 303 | 304 | void ImpedanceModulationManager::TaskParamSubscriberCallback(const rim::msg::TaskMsg::SharedPtr msg) 305 | { 306 | // task param : inputs 307 | if(msg->cartesian_space) 308 | { 309 | _kdl.setPose(_utils.toEigen(msg->task_pose_reference)); 310 | _kdl.FK(); 311 | _qref = _kdl.getJointsPositions(); 312 | } 313 | else 314 | { 315 | _qref = _utils.toEigen(msg->joints_position_reference); 316 | } 317 | 318 | _q = _utils.toEigen(msg->joints_position); 319 | _w_ee_td = _utils.toEigen(msg->task_wrench); 320 | _delta_x_ee_td = _utils.toEigen(msg->task_precision); 321 | 322 | _subscribed = true; 323 | } 324 | 325 | Eigen::MatrixXd ImpedanceModulationManager::getJacobian() 326 | { 327 | // getting jacobian matrix 328 | _kdl.setJointsPositions(_q); 329 | return _kdl.getJacobian(); 330 | } 331 | 332 | Eigen::VectorXd ImpedanceModulationManager::getGravityCompensation() 333 | { 334 | // gravity wrench 335 | _kdl.setJointsPositions(_q); 336 | return _kdl.getGravity(); 337 | } 338 | 339 | Eigen::VectorXd ImpedanceModulationManager::getGravityWrench() 340 | { 341 | // gravity wrench 342 | _J = getJacobian(); 343 | return _algebra.pseudoInverse(_J.transpose()) * getGravityCompensation(); 344 | } 345 | 346 | void ImpedanceModulationManager::log() 347 | { 348 | // log data 349 | _loggerL.logData("EEwrench",_w_ee_d); 350 | _loggerL.logData("EETaskwrench",_w_ee_td); 351 | _loggerL.logData("TaskErrorDesired",_delta_x_ee_td); 352 | _loggerL.logData("CartesianStiffness",_k_c_d); 353 | _loggerL.logData("JointStiffness",_k); 354 | _loggerL.logData("JointDamping",_d); 355 | _loggerL.logData("FeedforwardTorque",_tau_ff); 356 | 357 | _loggerL.logData("JointsPosition",_q); 358 | _loggerL.logData("JointsPositionReference",_qref); 359 | } 360 | 361 | ImpedanceModulationManager::~ImpedanceModulationManager() 362 | { 363 | 364 | } -------------------------------------------------------------------------------- /urdf/inail2arm.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | -------------------------------------------------------------------------------- /utils/KDLhelper.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2023 IIT-HHCM 3 | * Author: Liana Bertoni 4 | * email: liana.bertoni@iit.it 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #include 19 | 20 | KDLHelper::KDLHelper() 21 | { 22 | 23 | } 24 | 25 | void KDLHelper::init(std::string fileURDFpath, std::string base_frame, std::string tip_frame) 26 | { 27 | // parse URDF 28 | if (!kdl_parser::treeFromFile(fileURDFpath,_tree)) 29 | { 30 | std::cout << "Failed to parse URDF -> NO kdl tree" << std::endl; 31 | return; 32 | } 33 | 34 | _nc = 6; 35 | _tree.getChain(base_frame,tip_frame,_chain); 36 | _nj = _chain.getNrOfJoints(); 37 | _q = KDL::JntArray(_nj); 38 | _q.resize(_nj); 39 | 40 | _fksolver = std::make_unique(_chain); 41 | _iksolver1v = std::make_unique(_chain); 42 | _iksolver = std::make_unique(_chain,*_fksolver.get(),*_iksolver1v.get()); 43 | 44 | _jnt_to_jac_solver = std::make_unique(_chain); 45 | _jacobian.resize(_nj); 46 | 47 | _dyn = std::make_unique(_chain,KDL::Vector(0.0,0.0,-9.81)); 48 | } 49 | 50 | void KDLHelper::setJointsPositions(Eigen::VectorXd q) 51 | { 52 | for (int i = 0; i < _nj; ++i) 53 | _q(i) = q(i); 54 | } 55 | 56 | void KDLHelper::setPose(Eigen::VectorXd pose) 57 | { 58 | KDL::Rotation R = KDL::Rotation::RPY(pose(3),pose(4),pose(5)); 59 | KDL::Vector t(pose(0),pose(1),pose(2)); 60 | 61 | _pose = KDL::Frame(R,t); 62 | } 63 | 64 | void KDLHelper::FK() 65 | { 66 | bool kinematics_status; 67 | kinematics_status = _fksolver->JntToCart(_q,_pose); 68 | } 69 | 70 | void KDLHelper::IK() 71 | { 72 | int ret = _iksolver->CartToJnt(_q,_pose,_q); 73 | } 74 | 75 | Eigen::VectorXd KDLHelper::getPose() 76 | { 77 | Eigen::VectorXd pose; 78 | pose = Eigen::VectorXd::Zero(_nc); 79 | 80 | pose(0) = _pose.p.x(); 81 | pose(1) = _pose.p.y(); 82 | pose(2) = _pose.p.z(); 83 | _pose.M.GetRPY(pose(3),pose(4),pose(5)); 84 | 85 | return pose; 86 | } 87 | 88 | Eigen::VectorXd KDLHelper::getJointsPositions() 89 | { 90 | Eigen::VectorXd q; 91 | q = Eigen::VectorXd::Zero(_nj); 92 | for (int i = 0; i < _nj; ++i) 93 | q(i) = _q(i); 94 | 95 | return q; 96 | } 97 | 98 | Eigen::MatrixXd KDLHelper::getJacobian() 99 | { 100 | Eigen::MatrixXd J; 101 | _jnt_to_jac_solver->JntToJac(_q,_jacobian,-1); 102 | J = Eigen::MatrixXd::Identity(_jacobian.data.rows(),_jacobian.data.cols()); 103 | 104 | for (int i = 0; i < J.rows(); ++i) 105 | for (int j = 0; j < J.cols(); ++j) 106 | J(i,j) = _jacobian.data(i,j); 107 | 108 | return J; 109 | } 110 | 111 | Eigen::VectorXd KDLHelper::getGravity() 112 | { 113 | Eigen::VectorXd tau_g; 114 | tau_g = Eigen::VectorXd::Zero(_nj); 115 | 116 | KDL::JntArray jgrav(_nj); 117 | _dyn->JntToGravity(_q,jgrav); 118 | 119 | for (int i = 0; i < _nj; ++i) 120 | tau_g(i) = jgrav(i); 121 | 122 | return tau_g; 123 | } 124 | 125 | KDLHelper::~KDLHelper() 126 | { 127 | 128 | } 129 | -------------------------------------------------------------------------------- /utils/algebra.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2023 IIT-HHCM 3 | * Author: Liana Bertoni 4 | * email: liana.bertoni@iit.it 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #include 19 | 20 | Algebra::Algebra() 21 | { 22 | 23 | } 24 | 25 | Eigen::MatrixXd Algebra::pseudoInverse(Eigen::MatrixXd M_) 26 | { 27 | // psuedo-inverse of a not-square matrix (SVD based) 28 | Eigen::BDCSVD svd(M_, Eigen::ComputeThinU | Eigen::ComputeThinV); 29 | double epsilon = std::numeric_limits::epsilon(); 30 | svd.setThreshold(epsilon*std::max(M_.cols(), M_.rows())); 31 | Eigen::Index rank = svd.rank(); 32 | Eigen::MatrixXd tmp = svd.matrixU().leftCols(rank).adjoint(); 33 | tmp = svd.singularValues().head(rank).asDiagonal().inverse() * tmp; 34 | return svd.matrixV().leftCols(rank) * tmp; 35 | } 36 | 37 | void Algebra::getSkewMatrix(Eigen::VectorXd p, Eigen::MatrixXd& Skew) 38 | { 39 | Skew = Eigen::MatrixXd::Zero(3,3); 40 | Skew(0,1) = -p(2); 41 | Skew(0,2) = p(1); 42 | Skew(1,0) = p(2); 43 | Skew(1,2) = -p(0); 44 | Skew(2,0) = -p(1); 45 | Skew(2,1) = p(0); 46 | } 47 | 48 | Eigen::VectorXd Algebra::quatToEulerDeg(Eigen::VectorXd quat) // quat : x y z w 49 | { 50 | return radTodeg(quatToEulerRad(quat)); 51 | } 52 | 53 | Eigen::VectorXd Algebra::quatToEulerRad(Eigen::VectorXd quat) 54 | { 55 | Eigen::Vector3d eulerAnglesDeg; 56 | Eigen::Quaterniond q; 57 | q.x() = quat(0); 58 | q.y() = quat(1); 59 | q.z() = quat(2); 60 | q.w() = quat(3); 61 | q.normalize(); 62 | 63 | Eigen::Matrix3d rot; 64 | rot = q.toRotationMatrix(); 65 | eulerAnglesDeg = rot.eulerAngles(0,1,2); // roll-x pitch-y yaw-z 66 | 67 | Eigen::VectorXd vecReturn(3); 68 | vecReturn(0) = eulerAnglesDeg(0); 69 | vecReturn(1) = eulerAnglesDeg(1); 70 | vecReturn(2) = eulerAnglesDeg(2); 71 | 72 | return vecReturn; 73 | } 74 | 75 | Eigen::VectorXd Algebra::quatToEuler(Eigen::VectorXd quat, bool areDegrees) 76 | { 77 | if(areDegrees) 78 | return quatToEulerDeg(quat); 79 | else 80 | return quatToEulerRad(quat); 81 | } 82 | 83 | Eigen::VectorXd Algebra::eulersToQuatsRad(Eigen::VectorXd eulerAngleOrients) 84 | { 85 | int dim = (eulerAngleOrients.size()/3)*4; 86 | Eigen::VectorXd quatOrients(dim); 87 | int j = 0; 88 | for (int i = 0; i < dim/4; i+=4) 89 | { 90 | quatOrients.segment<4>(i) = EulerToQuatVec(eulerAngleOrients.segment<3>(j)); 91 | j += 3; 92 | } 93 | 94 | return quatOrients; 95 | } 96 | 97 | Eigen::VectorXd Algebra::eulersToQuatsDeg(Eigen::VectorXd eulerAngleOrients) 98 | { 99 | return radTodeg(eulersToQuatsRad(eulerAngleOrients)); 100 | } 101 | 102 | Eigen::VectorXd Algebra::eulersToQuats(Eigen::VectorXd eulerAngleOrients, bool areDegrees) 103 | { 104 | if(areDegrees) 105 | return eulersToQuatsDeg(eulerAngleOrients); 106 | else 107 | return eulersToQuatsRad(eulerAngleOrients); 108 | } 109 | 110 | Eigen::VectorXd Algebra::EulerToQuatVec(Eigen::VectorXd eulerAngles) 111 | { 112 | Eigen::VectorXd quatVec(4); 113 | Eigen::Quaternionf q; 114 | q = EulerToQuat(eulerAngles); 115 | 116 | quatVec(0) = q.x(); 117 | quatVec(1) = q.y(); 118 | quatVec(2) = q.z(); 119 | quatVec(3) = q.w(); 120 | 121 | return quatVec; 122 | } 123 | 124 | Eigen::Quaternionf Algebra::EulerToQuat(Eigen::VectorXd eulerAngles) 125 | { 126 | // psuedo-inverse of a not-square matrix (SVD based) 127 | degTorad(eulerAngles,eulerAngles); 128 | Eigen::Quaternionf q; 129 | q = Eigen::AngleAxisf(eulerAngles[2], Eigen::Vector3f::UnitZ()) 130 | * Eigen::AngleAxisf(eulerAngles[1], Eigen::Vector3f::UnitY()) 131 | * Eigen::AngleAxisf(eulerAngles[0], Eigen::Vector3f::UnitX()); 132 | return q; 133 | } 134 | 135 | Eigen::VectorXd Algebra::RotToQuat(Eigen::Matrix3d rot) 136 | { 137 | Eigen::VectorXd q_vec(4); // x y z w 138 | Eigen::Quaterniond q(rot); 139 | 140 | q_vec(0) = q.x(); 141 | q_vec(1) = q.y(); 142 | q_vec(2) = q.z(); 143 | q_vec(3) = q.w(); 144 | 145 | return q_vec; 146 | } 147 | 148 | void Algebra::getTransformationRotTrasl(Eigen::Affine3d T, Eigen::Matrix3d& rot, Eigen::VectorXd& trasl) 149 | { 150 | trasl = T.translation(); 151 | rot = T.rotation(); 152 | } 153 | 154 | void Algebra::minToT(Eigen::VectorXd position, Eigen::VectorXd orientation, Eigen::Affine3d& T, bool areDegree) 155 | { 156 | // set translation 157 | T = Eigen::Affine3d::Identity(); 158 | T.translation()[0] = position[0]; 159 | T.translation()[1] = position[1]; 160 | T.translation()[2] = position[2]; 161 | // set orientation 162 | // set quat 163 | Eigen::Quaterniond q; 164 | if(orientation.size() == 3) 165 | { 166 | if(areDegree) 167 | degTorad(orientation,orientation); 168 | 169 | q = Eigen::AngleAxisd(orientation[2], Eigen::Vector3d::UnitZ()) 170 | * Eigen::AngleAxisd(orientation[1], Eigen::Vector3d::UnitY()) 171 | * Eigen::AngleAxisd(orientation[0], Eigen::Vector3d::UnitX()); 172 | } 173 | else 174 | { 175 | q.x() = orientation[0]; 176 | q.y() = orientation[1]; 177 | q.z() = orientation[2]; 178 | q.w() = orientation[3]; 179 | } 180 | 181 | T.rotate(q.toRotationMatrix()); 182 | } 183 | 184 | void Algebra::TToMin(Eigen::Affine3d T, Eigen::VectorXd& t) 185 | { 186 | t = Eigen::VectorXd::Zero(6); 187 | t.head(3) = T.translation(); 188 | t.tail<3>() = T.rotation().eulerAngles(0,1,2); 189 | } 190 | 191 | void Algebra::TToMinDeg(Eigen::Affine3d T, Eigen::VectorXd& t) 192 | { 193 | t = Eigen::VectorXd::Zero(6); 194 | t.head(3) = T.translation(); 195 | t.tail<3>() = T.rotation().eulerAngles(0,1,2); 196 | t(3) = t(3) * (180.0/M_PI); 197 | t(4) = t(4) * (180.0/M_PI); 198 | t(5) = t(5) * (180.0/M_PI); 199 | } 200 | 201 | void Algebra::TToMinQuat(Eigen::Affine3d T, Eigen::VectorXd& t) 202 | { 203 | t = Eigen::VectorXd::Zero(7); 204 | t.head(3) = T.translation(); 205 | t.tail<4>() = RotToQuat(T.rotation()); 206 | } 207 | 208 | Eigen::VectorXd Algebra::TToMin(Eigen::Affine3d T) 209 | { 210 | Eigen::VectorXd t; 211 | TToMin(T,t); 212 | return t; 213 | } 214 | 215 | Eigen::VectorXd Algebra::TToMinDeg(Eigen::Affine3d T) 216 | { 217 | Eigen::VectorXd t; 218 | TToMinDeg(T,t); 219 | return t; 220 | } 221 | 222 | Eigen::VectorXd Algebra::TToMinQuat(Eigen::Affine3d T) 223 | { 224 | Eigen::VectorXd t; 225 | TToMinQuat(T,t); 226 | return t; 227 | } 228 | 229 | void Algebra::degTorad(Eigen::VectorXd deg, Eigen::VectorXd& rad) 230 | { 231 | rad = Eigen::VectorXd::Zero(deg.size()); 232 | for (int i = 0; i < deg.size(); ++i) 233 | rad[i] = deg[i]*(M_PI/180.0); 234 | } 235 | 236 | Eigen::VectorXd Algebra::radTodeg(Eigen::VectorXd vecRad) 237 | { 238 | Eigen::VectorXd vecDeg(vecRad.size()); 239 | for (int i = 0; i < vecRad.size(); ++i) 240 | vecDeg[i] = vecRad[i]*(180.0/M_PI); 241 | 242 | return vecDeg; 243 | } 244 | 245 | Algebra::~Algebra() 246 | { 247 | 248 | } 249 | -------------------------------------------------------------------------------- /utils/logger.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2023 IIT-HHCM 3 | * Author: Liana Bertoni 4 | * email: liana.bertoni@iit.it 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #include 19 | 20 | LoggerL::LoggerL() 21 | { 22 | 23 | } 24 | 25 | void LoggerL::initLogger(std::string logFilesPath) 26 | { 27 | // Create matlogger 28 | _logger = XBot::MatLogger2::MakeLogger(logFilesPath); // date-time automatically appended 29 | } 30 | 31 | void LoggerL::setLogger(bool setMode) 32 | { 33 | if(setMode) // fixed size 10000 34 | _logger->set_buffer_mode(XBot::VariableBuffer::Mode::circular_buffer); 35 | else 36 | { 37 | _appender = XBot::MatAppender::MakeInstance(); 38 | _appender->add_logger(_logger); 39 | _appender->start_flush_thread(); 40 | } 41 | } 42 | 43 | bool LoggerL::logData(std::string dataName, Eigen::VectorXd data) 44 | { 45 | // log data 46 | _logger->add(dataName,data); 47 | return true; 48 | } 49 | 50 | bool LoggerL::logData(std::string dataName, double data) 51 | { 52 | // log data 53 | _logger->add(dataName,data); 54 | return true; 55 | } 56 | 57 | LoggerL::~LoggerL() 58 | { 59 | 60 | } -------------------------------------------------------------------------------- /utils/utilities.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2023 IIT-HHCM 3 | * Author: Liana Bertoni 4 | * email: liana.bertoni@iit.it 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #include 19 | 20 | Utilities::Utilities() 21 | { 22 | 23 | } 24 | 25 | Eigen::VectorXd Utilities::toEigen(std::vector vec) 26 | { 27 | // psuedo-inverse of a not-square matrix (SVD based) 28 | Eigen::VectorXd vecEigen(vec.size()); 29 | for (int i = 0; i < vec.size(); ++i) 30 | vecEigen[i] = vec[i]; 31 | return vecEigen; 32 | } 33 | 34 | std::vector Utilities::toStdVector(Eigen::VectorXd vecEigen) 35 | { 36 | std::vector vecStd; 37 | for (int i = 0; i < vecEigen.size(); ++i) 38 | vecStd.push_back(vecEigen[i]); 39 | 40 | return vecStd; 41 | } 42 | 43 | std::vector Utilities::toQuaternionPose(std::vector poses, bool areDegrees) 44 | { 45 | // to eigen 46 | Eigen::VectorXd vecEigenPoses; 47 | vecEigenPoses = toEigen(poses); 48 | 49 | // getting orientation parts 50 | int nposes = (poses.size()/6); 51 | int dim = nposes*3; 52 | Eigen::VectorXd vecEigenAnglesPoses(dim); 53 | int j = 3; 54 | for (int i = 0; i < nposes; i+=3) 55 | { 56 | vecEigenAnglesPoses.segment<3>(i) = vecEigenPoses.segment<3>(j); 57 | j+=6; 58 | } 59 | 60 | // getting the orientations as a quaternions 61 | Eigen::VectorXd vecEigenQuatOrients; 62 | vecEigenQuatOrients = _algebra.eulersToQuats(vecEigenAnglesPoses,areDegrees); 63 | 64 | // recombining poses 65 | dim = nposes*7; 66 | Eigen::VectorXd vecEigenQuatPoses(dim); 67 | j = 0; 68 | int k = 0; 69 | for (int i = 0; i < nposes; i+=7) 70 | { 71 | vecEigenQuatPoses.segment<3>(i) = vecEigenPoses.segment<3>(j); 72 | vecEigenQuatPoses.segment<4>(i+3) = vecEigenQuatOrients.segment<4>(k); 73 | j+=6; 74 | k+=4; 75 | } 76 | 77 | return toStdVector(vecEigenQuatPoses); 78 | } 79 | 80 | Utilities::~Utilities() 81 | { 82 | 83 | } 84 | --------------------------------------------------------------------------------