├── config ├── joint_space_compliant_controller.yaml ├── task_space_compliant_controller.yaml ├── joint_task_space_compliant_controller.yaml ├── default_gains │ ├── task_space_params.yaml │ ├── joint_space_params.yaml │ └── joint_task_space_params.yaml ├── JointSpaceParams.cfg ├── TaskSpaceParams.cfg └── JointTaskSpaceParams.cfg ├── scripts ├── test_controller_server.py └── switch_mode.py ├── test ├── launch │ └── view_robot.launch ├── test_task_space_control.cpp ├── urdf │ ├── simple_robot.urdf.xacro │ └── oxf20_right_arm.urdf └── rviz │ └── config.rviz ├── launch ├── switch_mode.launch ├── task_space_compliant_controller.launch ├── joint_space_compliant_controller.launch └── joint_task_space_compliant_controller.launch ├── package.xml ├── include └── compliant_controllers │ ├── utils.h │ ├── robot_state.h │ ├── extended_joint_positions.h │ ├── joint_space │ ├── hardware_interface_adapter.h │ ├── hardware_interface_adapter_impl.h │ └── controller.h │ ├── task_space │ ├── hardware_interface_adapter.h │ ├── hardware_interface_adapter_impl.h │ └── controller.h │ └── joint_task_space │ ├── hardware_interface_adapter.h │ ├── hardware_interface_adapter_impl.h │ └── controller.h ├── controller_plugins.xml ├── src ├── utils.cpp ├── task_space │ ├── compliant_controllers.cpp │ └── controller.cpp ├── joint_space │ ├── compliant_controllers.cpp │ └── controller.cpp ├── joint_task_space │ ├── compliant_controllers.cpp │ └── controller.cpp └── extended_joint_positions.cpp ├── LICENSE.md ├── CMakeLists.txt └── ReadMe.md /config/joint_space_compliant_controller.yaml: -------------------------------------------------------------------------------- 1 | joint_state_controller: 2 | type: joint_state_controller/JointStateController 3 | publish_rate: 1000 4 | 5 | joint_space_compliant_controller: 6 | type: compliant_controllers/JointSpaceJointTrajectoryController 7 | robot_description_parameter: $(arg robot_description_parameter) 8 | end_effector_link: $(arg end_effector_link) 9 | apply_gravity_compensation: $(arg apply_gravity_compensation) 10 | joints: [joint_1, joint_2, joint_3, 11 | joint_4, joint_5, joint_6, joint_7] 12 | 13 | -------------------------------------------------------------------------------- /config/task_space_compliant_controller.yaml: -------------------------------------------------------------------------------- 1 | joint_state_controller: 2 | type: joint_state_controller/JointStateController 3 | publish_rate: 1000 4 | 5 | task_space_compliant_controller: 6 | type: compliant_controllers/TaskSpaceJointTrajectoryController 7 | robot_description_parameter: $(arg robot_description_parameter) 8 | end_effector_link: $(arg end_effector_link) 9 | apply_gravity_compensation: $(arg apply_gravity_compensation) 10 | joints: [joint_1, joint_2, joint_3, 11 | joint_4, joint_5, joint_6, joint_7] 12 | 13 | -------------------------------------------------------------------------------- /config/joint_task_space_compliant_controller.yaml: -------------------------------------------------------------------------------- 1 | joint_state_controller: 2 | type: joint_state_controller/JointStateController 3 | publish_rate: 1000 4 | 5 | joint_task_space_compliant_controller: 6 | type: compliant_controllers/JointTaskSpaceJointTrajectoryController 7 | robot_description_parameter: $(arg robot_description_parameter) 8 | end_effector_link: $(arg end_effector_link) 9 | apply_gravity_compensation: $(arg apply_gravity_compensation) 10 | joints: [joint_1, joint_2, joint_3, 11 | joint_4, joint_5, joint_6, joint_7] 12 | 13 | -------------------------------------------------------------------------------- /config/default_gains/task_space_params.yaml: -------------------------------------------------------------------------------- 1 | TaskKp: 2 | task_k_0: 1000 3 | task_k_1: 1000 4 | task_k_2: 650 5 | task_k_3: 45 6 | task_k_4: 25 7 | task_k_5: 25 8 | 9 | TaskKd: 10 | task_d_0: 4.6 11 | task_d_1: 4.6 12 | task_d_2: 4.6 13 | task_d_3: 2.0 14 | task_d_4: 2.0 15 | task_d_5: 2.0 16 | 17 | JointKp: 18 | joint_k_0: 0.0 19 | joint_k_1: 0.0 20 | joint_k_2: 0.0 21 | joint_k_3: 0.0 22 | joint_k_4: 0.0 23 | joint_k_5: 0.0 24 | joint_k_6: 0.0 25 | 26 | JointKd: 27 | joint_d_0: 1.0 28 | joint_d_1: 1.0 29 | joint_d_2: 1.0 30 | joint_d_3: 1.0 31 | joint_d_4: 1.0 32 | joint_d_5: 1.0 33 | joint_d_6: 1.0 34 | -------------------------------------------------------------------------------- /config/default_gains/joint_space_params.yaml: -------------------------------------------------------------------------------- 1 | TaskKp: 2 | task_k_0: 0.0 3 | task_k_1: 0.0 4 | task_k_2: 0.0 5 | task_k_3: 0.0 6 | task_k_4: 0.0 7 | task_k_5: 0.0 8 | 9 | TaskKd: 10 | task_d_0: 0.0 11 | task_d_1: 0.0 12 | task_d_2: 0.0 13 | task_d_3: 0.0 14 | task_d_4: 0.0 15 | task_d_5: 0.0 16 | 17 | JointKp: 18 | joint_k_0: 120.0 19 | joint_k_1: 80.0 20 | joint_k_2: 80.0 21 | joint_k_3: 120.0 22 | joint_k_4: 25.0 23 | joint_k_5: 20.0 24 | joint_k_6: 15.0 25 | 26 | JointKd: 27 | joint_d_0: 8.0 28 | joint_d_1: 6.0 29 | joint_d_2: 6.0 30 | joint_d_3: 8.0 31 | joint_d_4: 3.5 32 | joint_d_5: 3.5 33 | joint_d_6: 3.0 34 | -------------------------------------------------------------------------------- /scripts/test_controller_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from dynamic_reconfigure.server import Server 5 | from compliant_controllers.cfg import JointTaskSpaceCompliantControllerConfig 6 | 7 | def callback(config, level): 8 | rospy.loginfo("Reconfigure request received:") 9 | for key, val in config.items(): 10 | rospy.loginfo(f" {key}: {val}") 11 | return config 12 | 13 | if __name__ == "__main__": 14 | rospy.init_node("compliant_controller_reconfigure_node") 15 | srv = Server(JointTaskSpaceCompliantControllerConfig, callback) 16 | rospy.loginfo("Dynamic reconfigure test server is up.") 17 | rospy.spin() 18 | -------------------------------------------------------------------------------- /test/launch/view_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /config/default_gains/joint_task_space_params.yaml: -------------------------------------------------------------------------------- 1 | TaskKp: 2 | task_k_0: 1000.0 3 | task_k_1: 1000.0 4 | task_k_2: 650.0 5 | task_k_3: 45.0 6 | task_k_4: 25.0 7 | task_k_5: 25.0 8 | 9 | TaskKd: 10 | task_d_0: 4.6 11 | task_d_1: 4.6 12 | task_d_2: 4.6 13 | task_d_3: 2.0 14 | task_d_4: 2.0 15 | task_d_5: 2.0 16 | 17 | JointKp: 18 | joint_k_0: 6.5 19 | joint_k_1: 5.6 20 | joint_k_2: 5.0 21 | joint_k_3: 3.9 22 | joint_k_4: 2.9 23 | joint_k_5: 1.8 24 | joint_k_6: 1.7 25 | 26 | JointKd: 27 | joint_d_0: 1.8 28 | joint_d_1: 1.8 29 | joint_d_2: 1.8 30 | joint_d_3: 1.8 31 | joint_d_4: 1.8 32 | joint_d_5: 1.8 33 | joint_d_6: 1.8 34 | -------------------------------------------------------------------------------- /launch/switch_mode.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | compliant_controllers 4 | 0.0.0 5 | A compliant joint trajectory controller with gravity compensation. 6 | Tobit Flatscher 7 | Rishabh Madan 8 | Rajat Kumar Jenamani 9 | Tobit Flatscher 10 | BSD-3-Clause 11 | 12 | catkin 13 | controller_interface 14 | driver_base 15 | dynamic_reconfigure 16 | eigen 17 | hardware_interface 18 | joint_trajectory_controller 19 | pinocchio 20 | pluginlib 21 | roscpp 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /launch/task_space_compliant_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 16 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /launch/joint_space_compliant_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 16 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /launch/joint_task_space_compliant_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 16 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /include/compliant_controllers/utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file utils.h 3 | * \mainpage 4 | * Different utility functions for the compliant controllers 5 | * 6 | * \authors 7 | * Tobit Flatscher 8 | * \copyright 9 | * Oxford Robotics Institute - University of Oxford (2024) 10 | * \license 11 | * This project is released under the 3-clause BSD license. 12 | */ 13 | 14 | #ifndef COMPLIANT_CONTROLLERS__UTILS 15 | #define COMPLIANT_CONTROLLERS__UTILS 16 | 17 | #include 18 | #include 19 | 20 | // Pinocchio has to be included before ROS 21 | // See https://github.com/wxmerkt/pinocchio_ros_example 22 | #include 23 | 24 | #include 25 | 26 | 27 | namespace compliant_controllers { 28 | 29 | /**\fn joint_ros_to_pinocchio 30 | * \brief 31 | * Convert the joint states to the Pinocchio convention 32 | * 33 | * \param[in] q 34 | * The input joint angles 35 | * \param[in] model 36 | * The Pinocchio robot model 37 | * \return 38 | * The joint angles converted to the Pinocchio convention 39 | */ 40 | [[nodiscard]] 41 | Eigen::VectorXd joint_ros_to_pinocchio(Eigen::VectorXd const& q, pinocchio::Model const& model); 42 | 43 | } 44 | 45 | #endif // COMPLIANT_CONTROLLERS__UTILS 46 | -------------------------------------------------------------------------------- /controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Joint-space compliant controller with gravity compensation 7 | 8 | 9 | 10 | 11 | 14 | 15 | Joint and task-space compliant controller with gravity compensation 16 | 17 | 18 | 19 | 20 | 23 | 24 | Task-space compliant controller with gravity compensation 25 | 26 | 27 | -------------------------------------------------------------------------------- /src/utils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file utils.cpp 3 | * \mainpage 4 | * Different utility functions for the compliant controllers 5 | * 6 | * \authors 7 | * Tobit Flatscher 8 | * \copyright 9 | * Oxford Robotics Institute - University of Oxford (2024) 10 | * \license 11 | * This project is released under the 3-clause BSD license. 12 | */ 13 | 14 | #include "compliant_controllers/utils.h" 15 | 16 | #include 17 | 18 | // Pinocchio has to be included before ROS 19 | // See https://github.com/wxmerkt/pinocchio_ros_example 20 | #include 21 | 22 | #include 23 | 24 | 25 | namespace compliant_controllers { 26 | 27 | Eigen::VectorXd joint_ros_to_pinocchio(Eigen::VectorXd const& q, pinocchio::Model const& model) { 28 | Eigen::VectorXd q_pin {model.nq}; 29 | for (std::size_t i = 0; i < model.joints.size() - 1; i++) { 30 | pinocchio::JointIndex const jidx {model.getJointId(model.names[i + 1])}; 31 | int const qidx {model.idx_qs[jidx]}; 32 | // 2 corresponds to continuous joints in Pinocchio 33 | if (model.nqs[jidx] == 2) { 34 | q_pin(qidx) = std::cos(q(i)); 35 | q_pin(qidx + 1) = std::sin(q(i)); 36 | } else { 37 | q_pin(qidx) = q(i); 38 | } 39 | } 40 | return q_pin; 41 | } 42 | 43 | } // compliant_controllers 44 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2025, Applied AI Lab, EmPRISE Lab 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /include/compliant_controllers/robot_state.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file robot_state.h 3 | * \mainpage 4 | * Struct that contains the robot state at a particular time stamp 5 | * 6 | * \authors 7 | * Tobit Flatscher 8 | * \copyright 9 | * Oxford Robotics Institute - University of Oxford (2024) 10 | * \license 11 | * This project is released under the 3-clause BSD license. 12 | */ 13 | 14 | #ifndef COMPLIANT_CONTROLLERS__ROBOT_STATE 15 | #define COMPLIANT_CONTROLLERS__ROBOT_STATE 16 | 17 | #include 18 | 19 | 20 | namespace compliant_controllers { 21 | 22 | /**\class RobotState 23 | * \brief 24 | * Holds the robot state at a particular time stamp 25 | */ 26 | struct RobotState { 27 | public: 28 | /**\fn RobotState 29 | * \brief 30 | * The state of the robot at a particular time stamp 31 | * 32 | * \param[in] num_of_dof 33 | * The degrees of freedom of all joints that the arrays should be resized to 34 | */ 35 | RobotState(int const num_of_dof) { 36 | resize(num_of_dof); 37 | return; 38 | } 39 | RobotState() = default; 40 | RobotState(RobotState const&) = default; 41 | RobotState& operator= (RobotState const&) = default; 42 | RobotState(RobotState&&) = default; 43 | RobotState& operator= (RobotState&&) = default; 44 | 45 | /**\fn resize 46 | * \brief 47 | * Resize the underlying arrays to a desired size 48 | * 49 | * \param[in] num_of_dof 50 | * The size that the underlying arrays should be resized to 51 | */ 52 | void resize(int const num_of_dof) { 53 | positions.resize(num_of_dof); 54 | velocities.resize(num_of_dof); 55 | accelerations.resize(num_of_dof); 56 | efforts.resize(num_of_dof); 57 | return; 58 | } 59 | 60 | Eigen::VectorXd positions; 61 | Eigen::VectorXd velocities; 62 | Eigen::VectorXd accelerations; 63 | Eigen::VectorXd efforts; 64 | }; 65 | 66 | } 67 | 68 | #endif // COMPLIANT_CONTROLLERS__ROBOT_STATE 69 | -------------------------------------------------------------------------------- /test/test_task_space_control.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "compliant_controllers/joint_task_space/controller.h" 6 | 7 | 8 | 9 | int main(int argc, char **argv) 10 | { 11 | 12 | std::string robot_description = PATH_TO_DIR + std::string("/test/urdf/oxf20_right_arm_gripper.urdf"); 13 | 14 | auto robot_model = std::make_unique(); 15 | pinocchio::urdf::buildModel(robot_description, *robot_model.get()); 16 | 17 | int num_of_dof_ = 7; 18 | std::string end_effector_link = "right_kinova_arm_tool_frame"; 19 | 20 | bool apply_gravity = true; 21 | 22 | Eigen::VectorXd torques = Eigen::VectorXd::Zero(num_of_dof_); 23 | 24 | auto compliant_controller_ = std::make_unique(std::move(robot_model), end_effector_link, num_of_dof_, apply_gravity); 25 | 26 | 27 | // Create the current and desired robot states 28 | compliant_controllers::RobotState current_state(num_of_dof_); 29 | compliant_controllers::RobotState desired_state(num_of_dof_); 30 | 31 | current_state.positions.setZero(); 32 | current_state.velocities.setZero(); 33 | current_state.accelerations.setZero(); 34 | current_state.efforts.setZero(); 35 | 36 | desired_state.positions.setZero(); 37 | desired_state.velocities.setZero(); 38 | desired_state.accelerations.setZero(); 39 | desired_state.efforts.setZero(); 40 | 41 | current_state.positions = 0.01 * Eigen::VectorXd::Ones(num_of_dof_); 42 | 43 | ros::Duration period(0.01); 44 | // Compute torque 45 | int no_runs = 1000; 46 | 47 | auto tic = std::chrono::high_resolution_clock::now(); 48 | 49 | for (int k=0; kcomputeEffort(desired_state, current_state, period); 52 | } 53 | 54 | auto toc = std::chrono::high_resolution_clock::now(); 55 | double us_taken = 1e-3 * std::chrono::duration_cast(toc - tic).count() / no_runs; 56 | 57 | std::cout << "Average computeEffort run time for " << no_runs << " runs: " << 58 | us_taken << " micro seconds." << std::endl; 59 | 60 | std::cout << torques.transpose() << std::endl; 61 | 62 | return 0; 63 | } 64 | -------------------------------------------------------------------------------- /src/task_space/compliant_controllers.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file compliant_controllers.cpp 3 | * \mainpage 4 | * Instantiation of the compliant joint-trajectory controllers 5 | * 6 | * \authors 7 | * Tobit Flatscher 8 | * Rishabh Madan 9 | * Rajat Kumar Jenamani 10 | * \copyright 11 | * Oxford Robotics Institute - University of Oxford (2024) 12 | * EmPRISE Lab - Cornell University (2023) 13 | * \license 14 | * This project is released under the 3-clause BSD license. 15 | */ 16 | 17 | // Pinocchio has to be included before ROS 18 | // See https://github.com/wxmerkt/pinocchio_ros_example 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "compliant_controllers/task_space/hardware_interface_adapter.h" 26 | 27 | 28 | // Convenient alias 29 | using TrajectoryState = joint_trajectory_controller::JointTrajectorySegment< 30 | trajectory_interface::QuinticSplineSegment 31 | >::State; 32 | /**\class HardwareInterfaceAdapter 33 | * \brief 34 | * This specialization is more specific than the previous specialization 35 | * template HardwareInterfaceAdapter 36 | */ 37 | template <> 38 | class HardwareInterfaceAdapter: public 39 | compliant_controllers::task_space::CompliantHardwareInterfaceAdapter< 40 | hardware_interface::EffortJointInterface,TrajectoryState 41 | > { 42 | public: 43 | HardwareInterfaceAdapter() = default; 44 | HardwareInterfaceAdapter(HardwareInterfaceAdapter const&) = default; 45 | HardwareInterfaceAdapter& operator= (HardwareInterfaceAdapter const&) = default; 46 | HardwareInterfaceAdapter(HardwareInterfaceAdapter&&) = default; 47 | HardwareInterfaceAdapter& operator= (HardwareInterfaceAdapter&&) = default; 48 | }; 49 | 50 | namespace compliant_controllers { 51 | namespace task_space { 52 | 53 | /**\class JointTrajectoryController 54 | * \brief 55 | * Compliant joint trajectory controller using the compliant hardware interface adapter 56 | */ 57 | class JointTrajectoryController: public 58 | joint_trajectory_controller::JointTrajectoryController< 59 | trajectory_interface::QuinticSplineSegment, 60 | hardware_interface::EffortJointInterface 61 | > { 62 | public: 63 | JointTrajectoryController() = default; 64 | JointTrajectoryController(JointTrajectoryController const&) = default; 65 | JointTrajectoryController& operator= (JointTrajectoryController const&) = default; 66 | JointTrajectoryController(JointTrajectoryController&&) = default; 67 | JointTrajectoryController& operator= (JointTrajectoryController&&) = default; 68 | }; 69 | } 70 | } 71 | 72 | #include 73 | 74 | PLUGINLIB_EXPORT_CLASS( 75 | compliant_controllers::task_space::JointTrajectoryController, controller_interface::ControllerBase 76 | ) 77 | 78 | -------------------------------------------------------------------------------- /src/joint_space/compliant_controllers.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file compliant_controllers.cpp 3 | * \mainpage 4 | * Instantiation of the compliant joint-trajectory controllers 5 | * 6 | * \authors 7 | * Tobit Flatscher 8 | * Rishabh Madan 9 | * Rajat Kumar Jenamani 10 | * \copyright 11 | * Oxford Robotics Institute - University of Oxford (2024) 12 | * EmPRISE Lab - Cornell University (2023) 13 | * \license 14 | * This project is released under the 3-clause BSD license. 15 | */ 16 | 17 | // Pinocchio has to be included before ROS 18 | // See https://github.com/wxmerkt/pinocchio_ros_example 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "compliant_controllers/joint_space/hardware_interface_adapter.h" 26 | 27 | 28 | // Convenient alias 29 | using TrajectoryState = joint_trajectory_controller::JointTrajectorySegment< 30 | trajectory_interface::QuinticSplineSegment 31 | >::State; 32 | /**\class HardwareInterfaceAdapter 33 | * \brief 34 | * This specialization is more specific than the previous specialization 35 | * template HardwareInterfaceAdapter 36 | */ 37 | template <> 38 | class HardwareInterfaceAdapter: public 39 | compliant_controllers::joint_space::CompliantHardwareInterfaceAdapter< 40 | hardware_interface::EffortJointInterface,TrajectoryState 41 | > { 42 | public: 43 | HardwareInterfaceAdapter() = default; 44 | HardwareInterfaceAdapter(HardwareInterfaceAdapter const&) = default; 45 | HardwareInterfaceAdapter& operator= (HardwareInterfaceAdapter const&) = default; 46 | HardwareInterfaceAdapter(HardwareInterfaceAdapter&&) = default; 47 | HardwareInterfaceAdapter& operator= (HardwareInterfaceAdapter&&) = default; 48 | }; 49 | 50 | namespace compliant_controllers { 51 | namespace joint_space { 52 | 53 | /**\class JointTrajectoryController 54 | * \brief 55 | * Compliant joint trajectory controller using the compliant hardware interface adapter 56 | */ 57 | class JointTrajectoryController: public 58 | joint_trajectory_controller::JointTrajectoryController< 59 | trajectory_interface::QuinticSplineSegment, 60 | hardware_interface::EffortJointInterface 61 | > { 62 | public: 63 | JointTrajectoryController() = default; 64 | JointTrajectoryController(JointTrajectoryController const&) = default; 65 | JointTrajectoryController& operator= (JointTrajectoryController const&) = default; 66 | JointTrajectoryController(JointTrajectoryController&&) = default; 67 | JointTrajectoryController& operator= (JointTrajectoryController&&) = default; 68 | }; 69 | } 70 | } 71 | 72 | #include 73 | 74 | PLUGINLIB_EXPORT_CLASS( 75 | compliant_controllers::joint_space::JointTrajectoryController, controller_interface::ControllerBase 76 | ) 77 | 78 | -------------------------------------------------------------------------------- /src/joint_task_space/compliant_controllers.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file compliant_controllers.cpp 3 | * \mainpage 4 | * Instantiation of the compliant joint-trajectory controllers 5 | * 6 | * \authors 7 | * Tobit Flatscher 8 | * Rishabh Madan 9 | * Rajat Kumar Jenamani 10 | * \copyright 11 | * Oxford Robotics Institute - University of Oxford (2024) 12 | * EmPRISE Lab - Cornell University (2023) 13 | * \license 14 | * This project is released under the 3-clause BSD license. 15 | */ 16 | 17 | // Pinocchio has to be included before ROS 18 | // See https://github.com/wxmerkt/pinocchio_ros_example 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "compliant_controllers/joint_task_space/hardware_interface_adapter.h" 26 | 27 | 28 | // Convenient alias 29 | using TrajectoryState = joint_trajectory_controller::JointTrajectorySegment< 30 | trajectory_interface::QuinticSplineSegment 31 | >::State; 32 | /**\class HardwareInterfaceAdapter 33 | * \brief 34 | * This specialization is more specific than the previous specialization 35 | * template HardwareInterfaceAdapter 36 | */ 37 | template <> 38 | class HardwareInterfaceAdapter: public 39 | compliant_controllers::joint_task_space::CompliantHardwareInterfaceAdapter< 40 | hardware_interface::EffortJointInterface,TrajectoryState 41 | > { 42 | public: 43 | HardwareInterfaceAdapter() = default; 44 | HardwareInterfaceAdapter(HardwareInterfaceAdapter const&) = default; 45 | HardwareInterfaceAdapter& operator= (HardwareInterfaceAdapter const&) = default; 46 | HardwareInterfaceAdapter(HardwareInterfaceAdapter&&) = default; 47 | HardwareInterfaceAdapter& operator= (HardwareInterfaceAdapter&&) = default; 48 | }; 49 | 50 | namespace compliant_controllers { 51 | namespace joint_task_space { 52 | 53 | /**\class JointTrajectoryController 54 | * \brief 55 | * Compliant joint trajectory controller using the compliant hardware interface adapter 56 | */ 57 | class JointTrajectoryController: public 58 | joint_trajectory_controller::JointTrajectoryController< 59 | trajectory_interface::QuinticSplineSegment, 60 | hardware_interface::EffortJointInterface 61 | > { 62 | public: 63 | JointTrajectoryController() = default; 64 | JointTrajectoryController(JointTrajectoryController const&) = default; 65 | JointTrajectoryController& operator= (JointTrajectoryController const&) = default; 66 | JointTrajectoryController(JointTrajectoryController&&) = default; 67 | JointTrajectoryController& operator= (JointTrajectoryController&&) = default; 68 | }; 69 | } 70 | } 71 | 72 | #include 73 | 74 | PLUGINLIB_EXPORT_CLASS( 75 | compliant_controllers::joint_task_space::JointTrajectoryController, controller_interface::ControllerBase 76 | ) 77 | 78 | -------------------------------------------------------------------------------- /scripts/switch_mode.py: -------------------------------------------------------------------------------- 1 | import os 2 | from enum import Enum 3 | import yaml 4 | import copy 5 | from typing import Dict 6 | 7 | import rospy 8 | from dynamic_reconfigure.client import Client 9 | 10 | 11 | class Modes(Enum): 12 | JOINT_TASK = 0 13 | JOINT = 1 14 | TASK = 2 15 | 16 | 17 | class DynamicReconfigure: 18 | def __init__(self, param_dict=None): 19 | # Create Dynamic Reconfigure Server 20 | # self._srv = Server(config, callback) 21 | # Create a Dynamic Reconfigure Client to update the parameters 22 | self._client = Client("compliant_controller_reconfigure_node") 23 | # Dictory containing the params 24 | self._default_params = param_dict 25 | self._params = copy.deepcopy(self._default_params) 26 | 27 | def callback(self, config): 28 | rospy.loginfo("Reconfigured with: %s", config) 29 | return config 30 | 31 | def update_parameters(self, params: Dict=None): 32 | if params is not None: 33 | self._params = params 34 | return self._client.update_configuration(self._params) 35 | 36 | # Setters and Getters 37 | @property 38 | def default_params(self) -> Dict: 39 | return copy.deepcopy(self._default_params) 40 | 41 | @default_params.setter 42 | def default_params(self, values: Dict): 43 | self._default_params = copy.deepcopy(values) 44 | 45 | @property 46 | def params(self) -> Dict: 47 | return self._params 48 | 49 | @params.setter 50 | def params(self, values: Dict): 51 | self._params = values 52 | 53 | 54 | def load_yaml_config(file_path): 55 | with open(file_path, 'r') as f: 56 | config = yaml.safe_load(f) 57 | return config 58 | 59 | 60 | def flatten_config(nested_dict): 61 | flat_dict = {} 62 | for group, params in nested_dict.items(): 63 | for key, value in params.items(): 64 | flat_dict[key] = value 65 | return flat_dict 66 | 67 | def load_default_params(config_path, file_dict): 68 | param_dict = {} 69 | for key, file_name in file_dict.items(): 70 | config = load_yaml_config(os.path.join(config_path, file_name + ".yaml")) 71 | param_dict[key] = flatten_config(config) 72 | return param_dict 73 | 74 | 75 | def create_reconfig_dict(gain_dict): 76 | reconfig_dict = {} 77 | for key, item in gain_dict.items(): 78 | reconfig_dict[key] = DynamicReconfigure(item) 79 | return reconfig_dict 80 | 81 | 82 | def main(): 83 | 84 | rospy.init_node("mode_switcher") 85 | 86 | config_path = rospy.get_param(rospy.get_name() + "/config_path") 87 | mode = rospy.get_param(rospy.get_name() + "/mode") 88 | 89 | file_dict = {Modes.JOINT_TASK: "joint_task_space_params", 90 | Modes.JOINT: "joint_space_params", 91 | Modes.TASK: "joint_task_space_params"} 92 | 93 | # The default gains for the three modes 94 | gain_dict = load_default_params(config_path, file_dict) 95 | 96 | # Reconfigure dict 97 | reconfig_dict = create_reconfig_dict(gain_dict) 98 | 99 | # Execute mode switch 100 | reconfig_dict[Modes(mode)].update_parameters() 101 | 102 | return 0 103 | 104 | 105 | if __name__ == "__main__": 106 | main() 107 | -------------------------------------------------------------------------------- /src/extended_joint_positions.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file extended_joint_position.cpp 3 | * \mainpage 4 | * Helpers for joint position calculations 5 | * 6 | * \authors 7 | * Tobit Flatscher 8 | * Rishabh Madan 9 | * Rajat Kumar Jenamani 10 | * \copyright 11 | * Oxford Robotics Institute - University of Oxford (2024) 12 | * EmPRISE Lab - Cornell University (2023) 13 | * \license 14 | * This project is released under the 3-clause BSD license. 15 | */ 16 | 17 | #include "compliant_controllers/extended_joint_positions.h" 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | 25 | namespace compliant_controllers { 26 | 27 | ExtendedJointPositions::ExtendedJointPositions(unsigned int const number_of_dof, 28 | double const threshold) 29 | : is_initialized_{false}, number_of_dof_{number_of_dof}, threshold_{threshold} { 30 | normalized_target_joint_positions_.resize(number_of_dof_, 1); 31 | diff_joint_positions_.resize(number_of_dof_, 1); 32 | current_joint_positions_.resize(number_of_dof_, 1); 33 | return; 34 | } 35 | 36 | bool ExtendedJointPositions::init(Eigen::VectorXd const& joint_positions) { 37 | if (is_initialized_ == false) { 38 | normalized_target_joint_positions_ = normalize(joint_positions); 39 | diff_joint_positions_ = normalized_target_joint_positions_; 40 | current_joint_positions_ = normalized_target_joint_positions_; 41 | is_initialized_ = true; 42 | return true; 43 | } 44 | return false; 45 | } 46 | 47 | void ExtendedJointPositions::update(Eigen::VectorXd const& target_joint_positions) { 48 | for (Eigen::Index i = 0; i < target_joint_positions.size(); ++i) { 49 | if (std::abs(target_joint_positions(i) - current_joint_positions_(i)) >= threshold_) { 50 | diff_joint_positions_(i) += normalize(target_joint_positions(i)) - normalize(current_joint_positions_(i)); 51 | // std::cout << "========================" << std::endl; 52 | // std::cout << " BRANCH 1 IN UPDATE JOINT POSITIONS: " << i << ", " << target_joint_positions(i) << ", " << current_joint_positions_(i) << std::endl; 53 | // std::cout << diff_joint_positions_(i) << std::endl; 54 | // std::cout << "========================" << std::endl; 55 | 56 | } else { 57 | // TODO: Verify that the following implementation is actually correct 58 | // It does not seem correct to me! 59 | int number_of_rotations {0}; 60 | if (diff_joint_positions_(i) >= 0) { 61 | number_of_rotations = static_cast(diff_joint_positions_(i)/(2.0*M_PI)); 62 | } 63 | else { 64 | number_of_rotations = static_cast(diff_joint_positions_(i)/(2.0*M_PI)) - 1; 65 | } 66 | diff_joint_positions_(i) = number_of_rotations*(2.0*M_PI) + target_joint_positions(i); 67 | } 68 | } 69 | current_joint_positions_ = target_joint_positions; 70 | return; 71 | } 72 | 73 | constexpr double ExtendedJointPositions::normalize(double const joint_angle) noexcept { 74 | // Taken from https://stackoverflow.com/questions/11980292/how-to-wrap-around-a-range 75 | double output = std::fmod(joint_angle + M_PI, 2.0*M_PI); 76 | if (output < 0.0) { 77 | output += 2.0*M_PI; 78 | } 79 | return output - M_PI; 80 | } 81 | 82 | Eigen::VectorXd ExtendedJointPositions::normalize(Eigen::VectorXd const& joint_positions) { 83 | Eigen::VectorXd output {joint_positions}; 84 | for (Eigen::Index i = 0; i < output.size(); ++i) { 85 | output(i) = normalize(joint_positions(i)); 86 | } 87 | return output; 88 | } 89 | 90 | } // compliant_controllers 91 | -------------------------------------------------------------------------------- /test/urdf/simple_robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 42 | 43 | 44 | 45 | 46 | true 47 | false 48 | ${not is_gravity} 49 | Gazebo/${colour} 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 | transmission_interface/SimpleTransmission 76 | 77 | hardware_interface/EffortJointInterface 78 | 79 | 80 | 1 81 | 82 | 83 | 84 | 85 | 86 | 87 | /simple_robot 88 | 89 | 90 | 91 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(compliant_controllers) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | 7 | set(BUILD_TESTS ON) 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | controller_interface 11 | driver_base 12 | dynamic_reconfigure 13 | hardware_interface 14 | joint_trajectory_controller 15 | pluginlib 16 | roscpp 17 | ) 18 | find_package(pinocchio REQUIRED) 19 | 20 | generate_dynamic_reconfigure_options( 21 | config/JointSpaceParams.cfg 22 | config/JointTaskSpaceParams.cfg 23 | config/TaskSpaceParams.cfg 24 | ) 25 | 26 | catkin_package( 27 | CATKIN_DEPENDS 28 | controller_interface 29 | driver_base 30 | dynamic_reconfigure 31 | hardware_interface 32 | joint_trajectory_controller 33 | pluginlib 34 | roscpp 35 | ) 36 | find_package(Eigen3 REQUIRED) 37 | 38 | catkin_install_python(PROGRAMS 39 | scripts/switch_mode.py 40 | scripts/test_controller_server.py 41 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 42 | ) 43 | 44 | include_directories( 45 | include 46 | ${catkin_INCLUDE_DIRS} 47 | ${EIGEN3_INCLUDE_DIRS} 48 | ) 49 | link_directories( 50 | ${catkin_LIBRARY_DIRS} 51 | ) 52 | 53 | # Compiled as three separate libraries to avoid linker error with joint_trajectory_controller internals 54 | add_library(joint_space_compliant_controller 55 | src/joint_space/compliant_controllers.cpp 56 | src/joint_space/controller.cpp 57 | src/extended_joint_positions.cpp 58 | src/utils.cpp 59 | ) 60 | target_compile_definitions(joint_space_compliant_controller PRIVATE ${PINOCCHIO_CFLAGS_OTHER}) 61 | target_link_libraries(joint_space_compliant_controller 62 | ${catkin_LIBRARIES} 63 | pinocchio::pinocchio 64 | ) 65 | add_dependencies(joint_space_compliant_controller ${PROJECT_NAME}_gencfg) 66 | 67 | add_library(joint_task_space_compliant_controller 68 | src/joint_task_space/compliant_controllers.cpp 69 | src/joint_task_space/controller.cpp 70 | src/extended_joint_positions.cpp 71 | src/utils.cpp 72 | ) 73 | target_compile_definitions(joint_task_space_compliant_controller PRIVATE ${PINOCCHIO_CFLAGS_OTHER}) 74 | target_link_libraries(joint_task_space_compliant_controller 75 | ${catkin_LIBRARIES} 76 | pinocchio::pinocchio 77 | ) 78 | add_dependencies(joint_task_space_compliant_controller ${PROJECT_NAME}_gencfg) 79 | 80 | add_library(task_space_compliant_controller 81 | src/task_space/compliant_controllers.cpp 82 | src/task_space/controller.cpp 83 | src/extended_joint_positions.cpp 84 | src/utils.cpp 85 | ) 86 | target_compile_definitions(task_space_compliant_controller PRIVATE ${PINOCCHIO_CFLAGS_OTHER}) 87 | target_link_libraries(task_space_compliant_controller 88 | ${catkin_LIBRARIES} 89 | pinocchio::pinocchio 90 | ) 91 | add_dependencies(task_space_compliant_controller ${PROJECT_NAME}_gencfg) 92 | 93 | install(TARGETS joint_space_compliant_controller joint_task_space_compliant_controller task_space_compliant_controller 94 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 95 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 96 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 97 | ) 98 | install(DIRECTORY include/${PROJECT_NAME}/ 99 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 100 | ) 101 | install(DIRECTORY config/ 102 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config 103 | ) 104 | install(DIRECTORY launch/ 105 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 106 | ) 107 | 108 | if(BUILD_TESTS) 109 | 110 | add_library(joint_task_space_compliant_controller_test 111 | src/joint_task_space/controller.cpp 112 | src/extended_joint_positions.cpp 113 | src/utils.cpp 114 | ) 115 | 116 | add_executable(${PROJECT_NAME}_joint_taskspace_ctrl_test 117 | test/test_task_space_control.cpp) 118 | 119 | target_link_libraries(${PROJECT_NAME}_joint_taskspace_ctrl_test 120 | joint_task_space_compliant_controller_test 121 | ${catkin_LIBRARIES} 122 | pinocchio::pinocchio) 123 | 124 | target_compile_definitions(${PROJECT_NAME}_joint_taskspace_ctrl_test PUBLIC PATH_TO_DIR="${CMAKE_CURRENT_SOURCE_DIR}") 125 | 126 | endif() 127 | 128 | -------------------------------------------------------------------------------- /include/compliant_controllers/extended_joint_positions.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file extended_joint_positions.h 3 | * \mainpage 4 | * Helpers for joint position calculations 5 | * 6 | * \authors 7 | * Tobit Flatscher 8 | * Rishabh Madan 9 | * Rajat Kumar Jenamani 10 | * \copyright 11 | * Oxford Robotics Institute - University of Oxford (2024) 12 | * EmPRISE Lab - Cornell University (2023) 13 | * \license 14 | * This project is released under the 3-clause BSD license. 15 | */ 16 | 17 | #ifndef COMPLIANT_CONTROLLERS__EXTENDED_JOINT_POSITIONS 18 | #define COMPLIANT_CONTROLLERS__EXTENDED_JOINT_POSITIONS 19 | 20 | #include 21 | 22 | 23 | namespace compliant_controllers { 24 | 25 | /**\class ExtendedJointPositions 26 | * \brief 27 | * Helper class for estimating difference joint positions 28 | */ 29 | class ExtendedJointPositions { 30 | public: 31 | /**\fn ExtendedJointPositions 32 | * \brief 33 | * Constructor that allocates required memory 34 | * 35 | * \param[in] number_of_dof 36 | * Degrees of freedom of all of the joints combined 37 | * \param[in] threshold 38 | * Threshold for the joint angles 39 | */ 40 | ExtendedJointPositions(unsigned int const number_of_dof, double const threshold = 3*M_PI/2); 41 | ExtendedJointPositions() = delete; 42 | ExtendedJointPositions(ExtendedJointPositions const&) = default; 43 | ExtendedJointPositions& operator= (ExtendedJointPositions const&) = default; 44 | ExtendedJointPositions(ExtendedJointPositions&&) = default; 45 | ExtendedJointPositions& operator= (ExtendedJointPositions&&) = default; 46 | 47 | /**\fn init 48 | * \brief 49 | * Initialize the class with new joint angles 50 | * \warning 51 | * Only works if called for the first time, else the value is discarded 52 | * 53 | * \param[in] joint_positions 54 | * Joint positions that the class should be initialized with 55 | * \return 56 | * Boolean value indicating success (true) or failure (false) 57 | */ 58 | [[nodiscard]] 59 | bool init(Eigen::VectorXd const& joint_positions); 60 | 61 | /**\fn update 62 | * \brief 63 | * Compute the new joint positions. For getting the estimated positions call the getter function. 64 | * 65 | * \param[in] target_joint_positions 66 | * The desired target 67 | * \return 68 | * Boolean value indicating success (true) or failure (false) 69 | */ 70 | void update(Eigen::VectorXd const& target_joint_positions); 71 | 72 | /**\fn getPositions 73 | * \brief 74 | * Getter for the newly computed joint positions 75 | * 76 | * \return 77 | * The newly computed joint positions 78 | */ 79 | [[nodiscard]] 80 | Eigen::VectorXd getPositions() const noexcept { 81 | return diff_joint_positions_; 82 | } 83 | 84 | /**\fn isInitialized 85 | * \brief 86 | * Check whether this structure was already initialized successfully 87 | * 88 | * \return 89 | * Boolean value indicating initialization (true) or not (false) 90 | */ 91 | [[nodiscard]] 92 | bool isInitialized() const noexcept { 93 | return is_initialized_; 94 | } 95 | 96 | protected: 97 | /**\fn normalize 98 | * \brief 99 | * Normalize a single joint angle to the domain [-pi, pi) 100 | * 101 | * \param[in] joint_angle 102 | * The single joint angle to be normalized 103 | * \return 104 | * The normalized joint angle [-pi, pi) 105 | */ 106 | [[nodiscard]] 107 | static constexpr double normalize(double const joint_angle) noexcept; 108 | 109 | /**\fn normalize 110 | * \brief 111 | * Normalize a vector of joint angles to the domain [-pi, pi) 112 | * 113 | * \param[in] joint_angles 114 | * The joint angles to be normalized 115 | * \return 116 | * The normalized joint angles [-pi, pi) 117 | */ 118 | [[nodiscard]] 119 | static Eigen::VectorXd normalize(Eigen::VectorXd const& joint_angles); 120 | 121 | bool is_initialized_; 122 | unsigned int number_of_dof_; 123 | double threshold_; 124 | 125 | Eigen::VectorXd normalized_target_joint_positions_; 126 | Eigen::VectorXd diff_joint_positions_; 127 | Eigen::VectorXd current_joint_positions_; 128 | }; 129 | 130 | } 131 | 132 | #endif // COMPLIANT_CONTROLLERS__EXTENDED_JOINT_POSITIONS 133 | -------------------------------------------------------------------------------- /include/compliant_controllers/joint_space/hardware_interface_adapter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file hardware_interface_adapter.h 3 | * \mainpage 4 | * Contains a custom hardware interface adapter for the compliant controller 5 | * This allows us to override the behavior of the default controller 6 | * 7 | * \authors 8 | * Tobit Flatscher 9 | * \copyright 10 | * Oxford Robotics Institute - University of Oxford (2024) 11 | * \license 12 | * This project is released under the 3-clause BSD license. 13 | */ 14 | 15 | #ifndef COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER 16 | #define COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | // Pinocchio has to be included before ROS 23 | // See https://github.com/wxmerkt/pinocchio_ros_example 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include "compliant_controllers/JointSpaceCompliantControllerConfig.h" 33 | #include "compliant_controllers/joint_space/controller.h" 34 | #include "compliant_controllers/robot_state.h" 35 | 36 | 37 | namespace compliant_controllers { 38 | namespace joint_space { 39 | 40 | // This is needed as we only want to specialize the effort implementation 41 | template 42 | class CompliantHardwareInterfaceAdapter; 43 | 44 | /**\class CompliantHardwareInterfaceAdapter 45 | * \brief 46 | * The hardware interface adapter that maps the position/velocity set-point to an effort command 47 | * by using the compliant controller 48 | * 49 | * \tparam State 50 | * The state representation used for the state of the controller (e.g. velocity, acceleration) 51 | */ 52 | template 53 | class CompliantHardwareInterfaceAdapter { 54 | public: 55 | /**\fn CompliantHardwareInterfaceAdapter 56 | * \brief 57 | * Constructor that allocates all necessary structures 58 | */ 59 | CompliantHardwareInterfaceAdapter(); 60 | CompliantHardwareInterfaceAdapter(CompliantHardwareInterfaceAdapter const&) = default; 61 | CompliantHardwareInterfaceAdapter& operator= (CompliantHardwareInterfaceAdapter const&) = default; 62 | CompliantHardwareInterfaceAdapter(CompliantHardwareInterfaceAdapter&&) = default; 63 | CompliantHardwareInterfaceAdapter& operator= (CompliantHardwareInterfaceAdapter&&) = default; 64 | 65 | /**\fn init 66 | * \brief 67 | * Constructor that allocates required memory 68 | * 69 | * \param[in] number_of_dof 70 | * Degrees of freedom of all of the joints combined 71 | * \param[in] threshold 72 | * Threshold for the joint angles 73 | */ 74 | [[nodiscard]] 75 | bool init(std::vector& joint_handles, 76 | ros::NodeHandle& controller_nh); 77 | 78 | /**\fn starting 79 | * \brief 80 | * Start the controller 81 | * 82 | * \param[in] time 83 | * The time when the controller was started 84 | */ 85 | void starting(ros::Time const& /*time*/); 86 | 87 | /**\fn stopping 88 | * \brief 89 | * Stop the controller 90 | * 91 | * \param[in] time 92 | * The time when the controller was stopped 93 | */ 94 | void stopping(ros::Time const& /*time*/); 95 | 96 | /**\fn updateCommand 97 | * \brief 98 | * Update the command value by writing to the internally stored joint handles 99 | * 100 | * \param[in] time 101 | * The time that the update command was called 102 | * \param[in] period 103 | * The period of the controller 104 | * \param[in] desired_state 105 | * The desired state that the controller should be set to 106 | * \param[in] state_error 107 | * The current state error 108 | */ 109 | void updateCommand(ros::Time const& /*time*/, ros::Duration const& period, 110 | State const& desired_state, State const& /*state_error*/); 111 | 112 | protected: 113 | /**\fn dynamicReconfigureCallback 114 | * \brief 115 | * Dynamically reconfigure the joint stiffness and other controller parameters 116 | * 117 | * \param[in] config 118 | * The config to be parsed 119 | * \param[in] period 120 | * The period of the controller 121 | * \param[in] desired_state 122 | * The desired state that the controller should be set to 123 | * \param[in] state_error 124 | * The current state error 125 | */ 126 | void dynamicReconfigureCallback(JointSpaceCompliantControllerConfig const& config, 127 | uint32_t const level); 128 | 129 | std::vector* joint_handles_ptr_; 130 | std::unique_ptr compliant_controller_; 131 | bool execute_default_command_; 132 | std::size_t num_of_dof_; 133 | RobotState desired_state_; 134 | RobotState current_state_; 135 | Eigen::VectorXd command_effort_; 136 | dynamic_reconfigure::Server< 137 | JointSpaceCompliantControllerConfig> dynamic_reconfigure_server_; 138 | dynamic_reconfigure::Server< 139 | JointSpaceCompliantControllerConfig>::CallbackType dynamic_reconfigure_callback_; 140 | }; 141 | 142 | } 143 | } 144 | 145 | #endif // COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER 146 | 147 | #include "compliant_controllers/joint_space/hardware_interface_adapter_impl.h" 148 | -------------------------------------------------------------------------------- /include/compliant_controllers/task_space/hardware_interface_adapter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file hardware_interface_adapter.h 3 | * \mainpage 4 | * Contains a custom hardware interface adapter for the compliant controller 5 | * This allows us to override the behavior of the default controller 6 | * 7 | * \authors 8 | * Tobit Flatscher 9 | * \copyright 10 | * Oxford Robotics Institute - University of Oxford (2024) 11 | * \license 12 | * This project is released under the 3-clause BSD license. 13 | */ 14 | 15 | #ifndef COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER 16 | #define COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | // Pinocchio has to be included before ROS 23 | // See https://github.com/wxmerkt/pinocchio_ros_example 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include "compliant_controllers/TaskSpaceCompliantControllerConfig.h" 33 | #include "compliant_controllers/task_space/controller.h" 34 | #include "compliant_controllers/robot_state.h" 35 | 36 | 37 | namespace compliant_controllers { 38 | namespace task_space { 39 | 40 | // This is needed as we only want to specialize the effort implementation 41 | template 42 | class CompliantHardwareInterfaceAdapter; 43 | 44 | /**\class CompliantHardwareInterfaceAdapter 45 | * \brief 46 | * The hardware interface adapter that maps the position/velocity set-point to an effort command 47 | * by using the compliant controller 48 | * 49 | * \tparam State 50 | * The state representation used for the state of the controller (e.g. velocity, acceleration) 51 | */ 52 | template 53 | class CompliantHardwareInterfaceAdapter { 54 | public: 55 | /**\fn CompliantHardwareInterfaceAdapter 56 | * \brief 57 | * Constructor that allocates all necessary structures 58 | */ 59 | CompliantHardwareInterfaceAdapter(); 60 | CompliantHardwareInterfaceAdapter(CompliantHardwareInterfaceAdapter const&) = default; 61 | CompliantHardwareInterfaceAdapter& operator= (CompliantHardwareInterfaceAdapter const&) = default; 62 | CompliantHardwareInterfaceAdapter(CompliantHardwareInterfaceAdapter&&) = default; 63 | CompliantHardwareInterfaceAdapter& operator= (CompliantHardwareInterfaceAdapter&&) = default; 64 | 65 | /**\fn init 66 | * \brief 67 | * Constructor that allocates required memory 68 | * 69 | * \param[in] number_of_dof 70 | * Degrees of freedom of all of the joints combined 71 | * \param[in] threshold 72 | * Threshold for the joint angles 73 | */ 74 | [[nodiscard]] 75 | bool init(std::vector& joint_handles, 76 | ros::NodeHandle& controller_nh); 77 | 78 | /**\fn starting 79 | * \brief 80 | * Start the controller 81 | * 82 | * \param[in] time 83 | * The time when the controller was started 84 | */ 85 | void starting(ros::Time const& /*time*/); 86 | 87 | /**\fn stopping 88 | * \brief 89 | * Stop the controller 90 | * 91 | * \param[in] time 92 | * The time when the controller was stopped 93 | */ 94 | void stopping(ros::Time const& /*time*/); 95 | 96 | /**\fn updateCommand 97 | * \brief 98 | * Update the command value by writing to the internally stored joint handles 99 | * 100 | * \param[in] time 101 | * The time that the update command was called 102 | * \param[in] period 103 | * The period of the controller 104 | * \param[in] desired_state 105 | * The desired state that the controller should be set to 106 | * \param[in] state_error 107 | * The current state error 108 | */ 109 | void updateCommand(ros::Time const& /*time*/, ros::Duration const& period, 110 | State const& desired_state, State const& /*state_error*/); 111 | 112 | protected: 113 | /**\fn dynamicReconfigureCallback 114 | * \brief 115 | * Dynamically reconfigure the joint stiffness and other controller parameters 116 | * 117 | * \param[in] config 118 | * The config to be parsed 119 | * \param[in] period 120 | * The period of the controller 121 | * \param[in] desired_state 122 | * The desired state that the controller should be set to 123 | * \param[in] state_error 124 | * The current state error 125 | */ 126 | void dynamicReconfigureCallback(TaskSpaceCompliantControllerConfig const& config, 127 | uint32_t const level); 128 | 129 | std::vector* joint_handles_ptr_; 130 | std::unique_ptr compliant_controller_; 131 | bool execute_default_command_; 132 | std::size_t num_of_dof_; 133 | RobotState desired_state_; 134 | RobotState current_state_; 135 | Eigen::VectorXd command_effort_; 136 | dynamic_reconfigure::Server< 137 | TaskSpaceCompliantControllerConfig> dynamic_reconfigure_server_; 138 | dynamic_reconfigure::Server< 139 | TaskSpaceCompliantControllerConfig>::CallbackType dynamic_reconfigure_callback_; 140 | }; 141 | } 142 | } 143 | 144 | #endif // COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER 145 | 146 | #include "compliant_controllers/task_space/hardware_interface_adapter_impl.h" 147 | -------------------------------------------------------------------------------- /include/compliant_controllers/joint_task_space/hardware_interface_adapter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file hardware_interface_adapter.h 3 | * \mainpage 4 | * Contains a custom hardware interface adapter for the compliant controller 5 | * This allows us to override the behavior of the default controller 6 | * 7 | * \authors 8 | * Tobit Flatscher 9 | * \copyright 10 | * Oxford Robotics Institute - University of Oxford (2024) 11 | * \license 12 | * This project is released under the 3-clause BSD license. 13 | */ 14 | 15 | #ifndef COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER 16 | #define COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | // Pinocchio has to be included before ROS 23 | // See https://github.com/wxmerkt/pinocchio_ros_example 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include "compliant_controllers/JointTaskSpaceCompliantControllerConfig.h" 33 | #include "compliant_controllers/joint_task_space/controller.h" 34 | #include "compliant_controllers/robot_state.h" 35 | 36 | 37 | namespace compliant_controllers { 38 | namespace joint_task_space { 39 | 40 | // This is needed as we only want to specialize the effort implementation 41 | template 42 | class CompliantHardwareInterfaceAdapter; 43 | 44 | /**\class CompliantHardwareInterfaceAdapter 45 | * \brief 46 | * The hardware interface adapter that maps the position/velocity set-point to an effort command 47 | * by using the compliant controller 48 | * 49 | * \tparam State 50 | * The state representation used for the state of the controller (e.g. velocity, acceleration) 51 | */ 52 | template 53 | class CompliantHardwareInterfaceAdapter { 54 | public: 55 | /**\fn CompliantHardwareInterfaceAdapter 56 | * \brief 57 | * Constructor that allocates all necessary structures 58 | */ 59 | CompliantHardwareInterfaceAdapter(); 60 | CompliantHardwareInterfaceAdapter(CompliantHardwareInterfaceAdapter const&) = default; 61 | CompliantHardwareInterfaceAdapter& operator= (CompliantHardwareInterfaceAdapter const&) = default; 62 | CompliantHardwareInterfaceAdapter(CompliantHardwareInterfaceAdapter&&) = default; 63 | CompliantHardwareInterfaceAdapter& operator= (CompliantHardwareInterfaceAdapter&&) = default; 64 | 65 | /**\fn init 66 | * \brief 67 | * Constructor that allocates required memory 68 | * 69 | * \param[in] number_of_dof 70 | * Degrees of freedom of all of the joints combined 71 | * \param[in] threshold 72 | * Threshold for the joint angles 73 | */ 74 | [[nodiscard]] 75 | bool init(std::vector& joint_handles, 76 | ros::NodeHandle& controller_nh); 77 | 78 | /**\fn starting 79 | * \brief 80 | * Start the controller 81 | * 82 | * \param[in] time 83 | * The time when the controller was started 84 | */ 85 | void starting(ros::Time const& /*time*/); 86 | 87 | /**\fn stopping 88 | * \brief 89 | * Stop the controller 90 | * 91 | * \param[in] time 92 | * The time when the controller was stopped 93 | */ 94 | void stopping(ros::Time const& /*time*/); 95 | 96 | /**\fn updateCommand 97 | * \brief 98 | * Update the command value by writing to the internally stored joint handles 99 | * 100 | * \param[in] time 101 | * The time that the update command was called 102 | * \param[in] period 103 | * The period of the controller 104 | * \param[in] desired_state 105 | * The desired state that the controller should be set to 106 | * \param[in] state_error 107 | * The current state error 108 | */ 109 | void updateCommand(ros::Time const& /*time*/, ros::Duration const& period, 110 | State const& desired_state, State const& /*state_error*/); 111 | 112 | protected: 113 | /**\fn dynamicReconfigureCallback 114 | * \brief 115 | * Dynamically reconfigure the joint stiffness and other controller parameters 116 | * 117 | * \param[in] config 118 | * The config to be parsed 119 | * \param[in] period 120 | * The period of the controller 121 | * \param[in] desired_state 122 | * The desired state that the controller should be set to 123 | * \param[in] state_error 124 | * The current state error 125 | */ 126 | void dynamicReconfigureCallback(JointTaskSpaceCompliantControllerConfig const& config, 127 | uint32_t const level); 128 | 129 | std::vector* joint_handles_ptr_; 130 | std::unique_ptr compliant_controller_; 131 | bool execute_default_command_; 132 | std::size_t num_of_dof_; 133 | RobotState desired_state_; 134 | RobotState current_state_; 135 | Eigen::VectorXd command_effort_; 136 | dynamic_reconfigure::Server< 137 | JointTaskSpaceCompliantControllerConfig> dynamic_reconfigure_server_; 138 | dynamic_reconfigure::Server< 139 | JointTaskSpaceCompliantControllerConfig>::CallbackType dynamic_reconfigure_callback_; 140 | }; 141 | } 142 | } 143 | 144 | #endif // COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER 145 | 146 | #include "compliant_controllers/joint_task_space/hardware_interface_adapter_impl.h" 147 | -------------------------------------------------------------------------------- /test/rviz/config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 1790 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Class: rviz/RobotModel 56 | Collision Enabled: false 57 | Enabled: true 58 | Links: 59 | All Links Enabled: true 60 | Expand Joint Details: false 61 | Expand Link Details: false 62 | Expand Tree: false 63 | Link Tree Style: Links in Alphabetic Order 64 | element_1: 65 | Alpha: 1 66 | Show Axes: false 67 | Show Trail: false 68 | Value: true 69 | element_2: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | world: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Name: RobotModel 79 | Robot Description: robot_description 80 | TF Prefix: "" 81 | Update Interval: 0 82 | Value: true 83 | Visual Enabled: true 84 | - Class: rviz/TF 85 | Enabled: true 86 | Filter (blacklist): "" 87 | Filter (whitelist): "" 88 | Frame Timeout: 15 89 | Frames: 90 | All Enabled: true 91 | element_1: 92 | Value: true 93 | element_2: 94 | Value: true 95 | world: 96 | Value: true 97 | Marker Alpha: 1 98 | Marker Scale: 1 99 | Name: TF 100 | Show Arrows: true 101 | Show Axes: true 102 | Show Names: true 103 | Tree: 104 | world: 105 | element_1: 106 | element_2: 107 | {} 108 | Update Interval: 0 109 | Value: true 110 | Enabled: true 111 | Global Options: 112 | Background Color: 48; 48; 48 113 | Default Light: true 114 | Fixed Frame: world 115 | Frame Rate: 30 116 | Name: root 117 | Tools: 118 | - Class: rviz/Interact 119 | Hide Inactive Objects: true 120 | - Class: rviz/MoveCamera 121 | - Class: rviz/Select 122 | - Class: rviz/FocusCamera 123 | - Class: rviz/Measure 124 | - Class: rviz/SetInitialPose 125 | Theta std deviation: 0.2617993950843811 126 | Topic: /initialpose 127 | X std deviation: 0.5 128 | Y std deviation: 0.5 129 | - Class: rviz/SetGoal 130 | Topic: /move_base_simple/goal 131 | - Class: rviz/PublishPoint 132 | Single click: true 133 | Topic: /clicked_point 134 | Value: true 135 | Views: 136 | Current: 137 | Class: rviz/Orbit 138 | Distance: 1.1003776788711548 139 | Enable Stereo Rendering: 140 | Stereo Eye Separation: 0.05999999865889549 141 | Stereo Focal Distance: 1 142 | Swap Stereo Eyes: false 143 | Value: false 144 | Field of View: 0.7853981852531433 145 | Focal Point: 146 | X: 0 147 | Y: 0 148 | Z: 0 149 | Focal Shape Fixed Size: true 150 | Focal Shape Size: 0.05000000074505806 151 | Invert Z Axis: false 152 | Name: Current View 153 | Near Clip Distance: 0.009999999776482582 154 | Pitch: 0.24039852619171143 155 | Target Frame: 156 | Yaw: 3.270400285720825 157 | Saved: ~ 158 | Window Geometry: 159 | Displays: 160 | collapsed: false 161 | Height: 2272 162 | Hide Left Dock: false 163 | Hide Right Dock: false 164 | QMainWindow State: 000000ff00000000fd0000000400000000000001dc000007ecfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000abfb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000007ec0000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f000007ecfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000069000007ec0000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e8400000056fc0100000002fb0000000800540069006d0065010000000000000e840000058200fffffffb0000000800540069006d0065010000000000000450000000000000000000000b41000007ec00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 165 | Selection: 166 | collapsed: false 167 | Time: 168 | collapsed: false 169 | Tool Properties: 170 | collapsed: false 171 | Views: 172 | collapsed: false 173 | Width: 3716 174 | X: 124 175 | Y: 54 176 | -------------------------------------------------------------------------------- /config/JointSpaceParams.cfg: -------------------------------------------------------------------------------- 1 | # Dynamic reconfigure parameters for joint-space compliant controller 2 | 3 | PACKAGE='compliant_controllers' 4 | 5 | from driver_base.msg import SensorLevels 6 | from dynamic_reconfigure.parameter_generator_catkin import * 7 | 8 | gen = ParameterGenerator() 9 | 10 | kj = gen.add_group("Kj") # Joint stiffness 11 | kj.add("j_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 0", 4000, 2000, 9000) 12 | kj.add("j_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 1", 4000, 2000, 9000) 13 | kj.add("j_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 2", 4000, 2000, 9000) 14 | kj.add("j_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 3", 4000, 2000, 9000) 15 | kj.add("j_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 4", 3500, 2000, 9000) 16 | kj.add("j_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 5", 3500, 2000, 9000) 17 | kj.add("j_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 6", 3500, 2000, 9000) 18 | 19 | b = gen.add_group("B") # Rotor inertia 20 | b.add("b_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 0", 0.3, 0.05, 0.5) 21 | b.add("b_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 1", 0.3, 0.05, 0.5) 22 | b.add("b_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 2", 0.3, 0.05, 0.5) 23 | b.add("b_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 3", 0.3, 0.05, 0.5) 24 | b.add("b_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 4", 0.18, 0.05, 0.5) 25 | b.add("b_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 5", 0.18, 0.05, 0.5) 26 | b.add("b_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 6", 0.18, 0.05, 0.5) 27 | 28 | kp = gen.add_group("Kp") # Stiffness gain 29 | kp.add("k_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for joint 0", 120, 10, 400) 30 | kp.add("k_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for joint 1", 80, 10, 400) 31 | kp.add("k_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for joint 2", 80, 10, 400) 32 | kp.add("k_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for joint 3", 120, 10, 400) 33 | kp.add("k_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for joint 4", 25, 5, 200) 34 | kp.add("k_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for joint 5", 20, 5, 200) 35 | kp.add("k_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for joint 6", 15, 5, 200) 36 | 37 | kd = gen.add_group("Kd") # Damping gain 38 | kd.add("d_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 0", 8.0, 1, 40) 39 | kd.add("d_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 1", 6.0, 1, 40) 40 | kd.add("d_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 2", 6.0, 1, 40) 41 | kd.add("d_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 3", 8.0, 1, 40) 42 | kd.add("d_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 4", 3.5, 1, 40) 43 | kd.add("d_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 5", 3.5, 1, 40) 44 | kd.add("d_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 6", 3.0, 1, 40) 45 | 46 | l = gen.add_group("L") # Friction observer gain 47 | l.add("l_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 0", 75, 20, 200) 48 | l.add("l_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 1", 75, 20, 200) 49 | l.add("l_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 2", 75, 20, 200) 50 | l.add("l_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 3", 75, 20, 200) 51 | l.add("l_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 4", 40, 20, 200) 52 | l.add("l_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 5", 40, 20, 200) 53 | l.add("l_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 6", 40, 20, 200) 54 | 55 | lp = gen.add_group("Lp") # Friction observer proportional gain 56 | lp.add("lp_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 0", 20, 1, 30) 57 | lp.add("lp_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 1", 20, 1, 30) 58 | lp.add("lp_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 2", 20, 1, 30) 59 | lp.add("lp_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 3", 20, 1, 30) 60 | lp.add("lp_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 4", 15, 1, 30) 61 | lp.add("lp_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 5", 15, 1, 30) 62 | lp.add("lp_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 6", 15, 1, 30) 63 | 64 | li = gen.add_group("Li") # Friction observer integral gain -- must be less than 0.5 * Lp^2 65 | li.add("li_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 0", 60.0, 0, 400.0) 66 | li.add("li_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 1", 60.0, 0, 400.0) 67 | li.add("li_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 2", 60.0, 0, 400.0) 68 | li.add("li_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 3", 60.0, 0, 400.0) 69 | li.add("li_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 4", 60.0, 0, 400.0) 70 | li.add("li_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 5", 60.0, 0, 400.0) 71 | li.add("li_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 6", 60.0, 0, 400.0) 72 | 73 | e_max = gen.add_group("E_max") # Friction observer maximum joint error 74 | e_max.add("e_max_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 0", 0.5, 0, 1.0) 75 | e_max.add("e_max_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 1", 0.5, 0, 1.0) 76 | e_max.add("e_max_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 2", 0.5, 0, 1.0) 77 | e_max.add("e_max_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 3", 0.5, 0, 1.0) 78 | e_max.add("e_max_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 4", 0.5, 0, 1.0) 79 | e_max.add("e_max_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 5", 0.5, 0, 1.0) 80 | e_max.add("e_max_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 6", 0.5, 0, 1.0) 81 | 82 | 83 | exit(gen.generate(PACKAGE, "joint_space_compliance", "JointSpaceCompliantController")) 84 | -------------------------------------------------------------------------------- /config/TaskSpaceParams.cfg: -------------------------------------------------------------------------------- 1 | # Dynamic reconfigure parameters for joint-space compliant controller 2 | 3 | PACKAGE='compliant_controllers' 4 | 5 | from driver_base.msg import SensorLevels 6 | from dynamic_reconfigure.parameter_generator_catkin import * 7 | 8 | gen = ParameterGenerator() 9 | 10 | kj = gen.add_group("Kj") # Joint stiffness 11 | kj.add("j_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 0", 4000, 2000, 9000) 12 | kj.add("j_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 1", 4000, 2000, 9000) 13 | kj.add("j_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 2", 4000, 2000, 9000) 14 | kj.add("j_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 3", 4000, 2000, 9000) 15 | kj.add("j_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 4", 3500, 2000, 9000) 16 | kj.add("j_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 5", 3500, 2000, 9000) 17 | kj.add("j_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 6", 3500, 2000, 9000) 18 | 19 | b = gen.add_group("B") # Rotor inertia 20 | b.add("b_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 0", 0.3, 0.05, 0.5) 21 | b.add("b_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 1", 0.3, 0.05, 0.5) 22 | b.add("b_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 2", 0.3, 0.05, 0.5) 23 | b.add("b_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 3", 0.3, 0.05, 0.5) 24 | b.add("b_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 4", 0.18, 0.05, 0.5) 25 | b.add("b_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 5", 0.18, 0.05, 0.5) 26 | b.add("b_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 6", 0.18, 0.05, 0.5) 27 | 28 | task_kp = gen.add_group("TaskKp") # Task space Stiffness gain 29 | task_kp.add("task_k_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for ee translation x", 330, 20, 1000) 30 | task_kp.add("task_k_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for ee translation y", 340, 20, 1000) 31 | task_kp.add("task_k_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for ee translation z", 450, 20, 1000) 32 | task_kp.add("task_k_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for ee rotation x", 100, 1, 100) 33 | task_kp.add("task_k_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for ee rotation y", 100, 1, 100) 34 | task_kp.add("task_k_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for ee rotation z", 100, 1, 100) 35 | 36 | task_kd = gen.add_group("TaskKd") # Task space Damping gain 37 | task_kd.add("task_d_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Task Damping gain for joint 0", 8.2, 0.0, 20) 38 | task_kd.add("task_d_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Task Damping gain for joint 1", 8.0, 0.0, 20) 39 | task_kd.add("task_d_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Task Damping gain for joint 2", 7.8, 0.0, 20) 40 | task_kd.add("task_d_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Task Damping gain for joint 3", 6.8, 0.0, 20) 41 | task_kd.add("task_d_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Task Damping gain for joint 4", 6.8, 0.0, 20) 42 | task_kd.add("task_d_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Task Damping gain for joint 5", 6.8, 0.0, 20) 43 | 44 | joint_kd = gen.add_group("JointKd") # Joint space Damping gain 45 | joint_kd.add("joint_d_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 0", 2.3, 1, 10) 46 | joint_kd.add("joint_d_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 1", 2.3, 1, 10) 47 | joint_kd.add("joint_d_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 2", 2.3, 1, 10) 48 | joint_kd.add("joint_d_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 3", 2.3, 1, 10) 49 | joint_kd.add("joint_d_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 4", 2.3, 1, 10) 50 | joint_kd.add("joint_d_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 5", 2.3, 1, 10) 51 | joint_kd.add("joint_d_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 6", 2.3, 1, 10) 52 | 53 | l = gen.add_group("L") # Friction observer gain 54 | l.add("l_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 0", 75, 20, 200) 55 | l.add("l_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 1", 75, 20, 200) 56 | l.add("l_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 2", 75, 20, 200) 57 | l.add("l_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 3", 75, 20, 200) 58 | l.add("l_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 4", 40, 20, 200) 59 | l.add("l_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 5", 40, 20, 200) 60 | l.add("l_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 6", 40, 20, 200) 61 | 62 | lp = gen.add_group("Lp") # Friction observer proportional gain 63 | lp.add("lp_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 0", 18, 1, 30) 64 | lp.add("lp_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 1", 18, 1, 30) 65 | lp.add("lp_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 2", 18, 1, 30) 66 | lp.add("lp_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 3", 15, 1, 30) 67 | lp.add("lp_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 4", 13, 1, 30) 68 | lp.add("lp_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 5", 13, 1, 30) 69 | lp.add("lp_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 6", 13, 1, 30) 70 | 71 | li = gen.add_group("Li") # Friction observer integral gain -- must be less than 0.5 * Lp^2 72 | li.add("li_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 0", 1.0, 0, 400.0) 73 | li.add("li_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 1", 1.0, 0, 400.0) 74 | li.add("li_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 2", 1.0, 0, 400.0) 75 | li.add("li_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 3", 1.0, 0, 400.0) 76 | li.add("li_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 4", 1.0, 0, 400.0) 77 | li.add("li_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 5", 1.0, 0, 400.0) 78 | li.add("li_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 6", 1.0, 0, 400.0) 79 | 80 | e_max = gen.add_group("E_max") # Friction observer maximum joint error 81 | e_max.add("e_max_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 0", 0.5, 0, 1.0) 82 | e_max.add("e_max_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 1", 0.5, 0, 1.0) 83 | e_max.add("e_max_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 2", 0.5, 0, 1.0) 84 | e_max.add("e_max_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 3", 0.5, 0, 1.0) 85 | e_max.add("e_max_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 4", 0.5, 0, 1.0) 86 | e_max.add("e_max_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 5", 0.5, 0, 1.0) 87 | e_max.add("e_max_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 6", 0.5, 0, 1.0) 88 | 89 | 90 | exit(gen.generate(PACKAGE, "task_space_compliance", "TaskSpaceCompliantController")) 91 | -------------------------------------------------------------------------------- /config/JointTaskSpaceParams.cfg: -------------------------------------------------------------------------------- 1 | # Dynamic reconfigure parameters for joint-space compliant controller 2 | 3 | PACKAGE='compliant_controllers' 4 | 5 | from driver_base.msg import SensorLevels 6 | from dynamic_reconfigure.parameter_generator_catkin import * 7 | 8 | gen = ParameterGenerator() 9 | 10 | kj = gen.add_group("Kj") # Joint stiffness 11 | kj.add("j_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 0", 4000, 2000, 9000) 12 | kj.add("j_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 1", 4000, 2000, 9000) 13 | kj.add("j_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 2", 4000, 2000, 9000) 14 | kj.add("j_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 3", 4000, 2000, 9000) 15 | kj.add("j_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 4", 3500, 2000, 9000) 16 | kj.add("j_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 5", 3500, 2000, 9000) 17 | kj.add("j_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness for joint 6", 3500, 2000, 9000) 18 | 19 | b = gen.add_group("B") # Rotor inertia 20 | b.add("b_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 0", 0.3, 0.05, 0.5) 21 | b.add("b_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 1", 0.3, 0.05, 0.5) 22 | b.add("b_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 2", 0.3, 0.05, 0.5) 23 | b.add("b_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 3", 0.3, 0.05, 0.5) 24 | b.add("b_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 4", 0.18, 0.05, 0.5) 25 | b.add("b_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 5", 0.18, 0.05, 0.5) 26 | b.add("b_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Rotor inertia for joint 6", 0.18, 0.05, 0.5) 27 | 28 | task_kp = gen.add_group("TaskKp") # Task space Stiffness gain 29 | task_kp.add("task_k_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for ee translation x", 1000, 20, 2000) 30 | task_kp.add("task_k_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for ee translation y", 1000, 20, 2000) 31 | task_kp.add("task_k_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for ee translation z", 1000, 20, 2000) 32 | task_kp.add("task_k_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for ee rotation x", 200, 1, 1000) 33 | task_kp.add("task_k_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for ee rotation y", 200, 1, 1000) 34 | task_kp.add("task_k_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for ee rotation z", 200, 1, 1000) 35 | 36 | task_kd = gen.add_group("TaskKd") # Task space Damping gain 37 | task_kd.add("task_d_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Task Damping gain for joint 0", 4.6, 0.0, 200) 38 | task_kd.add("task_d_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Task Damping gain for joint 1", 4.6, 0.0, 200) 39 | task_kd.add("task_d_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Task Damping gain for joint 2", 4.6, 0.0, 200) 40 | task_kd.add("task_d_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Task Damping gain for joint 3", 2.0, 0.0, 200) 41 | task_kd.add("task_d_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Task Damping gain for joint 4", 2.0, 0.0, 200) 42 | task_kd.add("task_d_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Task Damping gain for joint 5", 2.0, 0.0, 200) 43 | 44 | joint_kp = gen.add_group("JointKp") # Joint Stiffness gain 45 | joint_kp.add("joint_k_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for joint 0", 22.0, 0, 200) 46 | joint_kp.add("joint_k_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for joint 1", 10.0, 0, 200) 47 | joint_kp.add("joint_k_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for joint 2", 10.0, 0, 200) 48 | joint_kp.add("joint_k_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for joint 3", 20.0, 0, 200) 49 | joint_kp.add("joint_k_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for joint 4", 8.0, 0, 200) 50 | joint_kp.add("joint_k_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for joint 5", 8.0, 0, 200) 51 | joint_kp.add("joint_k_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Stiffness gain for joint 6", 6.0, 0, 200) 52 | 53 | joint_kd = gen.add_group("JointKd") # Joint space Damping gain 54 | joint_kd.add("joint_d_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 0", 4.0, 1, 40) 55 | joint_kd.add("joint_d_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 1", 4.0, 1, 40) 56 | joint_kd.add("joint_d_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 2", 4.0, 1, 40) 57 | joint_kd.add("joint_d_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 3", 4.0, 1, 40) 58 | joint_kd.add("joint_d_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 4", 4.0, 1, 40) 59 | joint_kd.add("joint_d_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 5", 4.0, 1, 40) 60 | joint_kd.add("joint_d_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Damping gain for joint 6", 4.0, 1, 40) 61 | 62 | l = gen.add_group("L") # Friction observer gain 63 | l.add("l_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 0", 75, 20, 200) 64 | l.add("l_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 1", 75, 20, 200) 65 | l.add("l_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 2", 75, 20, 200) 66 | l.add("l_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 3", 75, 20, 200) 67 | l.add("l_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 4", 40, 20, 200) 68 | l.add("l_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 5", 40, 20, 200) 69 | l.add("l_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer gain for joint 6", 40, 20, 200) 70 | 71 | lp = gen.add_group("Lp") # Friction observer proportional gain 72 | lp.add("lp_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 0", 20, 1, 30) 73 | lp.add("lp_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 1", 20, 1, 30) 74 | lp.add("lp_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 2", 20, 1, 30) 75 | lp.add("lp_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 3", 20, 1, 30) 76 | lp.add("lp_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 4", 15, 1, 30) 77 | lp.add("lp_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 5", 15, 1, 30) 78 | lp.add("lp_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer proportional gain for joint 6", 15, 1, 30) 79 | 80 | li = gen.add_group("Li") # Friction observer integral gain -- must be less than 0.5 * Lp^2 81 | li.add("li_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 0", 60.0, 0, 400.0) 82 | li.add("li_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 1", 60.0, 0, 400.0) 83 | li.add("li_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 2", 60.0, 0, 400.0) 84 | li.add("li_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 3", 60.0, 0, 400.0) 85 | li.add("li_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 4", 60.0, 0, 400.0) 86 | li.add("li_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 5", 60.0, 0, 400.0) 87 | li.add("li_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer integral gain for joint 6", 60.0, 0, 400.0) 88 | 89 | e_max = gen.add_group("E_max") # Friction observer maximum joint error 90 | e_max.add("e_max_0", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 0", 0.5, 0, 1.0) 91 | e_max.add("e_max_1", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 1", 0.5, 0, 1.0) 92 | e_max.add("e_max_2", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 2", 0.5, 0, 1.0) 93 | e_max.add("e_max_3", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 3", 0.5, 0, 1.0) 94 | e_max.add("e_max_4", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 4", 0.5, 0, 1.0) 95 | e_max.add("e_max_5", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 5", 0.5, 0, 1.0) 96 | e_max.add("e_max_6", double_t, SensorLevels.RECONFIGURE_RUNNING, "Friction observer max joint error for joint 6", 0.5, 0, 1.0) 97 | 98 | 99 | exit(gen.generate(PACKAGE, "joint_task_space_compliance", "JointTaskSpaceCompliantController")) 100 | -------------------------------------------------------------------------------- /src/joint_space/controller.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file joint_space_compliant_controller.cpp 3 | * \mainpage 4 | * Compliant controller in joint space 5 | * 6 | * \authors 7 | * Tobit Flatscher 8 | * Rishabh Madan 9 | * Rajat Kumar Jenamani 10 | * \copyright 11 | * Oxford Robotics Institute - University of Oxford (2024) 12 | * EmPRISE Lab - Cornell University (2023) 13 | * \license 14 | * This project is released under the 3-clause BSD license. 15 | */ 16 | 17 | #include "compliant_controllers/joint_space/controller.h" 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | // Pinocchio has to be included before ROS 25 | // See https://github.com/wxmerkt/pinocchio_ros_example 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include "compliant_controllers/extended_joint_positions.h" 37 | #include "compliant_controllers/robot_state.h" 38 | #include "compliant_controllers/utils.h" 39 | 40 | 41 | namespace compliant_controllers { 42 | namespace joint_space { 43 | 44 | CompliantController::CompliantController( 45 | std::unique_ptr robot_model, std::string const& end_effector_link, 46 | int const num_controlled_dofs, bool apply_gravity) 47 | : robot_model_{std::move(robot_model)}, end_effector_link_{}, 48 | data_{std::make_unique(*robot_model_.get())}, 49 | num_controlled_dofs_{num_controlled_dofs}, apply_gravity_{apply_gravity}, 50 | end_effector_index_{robot_model_->getFrameId(end_effector_link)} { 51 | setDefaultValues(); 52 | extended_joints_ = std::make_unique(num_controlled_dofs_); 53 | // TODO: Potentially pre-allocate also other structures 54 | return; 55 | } 56 | 57 | void CompliantController::setDefaultValues() { 58 | Eigen::MatrixXd const joint_stiffness_matrix {constructDiagonalMatrix(3750, num_controlled_dofs_)}; 59 | inverse_joint_stiffness_matrix_ = joint_stiffness_matrix.inverse(); 60 | rotor_inertia_matrix_ = constructDiagonalMatrix(0.3, num_controlled_dofs_); 61 | inverse_rotor_inertia_matrix_ = rotor_inertia_matrix_.inverse(); 62 | friction_l_ = constructDiagonalMatrix(60, num_controlled_dofs_); 63 | friction_lp_ = constructDiagonalMatrix(4, num_controlled_dofs_); 64 | friction_li_ = constructDiagonalMatrix(0, num_controlled_dofs_); 65 | joint_k_matrix_ = constructDiagonalMatrix(10, num_controlled_dofs_); 66 | joint_d_matrix_ = constructDiagonalMatrix(2, num_controlled_dofs_); 67 | q_error_ = Eigen::VectorXd::Zero(num_controlled_dofs_); 68 | q_error_sum_ = Eigen::VectorXd::Zero(num_controlled_dofs_); 69 | q_error_max_ = Eigen::VectorXd::Zero(num_controlled_dofs_); 70 | return; 71 | } 72 | 73 | bool CompliantController::setJointStiffnessMatrix( 74 | Eigen::MatrixXd const& joint_stiffness_matrix) { 75 | if (!checkMatrix(joint_stiffness_matrix)) { 76 | return false; 77 | } 78 | inverse_joint_stiffness_matrix_ = joint_stiffness_matrix.inverse(); 79 | return true; 80 | } 81 | 82 | bool CompliantController::setRotorInertiaMatrix( 83 | Eigen::MatrixXd const& rotor_inertia_matrix) { 84 | if (!checkMatrix(rotor_inertia_matrix)) { 85 | return false; 86 | } 87 | rotor_inertia_matrix_ = rotor_inertia_matrix; 88 | inverse_rotor_inertia_matrix_ = rotor_inertia_matrix.inverse(); 89 | return true; 90 | } 91 | 92 | bool CompliantController::setFrictionL(Eigen::MatrixXd const& friction_l) { 93 | if (!checkMatrix(friction_l)) { 94 | return false; 95 | } 96 | friction_l_ = friction_l; 97 | return true; 98 | } 99 | 100 | bool CompliantController::setFrictionLp(Eigen::MatrixXd const& friction_lp) { 101 | if (!checkMatrix(friction_lp)) { 102 | return false; 103 | } 104 | friction_lp_ = friction_lp; 105 | return true; 106 | } 107 | 108 | bool CompliantController::setFrictionLi(Eigen::MatrixXd const& friction_li) { 109 | if (!checkMatrix(friction_li)) { 110 | return false; 111 | } 112 | friction_li_ = friction_li; 113 | return true; 114 | } 115 | 116 | bool CompliantController::setJointKMatrix(Eigen::MatrixXd const& joint_k_matrix) { 117 | if (!checkMatrix(joint_k_matrix)) { 118 | return false; 119 | } 120 | joint_k_matrix_ = joint_k_matrix; 121 | return true; 122 | } 123 | 124 | bool CompliantController::setJointDMatrix(Eigen::MatrixXd const& joint_d_matrix) { 125 | if (!checkMatrix(joint_d_matrix)) { 126 | return false; 127 | } 128 | joint_d_matrix_ = joint_d_matrix; 129 | return true; 130 | } 131 | 132 | bool CompliantController::setMaxJointError(Eigen::VectorXd const& joint_error_max) 133 | { 134 | if (!joint_error_max.rows() != num_controlled_dofs_) return false; 135 | q_error_max_ = joint_error_max; 136 | return true; 137 | } 138 | 139 | bool CompliantController::init() { 140 | count_ = 0; 141 | q_error_.setZero(); 142 | q_error_sum_.setZero(); 143 | return true; 144 | } 145 | 146 | Eigen::VectorXd CompliantController::computeEffort(RobotState const& desired_state, 147 | RobotState const& current_state, ros::Duration const& period) { 148 | desired_positions_ = desired_state.positions; 149 | 150 | if (!extended_joints_->isInitialized()) { 151 | last_desired_positions_ = desired_state.positions; 152 | [[maybe_unused]] bool const is_success {extended_joints_->init(current_state.positions)}; 153 | extended_joints_->update(current_state.positions); 154 | nominal_theta_prev_ = extended_joints_->getPositions(); 155 | nominal_theta_dot_prev_ = current_state.velocities; 156 | desired_positions_ = nominal_theta_prev_; 157 | } 158 | 159 | extended_joints_->update(current_state.positions); 160 | current_theta_ = extended_joints_->getPositions(); 161 | gravity_ = pinocchio::computeGeneralizedGravity(*robot_model_, *data_, joint_ros_to_pinocchio(current_theta_, *robot_model_)); 162 | 163 | // Gravity compensation is performed inside the hardware interface 164 | task_effort_ = -joint_k_matrix_*(nominal_theta_prev_ - desired_positions_ - inverse_joint_stiffness_matrix_*gravity_) - 165 | joint_d_matrix_*(nominal_theta_dot_prev_ - desired_state.velocities); 166 | 167 | // Time step smaller than period.toSec() potentially because of too small rotor inertia matrix 168 | double const step_time {0.001}; 169 | // Sign of the current state effort seems to be negative of commanded 170 | nominal_theta_d_dot_ = inverse_rotor_inertia_matrix_*(task_effort_ + gravity_ + current_state.efforts); 171 | nominal_theta_dot_ = nominal_theta_dot_prev_ + nominal_theta_d_dot_*step_time; 172 | nominal_theta_ = nominal_theta_prev_ + nominal_theta_dot_*step_time; 173 | 174 | // Integrate friction error 175 | q_error_sum_ = integrate_error(nominal_theta_, current_theta_); 176 | 177 | nominal_friction_ = rotor_inertia_matrix_*friction_l_*((nominal_theta_dot_ - current_state.velocities) + 178 | friction_lp_*(nominal_theta_ - current_theta_) 179 | + friction_li_ * q_error_sum_); 180 | 181 | efforts_ = task_effort_ + nominal_friction_; 182 | 183 | // Apply gravity compensation if required 184 | if (apply_gravity_) { efforts_ += gravity_; } 185 | 186 | nominal_theta_prev_ = nominal_theta_; 187 | nominal_theta_dot_prev_ = nominal_theta_dot_; 188 | 189 | return efforts_; 190 | } 191 | 192 | Eigen::VectorXd CompliantController::integrate_error(Eigen::VectorXd const& desired_q, 193 | Eigen::VectorXd const& current_q) 194 | { 195 | q_error_ = desired_q - current_q; 196 | q_error_sum_ += q_error_; 197 | // Clamp the integrated error 198 | q_error_sum_ = q_error_sum_.cwiseMin(q_error_max_).cwiseMax(-q_error_max_); 199 | return q_error_sum_; 200 | } 201 | 202 | Eigen::MatrixXd CompliantController::constructDiagonalMatrix(double const value, int const dim) { 203 | Eigen::VectorXd diag_elements {Eigen::VectorXd::Zero(dim)}; 204 | for (Eigen::Index i = 0; i < diag_elements.size(); ++i) { 205 | diag_elements[i] = value; 206 | } 207 | Eigen::MatrixXd matrix {Eigen::MatrixXd::Zero(dim, dim)}; 208 | matrix.diagonal() = diag_elements; 209 | return matrix; 210 | } 211 | 212 | bool CompliantController::checkMatrix(Eigen::MatrixXd const& matrix) const noexcept { 213 | if ((matrix.rows() != num_controlled_dofs_) || (matrix.cols() != num_controlled_dofs_)) { 214 | return false; 215 | } 216 | return true; 217 | } 218 | 219 | } // namespace joint_space 220 | } // namespace compliant_controllers -------------------------------------------------------------------------------- /ReadMe.md: -------------------------------------------------------------------------------- 1 | # Compliant controllers 2 | 3 | Authors: [Tobit Flatscher](https://github.com/2b-t), [Alexander Mitchell](https://github.com/mitch722) (2024-25) 4 | 5 | 6 | 7 | This package contains a **compliant controller for robotic arms in ROS Noetic** and extends on the [`gen3_compliant_controllers`](https://github.com/empriselab/gen3_compliant_controllers) repository and wraps it as a [`JointTrajectoryController`](http://wiki.ros.org/joint_trajectory_controller) that performs interpolation in between different set-points. This means that the controller can be used in combination with [MoveIt](https://moveit.ros.org/install/). As a result of wrapping the controller as JointTrajectoryController, this code base has been largely rewritten in comparison to gen3_compliant_controllers. 8 | Currently this package only supports robotic arms with revolute and no prismatic joints. 9 | 10 | We include a video of some of the capabilities of the controller below. 11 | 12 | 13 | https://github.com/user-attachments/assets/e98561c3-c4f2-404f-8530-9cf1f278964a 14 | 15 | 16 | 17 | The video above shows the joint-, task- and joint and task-space control modes. The joint and task-space mode is default. It is possible to vary the gains of the joint and task space controller so that it becomes either a task or joint space controller only using dynamic reconfigure. 18 | 19 | The controller can be used for pin insertion and for replanning in the event of human interaction as is shown in the video below: 20 | 21 | https://github.com/user-attachments/assets/611a5bd9-6b1f-4e88-bd31-5ce5a897ff2d 22 | 23 | The default task and joint-space controller is tuned for both expansive movements as well as fine assembly insertions. We show these behaviours in the video below. 24 | 25 | https://github.com/user-attachments/assets/c386813f-6a8c-42f3-81cf-0325f8897e6d 26 | 27 | ## Usage 28 | 29 | ### Running on the hardware 30 | 31 | This code must be used in combination with the **`kortex_hardware` hardware interface** (see [here](https://github.com/applied-ai-lab/kortex_hardware) for the installation instructions) as the hardware interface implements torque filtering as well as **gravity compensation**. 32 | 33 | You will have to run the `kortex_hardware` hardware interface as well as this controller. The hardware interface launches in effort mode by default, meaning that the controller can be started. 34 | 35 | Proceed to activate the controller by opening the `rqt_controller_manager` GUI, selecting the controller and switching it to `running`: 36 | 37 | ```bash 38 | $ rosrun rqt_controller_manager rqt_controller_manager 39 | ``` 40 | 41 | 42 | ### Running in simulation 43 | 44 | This controller does not work well in simulation, even when increasing friction and damping for Gazebo inside the URDF. Probably the reason for this is that the `kortex_hardware` hardware interface implements torque filtering as well as gravity compensation that are both not handled by the simulated hardware interface. It seems to be workable when increasing stiffness of the system by increasing the joint stiffness matrix (e.g. to `8000`), joint compliance proportional gain matrix (e.g. to `100`) as well as the joint compliance derivative gain matrix (e.g. to `10`). 45 | 46 | You can test the controllers on a single arm by installing `ros_kortex` (see [here](https://github.com/Kinovarobotics/ros_kortex)) and then **launching the Gazebo simulation**: 47 | 48 | ```bash 49 | $ roslaunch kortex_gazebo spawn_kortex_robot.launch 50 | ``` 51 | 52 | Then **run the `compliant_controller`** making sure it uses the same name space as the robot, by default `my_gen3`, 53 | 54 | ```bash 55 | $ roslaunch compliant_controllers joint_space_compliant_controller.launch robot_description_parameter:=/my_gen3/robot_description __ns:=my_gen3 56 | ``` 57 | 58 | adjust the gains if necessary (as discussed above) and **switch to the compliant controller**: 59 | 60 | ```bash 61 | $ rosservice call /my_gen3/controller_manager/switch_controller "start_controllers: ['joint_space_compliant_controller'] 62 | stop_controllers: ['gen3_joint_trajectory_controller'] 63 | strictness: 1 64 | start_asap: false 65 | timeout: 0.0" 66 | ``` 67 | 68 | Finally you can control the arm with the **`rqt_joint_trajectory_controller`** graphic user interface: 69 | 70 | ```bash 71 | $ rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller __ns:=my_gen3 72 | ``` 73 | 74 | Choose the corresponding controller manager, select the desired joint trajectory controller and switch it to active. You should now be able to move all joints independently in a smooth manner with the sliders. 75 | 76 | 77 | ## Sending Smooth Trajecotories and Replanning 78 | 79 | As mentioned, this controller is a compliant controller wrapping a [`JointTrajectoryController`](http://wiki.ros.org/joint_trajectory_controller). You can stream a set point message or a **sparse** trajectory, which is a list of set points. When a single set point is sent to the controller, this will be the input to the control algorithm. Alternatively, the controller will interpolate between the current robot motion and the input trajectory. This is explained in more detail in the subsections below. 80 | 81 | ### Streaming Joint Trajectory Points 82 | 83 | Streaming single [`JointTrajectoryPoint`](https://docs.ros.org/en/noetic/api/trajectory_msgs/html/msg/JointTrajectoryPoint.html) at high frequency works well for closed-loop replanning and tele-operation. This is achieved by publishing [`JointTrajectoryPoint`](https://docs.ros.org/en/noetic/api/trajectory_msgs/html/msg/JointTrajectoryPoint.html) to the [`JointTrajectoryController`](http://wiki.ros.org/joint_trajectory_controller) topic. 84 | 85 | ### Sending Joint Trajectories 86 | 87 | Sending a [`trajectory of set points`](https://docs.ros.org/en/noetic/api/trajectory_msgs/html/msg/JointTrajectory.html) to the `JointTrajectoryController's`](http://wiki.ros.org/joint_trajectory_controller) action server, triggers the controller to interpolate between the robot's current motion and the new trajectory. Documentation on replanning with the trajectory follower controller is found [`here`](https://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement). 88 | 89 | The controller interpolates between trajectories using splines of varying order. See the table below for more details: 90 | 91 | | Spline Order | Poses | Velocities | Accelerations | Notes | 92 | | ------------- | ----- | ---------- | ------------- | -------------------- | 93 | | 3 | Yes | No | No | Not recommended | 94 | | 4 | Yes | Yes | No | | 95 | | 5 | Yes | Yes | Yes | | 96 | 97 | 98 | The trajectory follower fits a spline starting at the *current* joint positions, velocities and accelerations and ending at the beginning of a *future* set point. As shown above, smoothest results are achieved when velocities and accelerations are included in the new trajectory. 99 | 100 | Furthermore, do not send a new trajectory with a set point at the current joint state. For example, if you are at timestep t, do not send a pose for this timestep, but send the set point for t + 1 as the initial or only pose. The trajectory follower will interpolate from its own states at t and find a smooth path to the set point at t + 1. See Figure 2 in [`UnderstaningJointTrajectoryReplacement`](https://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement) for an example of this. 101 | 102 | 103 | ## Code Breakdown 104 | 105 | The algorithms and ROS control architecture are written in separate files. The non-ROS parts algorithms are found in 106 | 107 | ```bash 108 | https://github.com/applied-ai-lab/compliant_controllers/blob/feat/joint-task-combo/include/compliant_controllers//controller.h 109 | and 110 | https://github.com/applied-ai-lab/compliant_controllers/blob/feat/joint-task-combo/src//controller.cpp 111 | ``` 112 | where the **MODE** is the controller type e.g. (joint_task_space, joint, task). 113 | 114 | ## Cite This Controller 115 | 116 | We have written a technical report and posted it on [arxiv](https://arxiv.org/abs/2504.21159). Please cite this paper when you use this controller in your publications. 117 | 118 | ```bash 119 | @misc{mitchell2025taskjointspacedualarm, 120 | title={Task and Joint Space Dual-Arm Compliant Control}, 121 | author={Alexander L. Mitchell and Tobit Flatscher and Ingmar Posner}, 122 | year={2025}, 123 | eprint={2504.21159}, 124 | archivePrefix={arXiv}, 125 | primaryClass={cs.RO}, 126 | url={https://arxiv.org/abs/2504.21159}, 127 | } 128 | ``` 129 | 130 | ## Known issues 131 | 132 | This package has a few limitations mostly stemming from its initial implementation. We will try to resolve these over time. 133 | 134 | - The controller **time step** is **hard-coded** into the controller, setting it to an actual realistic time does not seem to work. 135 | - Running the simulation currently requires modifications to the `ros_controllers` repository to run in **simulation**. A template argument has to be added to the `JointTrajectoryController` with the default value `HardwareInterfaceAdapter` to the `JointTrajectoryController`: `template class Adapter = HardwareInterfaceAdapter> class JointTrajectoryController` (see [here](https://github.com/ros-controls/ros_controllers/blob/678b92adfd9242c93b78c066a8369c7665ea1421/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h#L178)) and the joint trajectory controller in this repository has to call this `CompliantHardwareInterface` instead. Otherwise the code will run into a **segmentation fault inside Gazebo**. 136 | - Currently dynamic reconfigure does not support variable size arrays. For this reason we implemented dynamic reconfigure for a fixed size list of parameters holding sufficient parameters for most traditional robotic arms (7). In case the robot does not have the corresponding joints, the last additional degrees of freedom will not be used. 137 | 138 | -------------------------------------------------------------------------------- /include/compliant_controllers/joint_space/hardware_interface_adapter_impl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file hardware_interface_adapter_impl.h 3 | * \mainpage 4 | * Implementation of the compliant hardware interface adapter 5 | * 6 | * \authors 7 | * Tobit Flatscher 8 | * \copyright 9 | * Oxford Robotics Institute - University of Oxford (2024) 10 | * \license 11 | * This project is released under the 3-clause BSD license. 12 | */ 13 | 14 | #ifndef COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_IMPL 15 | #define COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_IMPL 16 | #pragma once 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | // Pinocchio has to be included before ROS 24 | // See https://github.com/wxmerkt/pinocchio_ros_example 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "compliant_controllers/JointSpaceCompliantControllerConfig.h" 35 | #include "compliant_controllers/joint_space/controller.h" 36 | #include "compliant_controllers/robot_state.h" 37 | 38 | 39 | namespace compliant_controllers { 40 | namespace joint_space { 41 | 42 | template 43 | CompliantHardwareInterfaceAdapter::CompliantHardwareInterfaceAdapter() 44 | : joint_handles_ptr_{nullptr} { 45 | return; 46 | } 47 | 48 | template 49 | bool CompliantHardwareInterfaceAdapter::init( 50 | std::vector& joint_handles, ros::NodeHandle& controller_nh) { 51 | joint_handles_ptr_ = &joint_handles; 52 | 53 | std::string robot_description_parameter {"/robot_description"}; 54 | if (!controller_nh.getParam("robot_description_parameter", robot_description_parameter)) { 55 | ROS_WARN_STREAM("Failed getting robot description parameter, defaulting to '" << robot_description_parameter << "'!"); 56 | } 57 | std::string robot_description {}; 58 | if (!controller_nh.getParam(robot_description_parameter, robot_description)) { 59 | ROS_ERROR_STREAM("Failed getting robot description from '" << robot_description_parameter << "'!"); 60 | return false; 61 | } 62 | auto robot_model = std::make_unique(); 63 | pinocchio::urdf::buildModelFromXML(robot_description, *robot_model.get()); 64 | if (!robot_model) { 65 | ROS_ERROR_STREAM("Failed to load Pinocchio model from robot description '" << robot_description << "'!"); 66 | return false; 67 | } 68 | std::string end_effector_link {}; 69 | if (!controller_nh.getParam("end_effector_link", end_effector_link)) { 70 | ROS_ERROR_STREAM("Failed to get robot end effector link!"); 71 | return false; 72 | } 73 | bool apply_gravity; 74 | if (!controller_nh.getParam("apply_gravity_compensation", apply_gravity)) { 75 | ROS_ERROR_STREAM("Failed to get apply gravity compensation parameter!"); 76 | return false; 77 | } 78 | 79 | num_of_dof_ = joint_handles_ptr_->size(); 80 | desired_state_ = RobotState(num_of_dof_); 81 | current_state_ = RobotState(num_of_dof_); 82 | command_effort_.resize(num_of_dof_); 83 | 84 | // TODO: This should be templated so that we can switch between the joint and task space implementations easily 85 | compliant_controller_ = std::make_unique(std::move(robot_model), end_effector_link, num_of_dof_, apply_gravity); 86 | 87 | using namespace boost::placeholders; 88 | dynamic_reconfigure_callback_ = boost::bind(&CompliantHardwareInterfaceAdapter::dynamicReconfigureCallback, this, _1, _2); 89 | dynamic_reconfigure_server_.setCallback(dynamic_reconfigure_callback_); 90 | return true; 91 | } 92 | 93 | template 94 | void CompliantHardwareInterfaceAdapter::starting( 95 | ros::Time const& /*time*/) { 96 | if (!joint_handles_ptr_) { 97 | ROS_ERROR_STREAM("Joint handles not initialized!"); 98 | return; 99 | } 100 | execute_default_command_ = true; 101 | bool is_success {compliant_controller_->init()}; 102 | return; 103 | } 104 | 105 | template 106 | void CompliantHardwareInterfaceAdapter::stopping( 107 | ros::Time const& /*time*/) { 108 | return; 109 | } 110 | 111 | template 112 | void CompliantHardwareInterfaceAdapter::updateCommand( 113 | ros::Time const& /*time*/, ros::Duration const& period, State const& desired_state, 114 | State const& state_error) { 115 | if (!joint_handles_ptr_) { 116 | ROS_ERROR_STREAM("Joint handles not initialized!"); 117 | return; 118 | } 119 | 120 | for (std::size_t i = 0; i < joint_handles_ptr_->size(); ++i) { 121 | auto& joint_handle {(*joint_handles_ptr_)[i]}; 122 | current_state_.positions(i) = joint_handle.getPosition(); 123 | if (current_state_.positions(i) < 0.0) { 124 | current_state_.positions(i) += 2.0*M_PI; 125 | } 126 | current_state_.velocities(i) = joint_handle.getVelocity(); 127 | current_state_.efforts(i) = joint_handle.getEffort(); 128 | } 129 | // TODO: Implement a compile-time type check for the State template parameter 130 | if (!execute_default_command_) { 131 | for (std::size_t i = 0; i < desired_state.position.size(); ++i) { 132 | desired_state_.positions(i) = desired_state.position[i]; 133 | desired_state_.velocities(i) = desired_state.velocity[i]; 134 | desired_state_.accelerations(i) = desired_state.acceleration[i]; 135 | } 136 | } else { 137 | desired_state_.positions = current_state_.positions; 138 | desired_state_.velocities.setZero(); 139 | execute_default_command_ = false; 140 | } 141 | 142 | // TODO: Perform checks that the dimensions are correct! 143 | command_effort_ = compliant_controller_->computeEffort(desired_state_, current_state_, period); 144 | 145 | for (Eigen::Index i = 0; i < command_effort_.size(); ++i) { 146 | (*joint_handles_ptr_)[i].setCommand(command_effort_(i)); 147 | } 148 | return; 149 | } 150 | 151 | template 152 | void CompliantHardwareInterfaceAdapter::dynamicReconfigureCallback(JointSpaceCompliantControllerConfig const& config, 154 | [[maybe_unused]] uint32_t const level) { 155 | Eigen::MatrixXd joint_stiffness_matrix {Eigen::MatrixXd::Zero(7,7)}; 156 | joint_stiffness_matrix.diagonal() << config.j_0, config.j_1, config.j_2, 157 | config.j_3, config.j_4, config.j_5, config.j_6; 158 | [[maybe_unused]] bool is_success {compliant_controller_->setJointStiffnessMatrix( 159 | joint_stiffness_matrix.block(0, 0, num_of_dof_, num_of_dof_) 160 | )}; 161 | 162 | Eigen::MatrixXd rotor_inertia_matrix {Eigen::MatrixXd::Zero(7,7)}; 163 | rotor_inertia_matrix.diagonal() << config.b_0, config.b_1, config.b_2, 164 | config.b_3, config.b_4, config.b_5, config.b_6; 165 | is_success = compliant_controller_->setRotorInertiaMatrix( 166 | rotor_inertia_matrix.block(0, 0, num_of_dof_, num_of_dof_) 167 | ); 168 | 169 | Eigen::MatrixXd friction_l {Eigen::MatrixXd::Zero(7,7)}; 170 | friction_l.diagonal() << config.l_0, config.l_1, config.l_2, 171 | config.l_3, config.l_4, config.l_5, config.l_6; 172 | is_success = compliant_controller_->setFrictionL( 173 | friction_l.block(0, 0, num_of_dof_, num_of_dof_) 174 | ); 175 | 176 | Eigen::MatrixXd friction_lp {Eigen::MatrixXd::Zero(7,7)}; 177 | friction_lp.diagonal() << config.lp_0, config.lp_1, config.lp_2, 178 | config.lp_3, config.lp_4, config.lp_5, config.lp_6; 179 | is_success = compliant_controller_->setFrictionLp( 180 | friction_lp.block(0, 0, num_of_dof_, num_of_dof_) 181 | ); 182 | 183 | Eigen::MatrixXd friction_li {Eigen::MatrixXd::Zero(7,7)}; 184 | friction_li.diagonal() << config.li_0, config.li_1, config.li_2, 185 | config.li_3, config.li_4, config.li_5, config.li_6; 186 | is_success = compliant_controller_->setFrictionLi( 187 | friction_li.block(0, 0, num_of_dof_, num_of_dof_) 188 | ); 189 | 190 | Eigen::VectorXd friction_e_max {Eigen::VectorXd::Zero(7)}; 191 | friction_e_max << config.e_max_0, config.e_max_1, config.e_max_2, 192 | config.e_max_3, config.e_max_4, config.e_max_5, config.e_max_6; 193 | is_success = compliant_controller_->setMaxJointError( 194 | friction_e_max.head(num_of_dof_) 195 | ); 196 | 197 | Eigen::MatrixXd joint_k_matrix {Eigen::MatrixXd::Zero(7,7)}; 198 | joint_k_matrix.diagonal() << config.k_0, config.k_1, config.k_2, 199 | config.k_3, config.k_4, config.k_5, config.k_6; 200 | is_success = compliant_controller_->setJointKMatrix( 201 | joint_k_matrix.block(0, 0, num_of_dof_, num_of_dof_) 202 | ); 203 | 204 | Eigen::MatrixXd joint_d_matrix {Eigen::MatrixXd::Zero(7,7)}; 205 | joint_d_matrix.diagonal() << config.d_0, config.d_1, config.d_2, 206 | config.d_3, config.d_4, config.d_5, config.d_6; 207 | is_success = compliant_controller_->setJointDMatrix( 208 | joint_d_matrix.block(0, 0, num_of_dof_, num_of_dof_) 209 | ); 210 | return; 211 | } 212 | } 213 | } 214 | 215 | #endif // COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_IMPL 216 | -------------------------------------------------------------------------------- /include/compliant_controllers/task_space/hardware_interface_adapter_impl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file hardware_interface_adapter_impl.h 3 | * \mainpage 4 | * Implementation of the compliant hardware interface adapter 5 | * 6 | * \authors 7 | * Alexander Mitchell 8 | * Tobit Flatscher 9 | * \copyright 10 | * Oxford Robotics Institute - University of Oxford (2024) 11 | * \license 12 | * This project is released under the 3-clause BSD license. 13 | */ 14 | 15 | #ifndef COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_IMPL 16 | #define COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_IMPL 17 | #pragma once 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | // Pinocchio has to be included before ROS 25 | // See https://github.com/wxmerkt/pinocchio_ros_example 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "compliant_controllers/TaskSpaceCompliantControllerConfig.h" 36 | #include "compliant_controllers/task_space/controller.h" 37 | #include "compliant_controllers/robot_state.h" 38 | 39 | 40 | namespace compliant_controllers { 41 | namespace task_space { 42 | 43 | template 44 | CompliantHardwareInterfaceAdapter::CompliantHardwareInterfaceAdapter() 45 | : joint_handles_ptr_{nullptr} { 46 | return; 47 | } 48 | 49 | template 50 | bool CompliantHardwareInterfaceAdapter::init( 51 | std::vector& joint_handles, ros::NodeHandle& controller_nh) { 52 | joint_handles_ptr_ = &joint_handles; 53 | 54 | std::string robot_description_parameter {"/robot_description"}; 55 | if (!controller_nh.getParam("robot_description_parameter", robot_description_parameter)) { 56 | ROS_WARN_STREAM("Failed getting robot description parameter, defaulting to '" << robot_description_parameter << "'!"); 57 | } 58 | std::string robot_description {}; 59 | if (!controller_nh.getParam(robot_description_parameter, robot_description)) { 60 | ROS_ERROR_STREAM("Failed getting robot description from '" << robot_description_parameter << "'!"); 61 | return false; 62 | } 63 | auto robot_model = std::make_unique(); 64 | pinocchio::urdf::buildModelFromXML(robot_description, *robot_model.get()); 65 | if (!robot_model) { 66 | ROS_ERROR_STREAM("Failed to load Pinocchio model from robot description '" << robot_description << "'!"); 67 | return false; 68 | } 69 | std::string end_effector_link {}; 70 | if (!controller_nh.getParam("end_effector_link", end_effector_link)) { 71 | ROS_ERROR_STREAM("Failed to get robot end effector link!"); 72 | return false; 73 | } 74 | bool apply_gravity; 75 | if (!controller_nh.getParam("apply_gravity_compensation", apply_gravity)) { 76 | ROS_ERROR_STREAM("Failed to get apply gravity compensation parameter!"); 77 | return false; 78 | } 79 | 80 | num_of_dof_ = joint_handles_ptr_->size(); 81 | desired_state_ = RobotState(num_of_dof_); 82 | current_state_ = RobotState(num_of_dof_); 83 | command_effort_.resize(num_of_dof_); 84 | 85 | // TODO: This should be templated so that we can switch between the joint and task space implementations easily 86 | compliant_controller_ = std::make_unique(std::move(robot_model), end_effector_link, num_of_dof_, apply_gravity); 87 | 88 | using namespace boost::placeholders; 89 | dynamic_reconfigure_callback_ = boost::bind(&CompliantHardwareInterfaceAdapter::dynamicReconfigureCallback, this, _1, _2); 90 | dynamic_reconfigure_server_.setCallback(dynamic_reconfigure_callback_); 91 | return true; 92 | } 93 | 94 | template 95 | void CompliantHardwareInterfaceAdapter::starting( 96 | ros::Time const& /*time*/) { 97 | if (!joint_handles_ptr_) { 98 | ROS_ERROR_STREAM("Joint handles not initialized!"); 99 | return; 100 | } 101 | execute_default_command_ = true; 102 | bool is_success {compliant_controller_->init()}; 103 | return; 104 | } 105 | 106 | template 107 | void CompliantHardwareInterfaceAdapter::stopping( 108 | ros::Time const& /*time*/) { 109 | return; 110 | } 111 | 112 | template 113 | void CompliantHardwareInterfaceAdapter::updateCommand( 114 | ros::Time const& /*time*/, ros::Duration const& period, State const& desired_state, 115 | State const& state_error) { 116 | if (!joint_handles_ptr_) { 117 | ROS_ERROR_STREAM("Joint handles not initialized!"); 118 | return; 119 | } 120 | 121 | for (std::size_t i = 0; i < joint_handles_ptr_->size(); ++i) { 122 | auto& joint_handle {(*joint_handles_ptr_)[i]}; 123 | current_state_.positions(i) = joint_handle.getPosition(); 124 | if (current_state_.positions(i) < 0.0) { 125 | current_state_.positions(i) += 2.0*M_PI; 126 | } 127 | current_state_.velocities(i) = joint_handle.getVelocity(); 128 | current_state_.efforts(i) = joint_handle.getEffort(); 129 | } 130 | // TODO: Implement a compile-time type check for the State template parameter 131 | if (!execute_default_command_) { 132 | for (std::size_t i = 0; i < desired_state.position.size(); ++i) { 133 | desired_state_.positions(i) = desired_state.position[i]; 134 | desired_state_.velocities(i) = desired_state.velocity[i]; 135 | desired_state_.accelerations(i) = desired_state.acceleration[i]; 136 | } 137 | } else { 138 | desired_state_.positions = current_state_.positions; 139 | desired_state_.velocities.setZero(); 140 | execute_default_command_ = false; 141 | } 142 | 143 | // TODO: Perform checks that the dimensions are correct! 144 | command_effort_ = compliant_controller_->computeEffort(desired_state_, current_state_, period); 145 | 146 | for (Eigen::Index i = 0; i < command_effort_.size(); ++i) { 147 | (*joint_handles_ptr_)[i].setCommand(command_effort_(i)); 148 | } 149 | return; 150 | } 151 | 152 | template 153 | void CompliantHardwareInterfaceAdapter::dynamicReconfigureCallback(TaskSpaceCompliantControllerConfig const& config, 155 | [[maybe_unused]] uint32_t const level) { 156 | Eigen::MatrixXd joint_stiffness_matrix {Eigen::MatrixXd::Zero(7,7)}; 157 | joint_stiffness_matrix.diagonal() << config.j_0, config.j_1, config.j_2, 158 | config.j_3, config.j_4, config.j_5, config.j_6; 159 | [[maybe_unused]] bool is_success {compliant_controller_->setJointStiffnessMatrix( 160 | joint_stiffness_matrix.block(0, 0, num_of_dof_, num_of_dof_) 161 | )}; 162 | 163 | Eigen::MatrixXd rotor_inertia_matrix {Eigen::MatrixXd::Zero(7,7)}; 164 | rotor_inertia_matrix.diagonal() << config.b_0, config.b_1, config.b_2, 165 | config.b_3, config.b_4, config.b_5, config.b_6; 166 | is_success = compliant_controller_->setRotorInertiaMatrix( 167 | rotor_inertia_matrix.block(0, 0, num_of_dof_, num_of_dof_) 168 | ); 169 | 170 | Eigen::MatrixXd friction_l {Eigen::MatrixXd::Zero(7,7)}; 171 | friction_l.diagonal() << config.l_0, config.l_1, config.l_2, 172 | config.l_3, config.l_4, config.l_5, config.l_6; 173 | is_success = compliant_controller_->setFrictionL( 174 | friction_l.block(0, 0, num_of_dof_, num_of_dof_) 175 | ); 176 | 177 | Eigen::MatrixXd friction_lp {Eigen::MatrixXd::Zero(7,7)}; 178 | friction_lp.diagonal() << config.lp_0, config.lp_1, config.lp_2, 179 | config.lp_3, config.lp_4, config.lp_5, config.lp_6; 180 | is_success = compliant_controller_->setFrictionLp( 181 | friction_lp.block(0, 0, num_of_dof_, num_of_dof_) 182 | ); 183 | 184 | Eigen::MatrixXd friction_li {Eigen::MatrixXd::Zero(7,7)}; 185 | friction_li.diagonal() << config.li_0, config.li_1, config.li_2, 186 | config.li_3, config.li_4, config.li_5, config.li_6; 187 | is_success = compliant_controller_->setFrictionLi( 188 | friction_li.block(0, 0, num_of_dof_, num_of_dof_) 189 | ); 190 | 191 | Eigen::VectorXd friction_e_max {Eigen::VectorXd::Zero(7)}; 192 | friction_e_max << config.e_max_0, config.e_max_1, config.e_max_2, 193 | config.e_max_3, config.e_max_4, config.e_max_5, config.e_max_6; 194 | is_success = compliant_controller_->setMaxJointError( 195 | friction_e_max.head(num_of_dof_) 196 | ); 197 | 198 | Eigen::MatrixXd task_k_matrix {Eigen::MatrixXd::Zero(6,6)}; 199 | task_k_matrix.diagonal() << config.task_k_0, config.task_k_1, config.task_k_2, 200 | config.task_k_3, config.task_k_4, config.task_k_5; 201 | is_success = compliant_controller_->setTaskKMatrix( 202 | task_k_matrix.block(0, 0, 6, 6) 203 | ); 204 | 205 | Eigen::MatrixXd task_d_matrix {Eigen::MatrixXd::Zero(6,6)}; 206 | task_d_matrix.diagonal() << config.task_d_0, config.task_d_1, config.task_d_2, 207 | config.task_d_3, config.task_d_4, config.task_d_5; 208 | is_success = compliant_controller_->setTaskDMatrix( 209 | task_d_matrix.block(0, 0, 6, 6) 210 | ); 211 | 212 | Eigen::MatrixXd joint_d_matrix {Eigen::MatrixXd::Zero(7,7)}; 213 | joint_d_matrix.diagonal() << config.joint_d_0, config.joint_d_1, config.joint_d_2, 214 | config.joint_d_3, config.joint_d_4, config.joint_d_5, config.joint_d_6; 215 | is_success = compliant_controller_->setJointDMatrix( 216 | joint_d_matrix.block(0, 0, num_of_dof_, num_of_dof_) 217 | ); 218 | return; 219 | } 220 | 221 | } 222 | } 223 | 224 | #endif // COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_IMPL 225 | -------------------------------------------------------------------------------- /include/compliant_controllers/joint_space/controller.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file joint_space_compliant_controller.h 3 | * \mainpage 4 | * Compliant controller in joint space 5 | * 6 | * \authors 7 | * Tobit Flatscher 8 | * Rishabh Madan 9 | * Rajat Kumar Jenamani 10 | * \copyright 11 | * Oxford Robotics Institute - University of Oxford (2024) 12 | * EmPRISE Lab - Cornell University (2023) 13 | * \license 14 | * This project is released under the 3-clause BSD license. 15 | */ 16 | 17 | #ifndef COMPLIANT_CONTROLLERS__JOINT_SPACE_COMPLIANT_CONTROLLER 18 | #define COMPLIANT_CONTROLLERS__JOINT_SPACE_COMPLIANT_CONTROLLER 19 | #pragma once 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | // Pinocchio has to be included before ROS 26 | // See https://github.com/wxmerkt/pinocchio_ros_example 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | #include "compliant_controllers/extended_joint_positions.h" 34 | #include "compliant_controllers/robot_state.h" 35 | 36 | 37 | namespace compliant_controllers { 38 | namespace joint_space { 39 | 40 | /**\class JointSpaceCompliantController 41 | * \brief 42 | * Controller that is compliant in joint space 43 | */ 44 | class CompliantController { 45 | public: 46 | /**\fn JointSpaceCompliantController 47 | * \brief 48 | * Constructor that allocates all necessary structures 49 | * 50 | * \param[in] robot_model 51 | * The robot model 52 | * \param[in] end_effector_link 53 | * The end-effector link of the kinematic chain 54 | * \param[in] num_controlled_dofs 55 | * The number of degree of freedoms that the controller should control 56 | * \param[in] apply_gravity 57 | * Toggle whether to add gravity compensatin to efforts_ during computeEffort 58 | */ 59 | CompliantController(std::unique_ptr robot_model, 60 | std::string const& end_effector_link, 61 | int const num_controlled_dofs, 62 | bool apply_gravity); 63 | CompliantController() = delete; 64 | CompliantController(CompliantController const&) = default; 65 | CompliantController& operator= (CompliantController const&) = default; 66 | CompliantController(CompliantController&&) = default; 67 | CompliantController& operator= (CompliantController&&) = default; 68 | 69 | /**\fn setDefaultValues 70 | * \brief 71 | * Set default values to all gains and friction parameters 72 | */ 73 | void setDefaultValues(); 74 | 75 | /**\fn setJointStiffnessMatrix 76 | * \brief 77 | * Set the joint stiffness matrix Ks 78 | * 79 | * \param[in] joint_stiffness_matrix 80 | * The joint stiffness matrix to be set 81 | * \return 82 | * Boolean variable signalling success or failure 83 | */ 84 | [[nodiscard]] 85 | bool setJointStiffnessMatrix(Eigen::MatrixXd const& joint_stiffness_matrix); 86 | 87 | /**\fn setRotorInertiaMatrix 88 | * \brief 89 | * Set the rotor inertia matrix Kr 90 | * 91 | * \param[in] rotor_inertia_matrix 92 | * The rotor inertia matrix to be set 93 | * \return 94 | * Boolean variable signalling success or failure 95 | */ 96 | [[nodiscard]] 97 | bool setRotorInertiaMatrix(Eigen::MatrixXd const& rotor_inertia_matrix); 98 | 99 | /**\fn setFrictionL 100 | * \brief 101 | * Set the friction observer matrix 1 102 | * 103 | * \param[in] friction_l 104 | * The friction observer matrix 1 to be set 105 | * \return 106 | * Boolean variable signalling success or failure 107 | */ 108 | [[nodiscard]] 109 | bool setFrictionL(Eigen::MatrixXd const& friction_l); 110 | 111 | /**\fn setFrictionLp 112 | * \brief 113 | * Set the friction observer matrix 2 114 | * 115 | * \param[in] friction_lp 116 | * The friction observer matrix 2 to be set 117 | * \return 118 | * Boolean variable signalling success or failure 119 | */ 120 | [[nodiscard]] 121 | bool setFrictionLp(Eigen::MatrixXd const& friction_lp); 122 | 123 | /**\fn setFrictionLi 124 | * \brief 125 | * Set the friction observer matrix integral gain 126 | * 127 | * \param[in] friction_li 128 | * The friction observer matrix integral gain 129 | * \return 130 | * Boolean variable signalling success or failure 131 | */ 132 | [[nodiscard]] 133 | bool setFrictionLi(Eigen::MatrixXd const& friction_li); 134 | 135 | /**\fn setJointKMatrix 136 | * \brief 137 | * Set the joint compliance proportional gain matrix 138 | * 139 | * \param[in] joint_k_matrix 140 | * The joint compliance proportional gain matrix to be set 141 | * \return 142 | * Boolean variable signalling success or failure 143 | */ 144 | [[nodiscard]] 145 | bool setJointKMatrix(Eigen::MatrixXd const& joint_k_matrix); 146 | 147 | /**\fn setJointDMatrix 148 | * \brief 149 | * Set the joint compliance derivative gain matrix 150 | * 151 | * \param[in] joint_d_matrix 152 | * The joint compliance derivative gain matrix to be set 153 | * \return 154 | * Boolean variable signalling success or failure 155 | */ 156 | [[nodiscard]] 157 | bool setJointDMatrix(Eigen::MatrixXd const& joint_d_matrix); 158 | 159 | /**\fn setMaxJointError 160 | * \brief 161 | * Set the joint integrator max error 162 | * 163 | * \param[in] joint_error_max 164 | * The joint compliance derivative gain matrix to be set 165 | * \return 166 | * Boolean variable signalling success or failure 167 | */ 168 | [[nodiscard]] 169 | bool setMaxJointError(Eigen::VectorXd const& joint_error_max); 170 | 171 | /**\fn init 172 | * \brief 173 | * Initialize and Reset the controller 174 | * 175 | * \return 176 | * Boolean variable signalling success or failure 177 | */ 178 | [[nodiscard]] 179 | bool init(); 180 | 181 | /**\fn computeEffort 182 | * \brief 183 | * Compute the required effort for bringing the robot from state \p current_state to state \p desired_state 184 | * 185 | * \param[in] desired_state 186 | * The state that the robot should be set to 187 | * \param[in] current_state 188 | * The state that the robot is currently in 189 | * \param[in] period 190 | * The period of the controller 191 | * \return 192 | * The effort that should be applied to the individual joints to move to the desired state 193 | */ 194 | [[nodiscard]] 195 | Eigen::VectorXd computeEffort(RobotState const& desired_state, RobotState const& current_state, ros::Duration const& period); 196 | 197 | /**\fn integrate_error 198 | * \brief 199 | * Sum error between \p current_q and \p desired_q clamp using q_error_max_ 200 | * 201 | * \param[in] desired_q 202 | * The state that the robot is currently in 203 | * \param[in] current_q 204 | * The state that the robot should be set to 205 | * \return 206 | * The sum of the errors 207 | */ 208 | [[nodiscard]] 209 | Eigen::VectorXd integrate_error(Eigen::VectorXd const& desired_q, 210 | Eigen::VectorXd const& current_q); 211 | 212 | 213 | protected: 214 | /**\fn constructDiagonalMatrix 215 | * \brief 216 | * Construct a diagonal matrix of dimension \p dim with \p value in its diagonal 217 | * 218 | * \param[in] value 219 | * The value to be set to the diagonal 220 | * \param[in] dim 221 | * The dimension of the matrix 222 | * \return 223 | * The matrix of dimension \p dim and \p value in its diagonal 224 | */ 225 | [[nodiscard]] 226 | static Eigen::MatrixXd constructDiagonalMatrix(double const value, int const dim); 227 | 228 | /**\fn checkMatrix 229 | * \brief 230 | * Check if the dimensions of the matrix match with the expected dimensions 231 | * 232 | * \param[in] matrix 233 | * The matrix to be checked 234 | * \return 235 | * Boolean value signalling same (true) or different dimensions (false) 236 | */ 237 | [[nodiscard]] 238 | bool checkMatrix(Eigen::MatrixXd const& matrix) const noexcept; 239 | 240 | bool apply_gravity_; 241 | std::unique_ptr robot_model_; 242 | std::string end_effector_link_; 243 | std::unique_ptr data_; 244 | int num_controlled_dofs_; 245 | pinocchio::Model::Index end_effector_index_; 246 | Eigen::MatrixXd inverse_joint_stiffness_matrix_; 247 | Eigen::MatrixXd rotor_inertia_matrix_; 248 | Eigen::MatrixXd inverse_rotor_inertia_matrix_; 249 | Eigen::MatrixXd friction_l_; 250 | Eigen::MatrixXd friction_lp_; 251 | Eigen::MatrixXd friction_li_; // Integral friction observer gain 252 | Eigen::MatrixXd joint_k_matrix_; 253 | Eigen::MatrixXd joint_d_matrix_; 254 | // Integrator quantities 255 | Eigen::VectorXd q_error_; 256 | Eigen::VectorXd q_error_sum_; 257 | Eigen::VectorXd q_error_max_; 258 | std::unique_ptr extended_joints_; 259 | int count_; 260 | 261 | Eigen::VectorXd last_desired_positions_; 262 | Eigen::VectorXd nominal_theta_prev_; 263 | Eigen::VectorXd nominal_theta_dot_prev_; 264 | Eigen::VectorXd desired_positions_; 265 | Eigen::VectorXd current_theta_; 266 | Eigen::VectorXd gravity_; 267 | Eigen::VectorXd task_effort_; 268 | Eigen::VectorXd nominal_theta_d_dot_; 269 | Eigen::VectorXd nominal_theta_dot_; 270 | Eigen::VectorXd nominal_theta_; 271 | Eigen::VectorXd nominal_friction_; 272 | Eigen::VectorXd efforts_; 273 | }; 274 | } // namespace compliant_controllers 275 | } // joint_space 276 | 277 | #endif // COMPLIANT_CONTROLLERS__JOINT_SPACE_COMPLIANT_CONTROLLER 278 | -------------------------------------------------------------------------------- /include/compliant_controllers/joint_task_space/hardware_interface_adapter_impl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file hardware_interface_adapter_impl.h 3 | * \mainpage 4 | * Implementation of the compliant hardware interface adapter 5 | * 6 | * \authors 7 | * Alexander Mitchell 8 | * Tobit Flatscher 9 | * \copyright 10 | * Oxford Robotics Institute - University of Oxford (2024) 11 | * \license 12 | * This project is released under the 3-clause BSD license. 13 | */ 14 | 15 | #ifndef COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_IMPL 16 | #define COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_IMPL 17 | #pragma once 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | // Pinocchio has to be included before ROS 25 | // See https://github.com/wxmerkt/pinocchio_ros_example 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "compliant_controllers/JointTaskSpaceCompliantControllerConfig.h" 36 | #include "compliant_controllers/joint_task_space/controller.h" 37 | #include "compliant_controllers/robot_state.h" 38 | 39 | 40 | namespace compliant_controllers { 41 | namespace joint_task_space { 42 | 43 | template 44 | CompliantHardwareInterfaceAdapter::CompliantHardwareInterfaceAdapter() 45 | : joint_handles_ptr_{nullptr} { 46 | return; 47 | } 48 | 49 | template 50 | bool CompliantHardwareInterfaceAdapter::init( 51 | std::vector& joint_handles, ros::NodeHandle& controller_nh) { 52 | joint_handles_ptr_ = &joint_handles; 53 | 54 | std::string robot_description_parameter {"/robot_description"}; 55 | if (!controller_nh.getParam("robot_description_parameter", robot_description_parameter)) { 56 | ROS_WARN_STREAM("Failed getting robot description parameter, defaulting to '" << robot_description_parameter << "'!"); 57 | } 58 | std::string robot_description {}; 59 | if (!controller_nh.getParam(robot_description_parameter, robot_description)) { 60 | ROS_ERROR_STREAM("Failed getting robot description from '" << robot_description_parameter << "'!"); 61 | return false; 62 | } 63 | auto robot_model = std::make_unique(); 64 | pinocchio::urdf::buildModelFromXML(robot_description, *robot_model.get()); 65 | if (!robot_model) { 66 | ROS_ERROR_STREAM("Failed to load Pinocchio model from robot description '" << robot_description << "'!"); 67 | return false; 68 | } 69 | std::string end_effector_link {}; 70 | if (!controller_nh.getParam("end_effector_link", end_effector_link)) { 71 | ROS_ERROR_STREAM("Failed to get robot end effector link!"); 72 | return false; 73 | } 74 | bool apply_gravity; 75 | if (!controller_nh.getParam("apply_gravity_compensation", apply_gravity)) { 76 | ROS_ERROR_STREAM("Failed to get apply gravity compensation parameter!"); 77 | return false; 78 | } 79 | 80 | num_of_dof_ = joint_handles_ptr_->size(); 81 | desired_state_ = RobotState(num_of_dof_); 82 | current_state_ = RobotState(num_of_dof_); 83 | command_effort_.resize(num_of_dof_); 84 | 85 | // TODO: This should be templated so that we can switch between the joint and task space implementations easily 86 | compliant_controller_ = std::make_unique(std::move(robot_model), end_effector_link, num_of_dof_, apply_gravity); 87 | 88 | using namespace boost::placeholders; 89 | dynamic_reconfigure_callback_ = boost::bind(&CompliantHardwareInterfaceAdapter::dynamicReconfigureCallback, this, _1, _2); 90 | dynamic_reconfigure_server_.setCallback(dynamic_reconfigure_callback_); 91 | return true; 92 | } 93 | 94 | template 95 | void CompliantHardwareInterfaceAdapter::starting( 96 | ros::Time const& /*time*/) { 97 | if (!joint_handles_ptr_) { 98 | ROS_ERROR_STREAM("Joint handles not initialized!"); 99 | return; 100 | } 101 | execute_default_command_ = true; 102 | bool is_success {compliant_controller_->init()}; 103 | return; 104 | } 105 | 106 | template 107 | void CompliantHardwareInterfaceAdapter::stopping( 108 | ros::Time const& /*time*/) { 109 | return; 110 | } 111 | 112 | template 113 | void CompliantHardwareInterfaceAdapter::updateCommand( 114 | ros::Time const& /*time*/, ros::Duration const& period, State const& desired_state, 115 | State const& state_error) { 116 | if (!joint_handles_ptr_) { 117 | ROS_ERROR_STREAM("Joint handles not initialized!"); 118 | return; 119 | } 120 | 121 | for (std::size_t i = 0; i < joint_handles_ptr_->size(); ++i) { 122 | auto& joint_handle {(*joint_handles_ptr_)[i]}; 123 | current_state_.positions(i) = joint_handle.getPosition(); 124 | if (current_state_.positions(i) < 0.0) { 125 | current_state_.positions(i) += 2.0*M_PI; 126 | } 127 | current_state_.velocities(i) = joint_handle.getVelocity(); 128 | current_state_.efforts(i) = joint_handle.getEffort(); 129 | } 130 | // TODO: Implement a compile-time type check for the State template parameter 131 | if (!execute_default_command_) { 132 | for (std::size_t i = 0; i < desired_state.position.size(); ++i) { 133 | desired_state_.positions(i) = desired_state.position[i]; 134 | desired_state_.velocities(i) = desired_state.velocity[i]; 135 | desired_state_.accelerations(i) = desired_state.acceleration[i]; 136 | } 137 | } else { 138 | desired_state_.positions = current_state_.positions; 139 | desired_state_.velocities.setZero(); 140 | execute_default_command_ = false; 141 | } 142 | 143 | // TODO: Perform checks that the dimensions are correct! 144 | command_effort_ = compliant_controller_->computeEffort(desired_state_, current_state_, period); 145 | 146 | for (Eigen::Index i = 0; i < command_effort_.size(); ++i) { 147 | (*joint_handles_ptr_)[i].setCommand(command_effort_(i)); 148 | } 149 | return; 150 | } 151 | 152 | template 153 | void CompliantHardwareInterfaceAdapter::dynamicReconfigureCallback(JointTaskSpaceCompliantControllerConfig const& config, 155 | [[maybe_unused]] uint32_t const level) { 156 | Eigen::MatrixXd joint_stiffness_matrix {Eigen::MatrixXd::Zero(7,7)}; 157 | joint_stiffness_matrix.diagonal() << config.j_0, config.j_1, config.j_2, 158 | config.j_3, config.j_4, config.j_5, config.j_6; 159 | [[maybe_unused]] bool is_success {compliant_controller_->setJointStiffnessMatrix( 160 | joint_stiffness_matrix.block(0, 0, num_of_dof_, num_of_dof_) 161 | )}; 162 | 163 | Eigen::MatrixXd rotor_inertia_matrix {Eigen::MatrixXd::Zero(7,7)}; 164 | rotor_inertia_matrix.diagonal() << config.b_0, config.b_1, config.b_2, 165 | config.b_3, config.b_4, config.b_5, config.b_6; 166 | is_success = compliant_controller_->setRotorInertiaMatrix( 167 | rotor_inertia_matrix.block(0, 0, num_of_dof_, num_of_dof_) 168 | ); 169 | 170 | Eigen::MatrixXd friction_l {Eigen::MatrixXd::Zero(7,7)}; 171 | friction_l.diagonal() << config.l_0, config.l_1, config.l_2, 172 | config.l_3, config.l_4, config.l_5, config.l_6; 173 | is_success = compliant_controller_->setFrictionL( 174 | friction_l.block(0, 0, num_of_dof_, num_of_dof_) 175 | ); 176 | 177 | Eigen::MatrixXd friction_lp {Eigen::MatrixXd::Zero(7,7)}; 178 | friction_lp.diagonal() << config.lp_0, config.lp_1, config.lp_2, 179 | config.lp_3, config.lp_4, config.lp_5, config.lp_6; 180 | is_success = compliant_controller_->setFrictionLp( 181 | friction_lp.block(0, 0, num_of_dof_, num_of_dof_) 182 | ); 183 | 184 | Eigen::MatrixXd friction_li {Eigen::MatrixXd::Zero(7,7)}; 185 | friction_li.diagonal() << config.li_0, config.li_1, config.li_2, 186 | config.li_3, config.li_4, config.li_5, config.li_6; 187 | is_success = compliant_controller_->setFrictionLi( 188 | friction_li.block(0, 0, num_of_dof_, num_of_dof_) 189 | ); 190 | 191 | Eigen::VectorXd friction_e_max {Eigen::VectorXd::Zero(7)}; 192 | friction_e_max << config.e_max_0, config.e_max_1, config.e_max_2, 193 | config.e_max_3, config.e_max_4, config.e_max_5, config.e_max_6; 194 | is_success = compliant_controller_->setMaxJointError( 195 | friction_e_max.head(num_of_dof_) 196 | ); 197 | 198 | Eigen::MatrixXd task_k_matrix {Eigen::MatrixXd::Zero(6,6)}; 199 | task_k_matrix.diagonal() << config.task_k_0, config.task_k_1, config.task_k_2, 200 | config.task_k_3, config.task_k_4, config.task_k_5; 201 | is_success = compliant_controller_->setTaskKMatrix( 202 | task_k_matrix.block(0, 0, 6, 6) 203 | ); 204 | 205 | Eigen::MatrixXd task_d_matrix {Eigen::MatrixXd::Zero(6,6)}; 206 | task_d_matrix.diagonal() << config.task_d_0, config.task_d_1, config.task_d_2, 207 | config.task_d_3, config.task_d_4, config.task_d_5; 208 | is_success = compliant_controller_->setTaskDMatrix( 209 | task_d_matrix.block(0, 0, 6, 6) 210 | ); 211 | 212 | Eigen::MatrixXd joint_k_matrix {Eigen::MatrixXd::Zero(7, 7)}; 213 | joint_k_matrix.diagonal() << config.joint_k_0, config.joint_k_1, config.joint_k_2, 214 | config.joint_k_3, config.joint_k_4, config.joint_k_5, config.joint_k_6; 215 | is_success = compliant_controller_->setJointKMatrix( 216 | joint_k_matrix.block(0, 0, num_of_dof_, num_of_dof_) 217 | ); 218 | 219 | Eigen::MatrixXd joint_d_matrix {Eigen::MatrixXd::Zero(7,7)}; 220 | joint_d_matrix.diagonal() << config.joint_d_0, config.joint_d_1, config.joint_d_2, 221 | config.joint_d_3, config.joint_d_4, config.joint_d_5, config.joint_d_6; 222 | is_success = compliant_controller_->setJointDMatrix( 223 | joint_d_matrix.block(0, 0, num_of_dof_, num_of_dof_) 224 | ); 225 | return; 226 | } 227 | 228 | } 229 | } 230 | 231 | #endif // COMPLIANT_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_IMPL 232 | -------------------------------------------------------------------------------- /test/urdf/oxf20_right_arm.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 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 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 | -------------------------------------------------------------------------------- /include/compliant_controllers/task_space/controller.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file task_space_compliant_controller.h 3 | * \mainpage 4 | * Compliant controller in joint space 5 | * 6 | * \authors 7 | * Tobit Flatscher 8 | * Alexander Mitchell 9 | * Rishabh Madan 10 | * Rajat Kumar Jenamani 11 | * \copyright 12 | * Oxford Robotics Institute - University of Oxford (2024) 13 | * EmPRISE Lab - Cornell University (2023) 14 | * \license 15 | * This project is released under the 3-clause BSD license. 16 | */ 17 | 18 | #ifndef COMPLIANT_CONTROLLERS__TASK_SPACE_COMPLIANT_CONTROLLER 19 | #define COMPLIANT_CONTROLLERS__TASK_SPACE_COMPLIANT_CONTROLLER 20 | #pragma once 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | // Pinocchio has to be included before ROS 27 | // See https://github.com/wxmerkt/pinocchio_ros_example 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | #include "compliant_controllers/extended_joint_positions.h" 35 | #include "compliant_controllers/robot_state.h" 36 | 37 | 38 | namespace compliant_controllers { 39 | namespace task_space { 40 | /**\class CompliantController 41 | * \brief 42 | * Controller that is compliant in joint space 43 | */ 44 | class CompliantController { 45 | public: 46 | /**\fn CompliantController 47 | * \brief 48 | * Constructor that allocates all necessary structures 49 | * 50 | * \param[in] robot_model 51 | * The robot model 52 | * \param[in] end_effector_link 53 | * The end-effector link of the kinematic chain 54 | * \param[in] num_controlled_dofs 55 | * The number of degree of freedoms that the controller should control 56 | * \param[in] apply_gravity 57 | * Toggle for applying gravity compensation to efforts_ in computeEffort 58 | */ 59 | CompliantController(std::unique_ptr robot_model, 60 | std::string const& end_effector_link, 61 | int const num_controlled_dofs, 62 | bool apply_gravity); 63 | CompliantController() = delete; 64 | CompliantController(CompliantController const&) = default; 65 | CompliantController& operator= (CompliantController const&) = default; 66 | CompliantController(CompliantController&&) = default; 67 | CompliantController& operator= (CompliantController&&) = default; 68 | 69 | /**\fn setDefaultValues 70 | * \brief 71 | * Set default values to all gains and friction parameters 72 | */ 73 | void setDefaultValues(); 74 | 75 | /**\fn setJointStiffnessMatrix 76 | * \brief 77 | * Set the joint stiffness matrix Ks 78 | * 79 | * \param[in] joint_stiffness_matrix 80 | * The joint stiffness matrix to be set 81 | * \return 82 | * Boolean variable signalling success or failure 83 | */ 84 | [[nodiscard]] 85 | bool setJointStiffnessMatrix(Eigen::MatrixXd const& joint_stiffness_matrix); 86 | 87 | /**\fn setRotorInertiaMatrix 88 | * \brief 89 | * Set the rotor inertia matrix Kr 90 | * 91 | * \param[in] rotor_inertia_matrix 92 | * The rotor inertia matrix to be set 93 | * \return 94 | * Boolean variable signalling success or failure 95 | */ 96 | [[nodiscard]] 97 | bool setRotorInertiaMatrix(Eigen::MatrixXd const& rotor_inertia_matrix); 98 | 99 | /**\fn setFrictionL 100 | * \brief 101 | * Set the friction observer matrix 1 102 | * 103 | * \param[in] friction_l 104 | * The friction observer matrix 1 to be set 105 | * \return 106 | * Boolean variable signalling success or failure 107 | */ 108 | [[nodiscard]] 109 | bool setFrictionL(Eigen::MatrixXd const& friction_l); 110 | 111 | /**\fn setFrictionLp 112 | * \brief 113 | * Set the friction observer matrix 2 114 | * 115 | * \param[in] friction_lp 116 | * The friction observer matrix 2 to be set 117 | * \return 118 | * Boolean variable signalling success or failure 119 | */ 120 | [[nodiscard]] 121 | bool setFrictionLp(Eigen::MatrixXd const& friction_lp); 122 | 123 | /**\fn setFrictionLi 124 | * \brief 125 | * Set the friction observer matrix integral gain 126 | * 127 | * \param[in] friction_li 128 | * The friction observer matrix integral gain 129 | * \return 130 | * Boolean variable signalling success or failure 131 | */ 132 | [[nodiscard]] 133 | bool setFrictionLi(Eigen::MatrixXd const& friction_li); 134 | 135 | /**\fn setTaskKMatrix 136 | * \brief 137 | * Set the task space compliance proportional gain matrix 138 | * 139 | * \param[in] task_k_matrix 140 | * The task space compliance proportional gain matrix to be set 141 | * \return 142 | * Boolean variable signalling success or failure 143 | */ 144 | [[nodiscard]] 145 | bool setTaskKMatrix(Eigen::MatrixXd const& task_k_matrix); 146 | 147 | /**\fn setTaskdMatrix 148 | * \brief 149 | * Set the task compliance derivative gain matrix 150 | * 151 | * \param[in] joint_d_matrix 152 | * The task space compliance proportional gain matrix to be set 153 | * \return 154 | * Boolean variable signalling success or failure 155 | */ 156 | [[nodiscard]] 157 | bool setTaskDMatrix(Eigen::MatrixXd const& task_d_matrix); 158 | 159 | /**\fn setJointDMatrix 160 | * \brief 161 | * Set the joint compliance derivative gain matrix 162 | * 163 | * \param[in] joint_d_matrix 164 | * The joint compliance derivative gain matrix to be set 165 | * \return 166 | * Boolean variable signalling success or failure 167 | */ 168 | [[nodiscard]] 169 | bool setJointDMatrix(Eigen::MatrixXd const& joint_d_matrix); 170 | 171 | /**\fn setMaxJointError 172 | * \brief 173 | * Set the joint integrator max error 174 | * 175 | * \param[in] joint_error_max 176 | * The joint compliance derivative gain matrix to be set 177 | * \return 178 | * Boolean variable signalling success or failure 179 | */ 180 | [[nodiscard]] 181 | bool setMaxJointError(Eigen::VectorXd const& joint_error_max); 182 | 183 | /**\fn init 184 | * \brief 185 | * Initialize and Reset the controller 186 | * 187 | * \return 188 | * Boolean variable signalling success or failure 189 | */ 190 | [[nodiscard]] 191 | bool init(); 192 | 193 | /**\fn computeEffort 194 | * \brief 195 | * Compute the required effort for bringing the robot from state \p current_state to state \p desired_state 196 | * 197 | * \param[in] desired_state 198 | * The state that the robot should be set to 199 | * \param[in] current_state 200 | * The state that the robot is currently in 201 | * \param[in] period 202 | * The period of the controller 203 | * \return 204 | * The effort that should be applied to the individual joints to move to the desired state 205 | */ 206 | [[nodiscard]] 207 | Eigen::VectorXd computeEffort(RobotState const& desired_state, RobotState const& current_state, ros::Duration const& period); 208 | 209 | /**\fn integrate_error 210 | * \brief 211 | * Sum error between \p current_q and \p desired_q clamp using q_error_max_ 212 | * 213 | * \param[in] desired_q 214 | * The state that the robot is currently in 215 | * \param[in] current_q 216 | * The state that the robot should be set to 217 | * \return 218 | * The sum of the errors 219 | */ 220 | [[nodiscard]] 221 | Eigen::VectorXd integrate_error(Eigen::VectorXd const& desired_q, 222 | Eigen::VectorXd const& current_q); 223 | 224 | void advanceKinematics(Eigen::VectorXd const& q); 225 | 226 | Eigen::Isometry3d getFrameTransform(Eigen::VectorXd const& q, 227 | pinocchio::Model::Index const& frame_idx); 228 | 229 | 230 | protected: 231 | /**\fn constructDiagonalMatrix 232 | * \brief 233 | * Construct a diagonal matrix of dimension \p dim with \p value in its diagonal 234 | * 235 | * \param[in] value 236 | * The value to be set to the diagonal 237 | * \param[in] dim 238 | * The dimension of the matrix 239 | * \return 240 | * The matrix of dimension \p dim and \p value in its diagonal 241 | */ 242 | [[nodiscard]] 243 | static Eigen::MatrixXd constructDiagonalMatrix(double const value, int const dim); 244 | 245 | /**\fn checkMatrix 246 | * \brief 247 | * Check if the dimensions of the matrix match with the expected dimensions 248 | * 249 | * \param[in] matrix 250 | * The matrix to be checked 251 | * \return 252 | * Boolean value signalling same (true) or different dimensions (false) 253 | */ 254 | [[nodiscard]] 255 | bool checkMatrix(Eigen::MatrixXd const& matrix) const noexcept; 256 | 257 | bool apply_gravity_; 258 | std::unique_ptr robot_model_; 259 | std::string end_effector_link_; 260 | std::unique_ptr data_; 261 | int num_controlled_dofs_; 262 | pinocchio::Model::Index end_effector_index_; 263 | Eigen::MatrixXd inverse_joint_stiffness_matrix_; 264 | Eigen::MatrixXd rotor_inertia_matrix_; 265 | Eigen::MatrixXd inverse_rotor_inertia_matrix_; 266 | Eigen::MatrixXd friction_l_; 267 | Eigen::MatrixXd friction_lp_; 268 | Eigen::MatrixXd friction_li_; // Integral friction observer gain 269 | // Joint space d matrix -- adds some joint space damping 270 | Eigen::MatrixXd joint_d_matrix_; 271 | // Task space k matrix 272 | Eigen::MatrixXd task_k_matrix_; 273 | Eigen::MatrixXd task_d_matrix_; 274 | // Integrator quantities 275 | Eigen::VectorXd q_error_; 276 | Eigen::VectorXd q_error_sum_; 277 | Eigen::VectorXd q_error_max_; 278 | std::unique_ptr extended_joints_; 279 | int count_; 280 | 281 | // Task-space deltas 282 | Eigen::Matrix taskspace_error_; 283 | // The jacobian of the complete robot model including gripper 284 | Eigen::MatrixXd jacobian_; 285 | Eigen::Isometry3d nominal_ee_transform_; 286 | Eigen::Quaterniond nominal_ee_quat_; 287 | Eigen::Isometry3d desired_ee_transform_; 288 | Eigen::Quaterniond desired_ee_quat_; 289 | Eigen::Quaterniond error_quat_; 290 | Eigen::Matrix3d tempMat3d_; 291 | Eigen::Isometry3d tempIsometry3d_; 292 | 293 | Eigen::VectorXd taskspace_effort_; 294 | 295 | Eigen::VectorXd last_desired_positions_; 296 | Eigen::VectorXd nominal_theta_prev_; 297 | Eigen::VectorXd nominal_theta_dot_prev_; 298 | Eigen::VectorXd desired_positions_; 299 | Eigen::VectorXd current_theta_; 300 | Eigen::VectorXd gravity_; 301 | Eigen::VectorXd task_effort_; 302 | Eigen::VectorXd nominal_theta_d_dot_; 303 | Eigen::VectorXd nominal_theta_dot_; 304 | Eigen::VectorXd nominal_theta_; 305 | Eigen::VectorXd nominal_friction_; 306 | Eigen::VectorXd efforts_; 307 | }; 308 | } // namespace task_space 309 | } // compliant_controller 310 | 311 | #endif // COMPLIANT_CONTROLLERS__TASK_SPACE_COMPLIANT_CONTROLLER 312 | -------------------------------------------------------------------------------- /include/compliant_controllers/joint_task_space/controller.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file task_space_compliant_controller.h 3 | * \mainpage 4 | * Compliant controller in joint space 5 | * 6 | * \authors 7 | * Tobit Flatscher 8 | * Alexander Mitchell 9 | * Rishabh Madan 10 | * Rajat Kumar Jenamani 11 | * \copyright 12 | * Oxford Robotics Institute - University of Oxford (2024) 13 | * EmPRISE Lab - Cornell University (2023) 14 | * \license 15 | * This project is released under the 3-clause BSD license. 16 | */ 17 | 18 | #ifndef COMPLIANT_CONTROLLERS__JOINT_TASK_SPACE_COMPLIANT_CONTROLLER 19 | #define COMPLIANT_CONTROLLERS__JOINT_TASK_SPACE_COMPLIANT_CONTROLLER 20 | #pragma once 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | // Pinocchio has to be included before ROS 27 | // See https://github.com/wxmerkt/pinocchio_ros_example 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | #include "compliant_controllers/extended_joint_positions.h" 35 | #include "compliant_controllers/robot_state.h" 36 | 37 | 38 | namespace compliant_controllers { 39 | namespace joint_task_space { 40 | /**\class CompliantController 41 | * \brief 42 | * Controller that is compliant in joint space 43 | */ 44 | class CompliantController { 45 | public: 46 | /**\fn CompliantController 47 | * \brief 48 | * Constructor that allocates all necessary structures 49 | * 50 | * \param[in] robot_model 51 | * The robot model 52 | * \param[in] end_effector_link 53 | * The end-effector link of the kinematic chain 54 | * \param[in] num_controlled_dofs 55 | * The number of degree of freedoms that the controller should control 56 | * \param[in] apply_gravity 57 | * Toggle for applying gravity compensation to the computed efforts 58 | */ 59 | CompliantController(std::unique_ptr robot_model, 60 | std::string const& end_effector_link, 61 | int const num_controlled_dofs, 62 | bool apply_gravity); 63 | CompliantController() = delete; 64 | CompliantController(CompliantController const&) = default; 65 | CompliantController& operator= (CompliantController const&) = default; 66 | CompliantController(CompliantController&&) = default; 67 | CompliantController& operator= (CompliantController&&) = default; 68 | 69 | /**\fn setDefaultValues 70 | * \brief 71 | * Set default values to all gains and friction parameters 72 | */ 73 | void setDefaultValues(); 74 | 75 | /**\fn setJointStiffnessMatrix 76 | * \brief 77 | * Set the joint stiffness matrix Ks 78 | * 79 | * \param[in] joint_stiffness_matrix 80 | * The joint stiffness matrix to be set 81 | * \return 82 | * Boolean variable signalling success or failure 83 | */ 84 | [[nodiscard]] 85 | bool setJointStiffnessMatrix(Eigen::MatrixXd const& joint_stiffness_matrix); 86 | 87 | /**\fn setRotorInertiaMatrix 88 | * \brief 89 | * Set the rotor inertia matrix Kr 90 | * 91 | * \param[in] rotor_inertia_matrix 92 | * The rotor inertia matrix to be set 93 | * \return 94 | * Boolean variable signalling success or failure 95 | */ 96 | [[nodiscard]] 97 | bool setRotorInertiaMatrix(Eigen::MatrixXd const& rotor_inertia_matrix); 98 | 99 | /**\fn setFrictionL 100 | * \brief 101 | * Set the friction observer matrix 1 102 | * 103 | * \param[in] friction_l 104 | * The friction observer matrix 1 to be set 105 | * \return 106 | * Boolean variable signalling success or failure 107 | */ 108 | [[nodiscard]] 109 | bool setFrictionL(Eigen::MatrixXd const& friction_l); 110 | 111 | /**\fn setFrictionLp 112 | * \brief 113 | * Set the friction observer matrix 2 114 | * 115 | * \param[in] friction_lp 116 | * The friction observer matrix 2 to be set 117 | * \return 118 | * Boolean variable signalling success or failure 119 | */ 120 | [[nodiscard]] 121 | bool setFrictionLp(Eigen::MatrixXd const& friction_lp); 122 | 123 | /**\fn setFrictionLi 124 | * \brief 125 | * Set the friction observer matrix integral gain 126 | * 127 | * \param[in] friction_li 128 | * The friction observer matrix integral gain 129 | * \return 130 | * Boolean variable signalling success or failure 131 | */ 132 | [[nodiscard]] 133 | bool setFrictionLi(Eigen::MatrixXd const& friction_li); 134 | 135 | /**\fn setTaskKMatrix 136 | * \brief 137 | * Set the task space compliance proportional gain matrix 138 | * 139 | * \param[in] task_k_matrix 140 | * The task space compliance proportional gain matrix to be set 141 | * \return 142 | * Boolean variable signalling success or failure 143 | */ 144 | [[nodiscard]] 145 | bool setTaskKMatrix(Eigen::MatrixXd const& task_k_matrix); 146 | 147 | /**\fn setTaskdMatrix 148 | * \brief 149 | * Set the task compliance derivative gain matrix 150 | * 151 | * \param[in] joint_d_matrix 152 | * The task space compliance proportional gain matrix to be set 153 | * \return 154 | * Boolean variable signalling success or failure 155 | */ 156 | [[nodiscard]] 157 | bool setTaskDMatrix(Eigen::MatrixXd const& task_d_matrix); 158 | 159 | /**\fn setJointKMatrix 160 | * \brief 161 | * Set the joint compliance proportional gain matrix 162 | * 163 | * \param[in] joint_k_matrix 164 | * The joint compliance proportional gain matrix to be set 165 | * \return 166 | * Boolean variable signalling success or failure 167 | */ 168 | [[nodiscard]] 169 | bool setJointKMatrix(Eigen::MatrixXd const& joint_k_matrix); 170 | 171 | /**\fn setJointDMatrix 172 | * \brief 173 | * Set the joint compliance derivative gain matrix 174 | * 175 | * \param[in] joint_d_matrix 176 | * The joint compliance derivative gain matrix to be set 177 | * \return 178 | * Boolean variable signalling success or failure 179 | */ 180 | [[nodiscard]] 181 | bool setJointDMatrix(Eigen::MatrixXd const& joint_d_matrix); 182 | 183 | /**\fn setMaxJointError 184 | * \brief 185 | * Set the joint integrator max error 186 | * 187 | * \param[in] joint_error_max 188 | * The joint compliance derivative gain matrix to be set 189 | * \return 190 | * Boolean variable signalling success or failure 191 | */ 192 | [[nodiscard]] 193 | bool setMaxJointError(Eigen::VectorXd const& joint_error_max); 194 | 195 | /**\fn init 196 | * \brief 197 | * Initialize and Reset the controller 198 | * 199 | * \return 200 | * Boolean variable signalling success or failure 201 | */ 202 | [[nodiscard]] 203 | bool init(); 204 | 205 | /**\fn computeEffort 206 | * \brief 207 | * Compute the required effort for bringing the robot from state \p current_state to state \p desired_state 208 | * 209 | * \param[in] desired_state 210 | * The state that the robot should be set to 211 | * \param[in] current_state 212 | * The state that the robot is currently in 213 | * \param[in] period 214 | * The period of the controller 215 | * \return 216 | * The effort that should be applied to the individual joints to move to the desired state 217 | */ 218 | [[nodiscard]] 219 | Eigen::VectorXd computeEffort(RobotState const& desired_state, RobotState const& current_state, ros::Duration const& period); 220 | 221 | /**\fn integrate_error 222 | * \brief 223 | * Sum error between \p current_q and \p desired_q clamp using q_error_max_ 224 | * 225 | * \param[in] desired_q 226 | * The state that the robot is currently in 227 | * \param[in] current_q 228 | * The state that the robot should be set to 229 | * \return 230 | * The sum of the errors 231 | */ 232 | [[nodiscard]] 233 | Eigen::VectorXd integrate_error(Eigen::VectorXd const& desired_q, 234 | Eigen::VectorXd const& current_q); 235 | 236 | void advanceKinematics(Eigen::VectorXd const& q); 237 | 238 | Eigen::Isometry3d getFrameTransform(Eigen::VectorXd const& q, 239 | pinocchio::Model::Index const& frame_idx); 240 | 241 | 242 | protected: 243 | /**\fn constructDiagonalMatrix 244 | * \brief 245 | * Construct a diagonal matrix of dimension \p dim with \p value in its diagonal 246 | * 247 | * \param[in] value 248 | * The value to be set to the diagonal 249 | * \param[in] dim 250 | * The dimension of the matrix 251 | * \return 252 | * The matrix of dimension \p dim and \p value in its diagonal 253 | */ 254 | [[nodiscard]] 255 | static Eigen::MatrixXd constructDiagonalMatrix(double const value, int const dim); 256 | 257 | /**\fn checkMatrix 258 | * \brief 259 | * Check if the dimensions of the matrix match with the expected dimensions 260 | * 261 | * \param[in] matrix 262 | * The matrix to be checked 263 | * \return 264 | * Boolean value signalling same (true) or different dimensions (false) 265 | */ 266 | [[nodiscard]] 267 | bool checkMatrix(Eigen::MatrixXd const& matrix) const noexcept; 268 | 269 | bool apply_gravity_; 270 | std::unique_ptr robot_model_; 271 | std::string end_effector_link_; 272 | std::unique_ptr data_; 273 | int num_controlled_dofs_; 274 | pinocchio::Model::Index end_effector_index_; 275 | Eigen::MatrixXd inverse_joint_stiffness_matrix_; 276 | Eigen::MatrixXd rotor_inertia_matrix_; 277 | Eigen::MatrixXd inverse_rotor_inertia_matrix_; 278 | Eigen::MatrixXd friction_l_; 279 | Eigen::MatrixXd friction_lp_; 280 | Eigen::MatrixXd friction_li_; // Integral friction observer gain 281 | // Joint space k matrix 282 | Eigen::MatrixXd joint_k_matrix_; 283 | Eigen::MatrixXd joint_d_matrix_; 284 | // Task space k matrix 285 | Eigen::MatrixXd task_k_matrix_; 286 | Eigen::MatrixXd task_d_matrix_; 287 | // Integrator quantities 288 | Eigen::VectorXd q_error_; 289 | Eigen::VectorXd q_error_sum_; 290 | Eigen::VectorXd q_error_max_; 291 | std::unique_ptr extended_joints_; 292 | int count_; 293 | 294 | // Task-space deltas 295 | Eigen::Matrix taskspace_error_; 296 | // The jacobian of the complete robot model including gripper 297 | Eigen::MatrixXd jacobian_; 298 | Eigen::Isometry3d nominal_ee_transform_; 299 | Eigen::Quaterniond nominal_ee_quat_; 300 | Eigen::Isometry3d desired_ee_transform_; 301 | Eigen::Quaterniond desired_ee_quat_; 302 | Eigen::Quaterniond error_quat_; 303 | Eigen::Matrix3d tempMat3d_; 304 | Eigen::Isometry3d tempIsometry3d_; 305 | 306 | Eigen::VectorXd taskspace_effort_; 307 | 308 | Eigen::VectorXd last_desired_positions_; 309 | Eigen::VectorXd nominal_theta_prev_; 310 | Eigen::VectorXd nominal_theta_dot_prev_; 311 | Eigen::VectorXd desired_positions_; 312 | Eigen::VectorXd current_theta_; 313 | Eigen::VectorXd gravity_; 314 | Eigen::VectorXd task_effort_; 315 | Eigen::VectorXd nominal_theta_d_dot_; 316 | Eigen::VectorXd nominal_theta_dot_; 317 | Eigen::VectorXd nominal_theta_; 318 | Eigen::VectorXd nominal_friction_; 319 | Eigen::VectorXd efforts_; 320 | }; 321 | } 322 | } 323 | 324 | #endif // COMPLIANT_CONTROLLERS__JOINT_TASK_SPACE_COMPLIANT_CONTROLLER 325 | -------------------------------------------------------------------------------- /src/task_space/controller.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file task_space_compliant_controller.cpp 3 | * \mainpage 4 | * Compliant controller in joint space 5 | * 6 | * \authors 7 | * Tobit Flatscher 8 | * Rishabh Madan 9 | * Rajat Kumar Jenamani 10 | * \copyright 11 | * Oxford Robotics Institute - University of Oxford (2024) 12 | * EmPRISE Lab - Cornell University (2023) 13 | * \license 14 | * This project is released under the 3-clause BSD license. 15 | */ 16 | 17 | #include "compliant_controllers/task_space/controller.h" 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | // Pinocchio has to be included before ROS 25 | // See https://github.com/wxmerkt/pinocchio_ros_example 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include "compliant_controllers/extended_joint_positions.h" 37 | #include "compliant_controllers/robot_state.h" 38 | #include "compliant_controllers/utils.h" 39 | 40 | 41 | namespace compliant_controllers { 42 | namespace task_space { 43 | 44 | CompliantController::CompliantController( 45 | std::unique_ptr robot_model, std::string const& end_effector_link, 46 | int const num_controlled_dofs, bool apply_gravity) 47 | : robot_model_{std::move(robot_model)}, end_effector_link_{end_effector_link}, 48 | data_{std::make_unique(*robot_model_.get())}, 49 | num_controlled_dofs_{num_controlled_dofs}, apply_gravity_{apply_gravity} 50 | { 51 | 52 | // Check to see if end effector link exits in the chain 53 | if(!robot_model_->existFrame(end_effector_link)) 54 | { 55 | std::cout << "The end effector link does not exist in the model. Link name: " 56 | << end_effector_link << std::endl; 57 | } 58 | else 59 | { 60 | end_effector_index_ = robot_model_->getFrameId(end_effector_link); 61 | } 62 | 63 | setDefaultValues(); 64 | extended_joints_ = std::make_unique(num_controlled_dofs_); 65 | // TODO: Potentially pre-allocate also other structures 66 | return; 67 | } 68 | 69 | void CompliantController::setDefaultValues() { 70 | Eigen::MatrixXd const joint_stiffness_matrix {constructDiagonalMatrix(3750, num_controlled_dofs_)}; 71 | inverse_joint_stiffness_matrix_ = joint_stiffness_matrix.inverse(); 72 | rotor_inertia_matrix_ = constructDiagonalMatrix(0.3, num_controlled_dofs_); 73 | inverse_rotor_inertia_matrix_ = rotor_inertia_matrix_.inverse(); 74 | friction_l_ = constructDiagonalMatrix(60, num_controlled_dofs_); 75 | friction_lp_ = constructDiagonalMatrix(4, num_controlled_dofs_); 76 | friction_li_ = constructDiagonalMatrix(0, num_controlled_dofs_); 77 | task_k_matrix_ = constructDiagonalMatrix(10, 6); 78 | task_d_matrix_ = constructDiagonalMatrix(0.1, 6); 79 | joint_d_matrix_ = constructDiagonalMatrix(2, num_controlled_dofs_); 80 | q_error_ = Eigen::VectorXd::Zero(num_controlled_dofs_); 81 | q_error_sum_ = Eigen::VectorXd::Zero(num_controlled_dofs_); 82 | q_error_max_ = Eigen::VectorXd::Zero(num_controlled_dofs_); 83 | tempMat3d_ = Eigen::Matrix3d::Zero(); 84 | tempIsometry3d_ = Eigen::Isometry3d::Identity(); 85 | jacobian_ = Eigen::MatrixXd::Zero(6, robot_model_->nv); 86 | return; 87 | } 88 | 89 | bool CompliantController::setJointStiffnessMatrix( 90 | Eigen::MatrixXd const& joint_stiffness_matrix) { 91 | if (!checkMatrix(joint_stiffness_matrix)) { 92 | return false; 93 | } 94 | inverse_joint_stiffness_matrix_ = joint_stiffness_matrix.inverse(); 95 | return true; 96 | } 97 | 98 | bool CompliantController::setRotorInertiaMatrix( 99 | Eigen::MatrixXd const& rotor_inertia_matrix) { 100 | if (!checkMatrix(rotor_inertia_matrix)) { 101 | return false; 102 | } 103 | rotor_inertia_matrix_ = rotor_inertia_matrix; 104 | inverse_rotor_inertia_matrix_ = rotor_inertia_matrix.inverse(); 105 | return true; 106 | } 107 | 108 | bool CompliantController::setFrictionL(Eigen::MatrixXd const& friction_l) { 109 | if (!checkMatrix(friction_l)) { 110 | return false; 111 | } 112 | friction_l_ = friction_l; 113 | return true; 114 | } 115 | 116 | bool CompliantController::setFrictionLp(Eigen::MatrixXd const& friction_lp) { 117 | if (!checkMatrix(friction_lp)) { 118 | return false; 119 | } 120 | friction_lp_ = friction_lp; 121 | return true; 122 | } 123 | 124 | bool CompliantController::setFrictionLi(Eigen::MatrixXd const& friction_li) { 125 | if (!checkMatrix(friction_li)) { 126 | return false; 127 | } 128 | friction_li_ = friction_li; 129 | return true; 130 | } 131 | 132 | bool CompliantController::setTaskKMatrix(Eigen::MatrixXd const& task_k_matrix) { 133 | if ((task_k_matrix.rows() != 6) || (task_k_matrix.cols() != 6)) { 134 | return false; 135 | } 136 | task_k_matrix_ = task_k_matrix; 137 | return true; 138 | } 139 | 140 | bool CompliantController::setTaskDMatrix(Eigen::MatrixXd const& task_d_matrix) 141 | { 142 | if ((task_d_matrix.rows() != 6) || (task_d_matrix.cols() != 6)) { 143 | return false; 144 | } 145 | task_d_matrix_ = task_d_matrix; 146 | return true; 147 | } 148 | 149 | bool CompliantController::setJointDMatrix(Eigen::MatrixXd const& joint_d_matrix) { 150 | if (!checkMatrix(joint_d_matrix)) { 151 | return false; 152 | } 153 | joint_d_matrix_ = joint_d_matrix; 154 | return true; 155 | } 156 | 157 | bool CompliantController::setMaxJointError(Eigen::VectorXd const& joint_error_max) 158 | { 159 | if (!joint_error_max.rows() != num_controlled_dofs_) return false; 160 | q_error_max_ = joint_error_max; 161 | return true; 162 | } 163 | 164 | bool CompliantController::init() { 165 | count_ = 0; 166 | q_error_.setZero(); 167 | q_error_sum_.setZero(); 168 | return true; 169 | } 170 | 171 | void CompliantController::advanceKinematics(Eigen::VectorXd const& q) 172 | { 173 | pinocchio::forwardKinematics(*robot_model_, *data_, joint_ros_to_pinocchio(q, *robot_model_)); 174 | pinocchio::computeJointJacobians(*robot_model_, *data_, joint_ros_to_pinocchio(q, *robot_model_)); 175 | pinocchio::updateFramePlacements(*robot_model_, *data_); 176 | } 177 | 178 | Eigen::Isometry3d CompliantController::getFrameTransform(Eigen::VectorXd const& q, 179 | pinocchio::Model::Index const& frame_idx) 180 | { 181 | advanceKinematics(q); 182 | tempIsometry3d_ = data_->oMf[frame_idx].toHomogeneousMatrix_impl(); 183 | tempMat3d_ = tempIsometry3d_.linear(); 184 | // [Emprise Lab] Make first and second column negative to account for axis convention 185 | tempMat3d_.col(0) *= -1.0; 186 | tempMat3d_.col(1) *= -1.0; 187 | tempIsometry3d_.linear() = tempMat3d_; 188 | return tempIsometry3d_; 189 | } 190 | 191 | Eigen::VectorXd CompliantController::computeEffort(RobotState const& desired_state, 192 | RobotState const& current_state, ros::Duration const& period) { 193 | desired_positions_ = desired_state.positions; 194 | 195 | if (!extended_joints_->isInitialized()) { 196 | last_desired_positions_ = desired_state.positions; 197 | [[maybe_unused]] bool const is_success {extended_joints_->init(current_state.positions)}; 198 | extended_joints_->update(current_state.positions); 199 | nominal_theta_prev_ = extended_joints_->getPositions(); 200 | // nominal_theta_prev_ = current_state.positions; 201 | nominal_theta_dot_prev_ = current_state.velocities; 202 | desired_positions_ = nominal_theta_prev_; 203 | } 204 | 205 | extended_joints_->update(current_state.positions); 206 | current_theta_ = extended_joints_->getPositions(); 207 | 208 | gravity_ = pinocchio::computeGeneralizedGravity(*robot_model_, *data_, joint_ros_to_pinocchio(current_theta_, *robot_model_)); 209 | 210 | // DESIRED AND NOMINAL TRANSFORMS FOR ERROR PREDICTION 211 | 212 | // Get desired ee position from desired joint states 213 | desired_ee_transform_ = getFrameTransform(desired_positions_, end_effector_index_); 214 | desired_ee_quat_ = Eigen::Quaterniond(desired_ee_transform_.linear()); 215 | 216 | // Get nominal ee position from desired joint states 217 | nominal_ee_transform_ = getFrameTransform(nominal_theta_prev_, end_effector_index_); 218 | nominal_ee_quat_ = Eigen::Quaterniond(nominal_ee_transform_.linear()); 219 | 220 | pinocchio::getFrameJacobian(*robot_model_, *data_, end_effector_index_, pinocchio::LOCAL_WORLD_ALIGNED, jacobian_); 221 | 222 | // Reorientate quat if needed 223 | if (desired_ee_quat_.coeffs().dot(nominal_ee_quat_.coeffs()) < 0.0) 224 | { 225 | nominal_ee_quat_.coeffs() = -nominal_ee_quat_.coeffs(); 226 | } 227 | 228 | // Taskspace orientation error 229 | error_quat_ = nominal_ee_quat_.inverse() * desired_ee_quat_; 230 | // Update the task space error 231 | taskspace_error_(3) = error_quat_.x(); 232 | taskspace_error_(4) = error_quat_.y(); 233 | taskspace_error_(5) = error_quat_.z(); 234 | taskspace_error_.tail(3) = -nominal_ee_transform_.linear() * taskspace_error_.tail(3); 235 | // Taskspace position error 236 | taskspace_error_.head(3) = nominal_ee_transform_.translation() - desired_ee_transform_.translation(); // positional error 237 | 238 | // Gravity compensation is performed inside the hardware interface 239 | // The jacobian includes derivatives for the gripper which does not affect the manipulator so these are ignored 240 | task_effort_.noalias() = jacobian_.block(0, 0, 6, num_controlled_dofs_).transpose() * (-task_k_matrix_ * taskspace_error_ 241 | -task_d_matrix_ * jacobian_.block(0, 0, 6, num_controlled_dofs_) * (nominal_theta_dot_prev_ - desired_state.velocities)) 242 | - joint_d_matrix_*(nominal_theta_dot_prev_ - desired_state.velocities); 243 | 244 | // Time step smaller than period.toSec() potentially because of too small rotor inertia matrix 245 | double const step_time {0.001}; 246 | // Sign of the current state effort seems to be negative of commanded 247 | nominal_theta_d_dot_.noalias() = inverse_rotor_inertia_matrix_*(task_effort_ + gravity_ + current_state.efforts); 248 | nominal_theta_dot_ = nominal_theta_dot_prev_ + nominal_theta_d_dot_*step_time; 249 | nominal_theta_ = nominal_theta_prev_ + nominal_theta_dot_*step_time; 250 | 251 | q_error_sum_ = integrate_error(nominal_theta_, current_theta_); 252 | 253 | nominal_friction_.noalias() = rotor_inertia_matrix_*friction_l_*((nominal_theta_dot_ - current_state.velocities) + 254 | friction_lp_*(nominal_theta_ - current_theta_) 255 | + friction_li_ * q_error_sum_); 256 | 257 | efforts_ = task_effort_ + nominal_friction_; 258 | 259 | // Apply gravity compensation if required 260 | if (apply_gravity_) { efforts_ += gravity_; } 261 | 262 | nominal_theta_prev_ = nominal_theta_; 263 | nominal_theta_dot_prev_ = nominal_theta_dot_; 264 | 265 | return efforts_; 266 | } 267 | 268 | Eigen::VectorXd CompliantController::integrate_error(Eigen::VectorXd const& desired_q, 269 | Eigen::VectorXd const& current_q) 270 | { 271 | q_error_ = desired_q - current_q; 272 | q_error_sum_ += q_error_; 273 | // Clamp the integrated error 274 | q_error_sum_ = q_error_sum_.cwiseMin(q_error_max_).cwiseMax(-q_error_max_); 275 | return q_error_sum_; 276 | } 277 | 278 | Eigen::MatrixXd CompliantController::constructDiagonalMatrix(double const value, int const dim) { 279 | Eigen::VectorXd diag_elements {Eigen::VectorXd::Zero(dim)}; 280 | for (Eigen::Index i = 0; i < diag_elements.size(); ++i) { 281 | diag_elements[i] = value; 282 | } 283 | Eigen::MatrixXd matrix {Eigen::MatrixXd::Zero(dim, dim)}; 284 | matrix.diagonal() = diag_elements; 285 | return matrix; 286 | } 287 | 288 | bool CompliantController::checkMatrix(Eigen::MatrixXd const& matrix) const noexcept { 289 | if ((matrix.rows() != num_controlled_dofs_) || (matrix.cols() != num_controlled_dofs_)) { 290 | return false; 291 | } 292 | return true; 293 | } 294 | } // namespace task_space 295 | } // namespace compliant_controllers -------------------------------------------------------------------------------- /src/joint_task_space/controller.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file task_space_compliant_controller.cpp 3 | * \mainpage 4 | * Compliant controller in joint space 5 | * 6 | * \authors 7 | * Tobit Flatscher 8 | * Rishabh Madan 9 | * Rajat Kumar Jenamani 10 | * \copyright 11 | * Oxford Robotics Institute - University of Oxford (2024) 12 | * EmPRISE Lab - Cornell University (2023) 13 | * \license 14 | * This project is released under the 3-clause BSD license. 15 | */ 16 | 17 | #include "compliant_controllers/joint_task_space/controller.h" 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | // Pinocchio has to be included before ROS 25 | // See https://github.com/wxmerkt/pinocchio_ros_example 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include "compliant_controllers/extended_joint_positions.h" 37 | #include "compliant_controllers/robot_state.h" 38 | #include "compliant_controllers/utils.h" 39 | 40 | 41 | namespace compliant_controllers { 42 | namespace joint_task_space { 43 | 44 | CompliantController::CompliantController( 45 | std::unique_ptr robot_model, std::string const& end_effector_link, 46 | int const num_controlled_dofs, bool apply_gravity) 47 | : robot_model_{std::move(robot_model)}, end_effector_link_{end_effector_link}, 48 | data_{std::make_unique(*robot_model_.get())}, 49 | num_controlled_dofs_{num_controlled_dofs}, apply_gravity_{apply_gravity} 50 | { 51 | 52 | // Check to see if end effector link exits in the chain 53 | if(!robot_model_->existFrame(end_effector_link)) 54 | { 55 | std::cout << "The end effector link does not exist in the model. Link name: " 56 | << end_effector_link << std::endl; 57 | } 58 | else 59 | { 60 | end_effector_index_ = robot_model_->getFrameId(end_effector_link); 61 | } 62 | 63 | setDefaultValues(); 64 | extended_joints_ = std::make_unique(num_controlled_dofs_); 65 | // TODO: Potentially pre-allocate also other structures 66 | return; 67 | } 68 | 69 | void CompliantController::setDefaultValues() { 70 | Eigen::MatrixXd const joint_stiffness_matrix {constructDiagonalMatrix(3750, num_controlled_dofs_)}; 71 | inverse_joint_stiffness_matrix_ = joint_stiffness_matrix.inverse(); 72 | rotor_inertia_matrix_ = constructDiagonalMatrix(0.3, num_controlled_dofs_); 73 | inverse_rotor_inertia_matrix_ = rotor_inertia_matrix_.inverse(); 74 | friction_l_ = constructDiagonalMatrix(60, num_controlled_dofs_); 75 | friction_lp_ = constructDiagonalMatrix(4, num_controlled_dofs_); 76 | friction_li_ = constructDiagonalMatrix(0, num_controlled_dofs_); 77 | joint_k_matrix_ = constructDiagonalMatrix(1, num_controlled_dofs_); 78 | task_k_matrix_ = constructDiagonalMatrix(10, 6); 79 | task_d_matrix_ = constructDiagonalMatrix(0.1, 6); 80 | joint_d_matrix_ = constructDiagonalMatrix(2, num_controlled_dofs_); 81 | q_error_ = Eigen::VectorXd::Zero(num_controlled_dofs_); 82 | q_error_sum_ = Eigen::VectorXd::Zero(num_controlled_dofs_); 83 | q_error_max_ = Eigen::VectorXd::Zero(num_controlled_dofs_); 84 | tempMat3d_ = Eigen::Matrix3d::Zero(); 85 | tempIsometry3d_ = Eigen::Isometry3d::Identity(); 86 | jacobian_ = Eigen::MatrixXd::Zero(6, robot_model_->nv); 87 | return; 88 | } 89 | 90 | bool CompliantController::setJointStiffnessMatrix( 91 | Eigen::MatrixXd const& joint_stiffness_matrix) { 92 | if (!checkMatrix(joint_stiffness_matrix)) { 93 | return false; 94 | } 95 | inverse_joint_stiffness_matrix_ = joint_stiffness_matrix.inverse(); 96 | return true; 97 | } 98 | 99 | bool CompliantController::setRotorInertiaMatrix( 100 | Eigen::MatrixXd const& rotor_inertia_matrix) { 101 | if (!checkMatrix(rotor_inertia_matrix)) { 102 | return false; 103 | } 104 | rotor_inertia_matrix_ = rotor_inertia_matrix; 105 | inverse_rotor_inertia_matrix_ = rotor_inertia_matrix.inverse(); 106 | return true; 107 | } 108 | 109 | bool CompliantController::setFrictionL(Eigen::MatrixXd const& friction_l) { 110 | if (!checkMatrix(friction_l)) { 111 | return false; 112 | } 113 | friction_l_ = friction_l; 114 | return true; 115 | } 116 | 117 | bool CompliantController::setFrictionLp(Eigen::MatrixXd const& friction_lp) { 118 | if (!checkMatrix(friction_lp)) { 119 | return false; 120 | } 121 | friction_lp_ = friction_lp; 122 | return true; 123 | } 124 | 125 | bool CompliantController::setFrictionLi(Eigen::MatrixXd const& friction_li) { 126 | if (!checkMatrix(friction_li)) { 127 | return false; 128 | } 129 | friction_li_ = friction_li; 130 | return true; 131 | } 132 | 133 | bool CompliantController::setTaskKMatrix(Eigen::MatrixXd const& task_k_matrix) { 134 | if ((task_k_matrix.rows() != 6) || (task_k_matrix.cols() != 6)) { 135 | return false; 136 | } 137 | task_k_matrix_ = task_k_matrix; 138 | return true; 139 | } 140 | 141 | bool CompliantController::setTaskDMatrix(Eigen::MatrixXd const& task_d_matrix) 142 | { 143 | if ((task_d_matrix.rows() != 6) || (task_d_matrix.cols() != 6)) { 144 | return false; 145 | } 146 | task_d_matrix_ = task_d_matrix; 147 | return true; 148 | } 149 | 150 | bool CompliantController::setJointKMatrix(Eigen::MatrixXd const& joint_k_matrix) { 151 | if (!checkMatrix(joint_k_matrix)) { 152 | std::cout << "Failed to set joint k matrix" << std::endl; 153 | return false; 154 | } 155 | joint_k_matrix_ = joint_k_matrix; 156 | return true; 157 | } 158 | 159 | bool CompliantController::setJointDMatrix(Eigen::MatrixXd const& joint_d_matrix) { 160 | if (!checkMatrix(joint_d_matrix)) { 161 | return false; 162 | } 163 | joint_d_matrix_ = joint_d_matrix; 164 | return true; 165 | } 166 | 167 | bool CompliantController::setMaxJointError(Eigen::VectorXd const& joint_error_max) 168 | { 169 | if (!joint_error_max.rows() != num_controlled_dofs_) return false; 170 | q_error_max_ = joint_error_max; 171 | return true; 172 | } 173 | 174 | bool CompliantController::init() { 175 | count_ = 0; 176 | q_error_.setZero(); 177 | q_error_sum_.setZero(); 178 | return true; 179 | } 180 | 181 | void CompliantController::advanceKinematics(Eigen::VectorXd const& q) 182 | { 183 | pinocchio::forwardKinematics(*robot_model_, *data_, joint_ros_to_pinocchio(q, *robot_model_)); 184 | pinocchio::computeJointJacobians(*robot_model_, *data_, joint_ros_to_pinocchio(q, *robot_model_)); 185 | pinocchio::updateFramePlacements(*robot_model_, *data_); 186 | } 187 | 188 | Eigen::Isometry3d CompliantController::getFrameTransform(Eigen::VectorXd const& q, 189 | pinocchio::Model::Index const& frame_idx) 190 | { 191 | advanceKinematics(q); 192 | tempIsometry3d_ = data_->oMf[frame_idx].toHomogeneousMatrix_impl(); 193 | tempMat3d_ = tempIsometry3d_.linear(); 194 | // [Emprise Lab] Make first and second column negative to account for axis convention 195 | tempMat3d_.col(0) *= -1.0; 196 | tempMat3d_.col(1) *= -1.0; 197 | tempIsometry3d_.linear() = tempMat3d_; 198 | return tempIsometry3d_; 199 | } 200 | 201 | Eigen::VectorXd CompliantController::computeEffort(RobotState const& desired_state, 202 | RobotState const& current_state, ros::Duration const& period) { 203 | desired_positions_ = desired_state.positions; 204 | 205 | if (!extended_joints_->isInitialized()) { 206 | last_desired_positions_ = desired_state.positions; 207 | [[maybe_unused]] bool const is_success {extended_joints_->init(current_state.positions)}; 208 | extended_joints_->update(current_state.positions); 209 | nominal_theta_prev_ = extended_joints_->getPositions(); 210 | // nominal_theta_prev_ = current_state.positions; 211 | nominal_theta_dot_prev_ = current_state.velocities; 212 | desired_positions_ = nominal_theta_prev_; 213 | } 214 | 215 | extended_joints_->update(current_state.positions); 216 | current_theta_ = extended_joints_->getPositions(); 217 | 218 | gravity_ = pinocchio::computeGeneralizedGravity(*robot_model_, *data_, joint_ros_to_pinocchio(current_theta_, *robot_model_)); 219 | 220 | // DESIRED AND NOMINAL TRANSFORMS FOR ERROR PREDICTION 221 | 222 | // Get desired ee position from desired joint states 223 | desired_ee_transform_ = getFrameTransform(desired_positions_, end_effector_index_); 224 | desired_ee_quat_ = Eigen::Quaterniond(desired_ee_transform_.linear()); 225 | 226 | // Get nominal ee position from desired joint states 227 | nominal_ee_transform_ = getFrameTransform(nominal_theta_prev_, end_effector_index_); 228 | nominal_ee_quat_ = Eigen::Quaterniond(nominal_ee_transform_.linear()); 229 | 230 | pinocchio::getFrameJacobian(*robot_model_, *data_, end_effector_index_, pinocchio::LOCAL_WORLD_ALIGNED, jacobian_); 231 | 232 | // Reorientate quat if needed 233 | if (desired_ee_quat_.coeffs().dot(nominal_ee_quat_.coeffs()) < 0.0) 234 | { 235 | nominal_ee_quat_.coeffs() = -nominal_ee_quat_.coeffs(); 236 | } 237 | 238 | // Taskspace orientation error 239 | error_quat_ = nominal_ee_quat_.inverse() * desired_ee_quat_; 240 | // Update the task space error 241 | taskspace_error_(3) = error_quat_.x(); 242 | taskspace_error_(4) = error_quat_.y(); 243 | taskspace_error_(5) = error_quat_.z(); 244 | taskspace_error_.tail(3) = -nominal_ee_transform_.linear() * taskspace_error_.tail(3); 245 | // Taskspace position error 246 | taskspace_error_.head(3) = nominal_ee_transform_.translation() - desired_ee_transform_.translation(); // positional error 247 | 248 | // Gravity compensation is performed inside the hardware interface 249 | // The jacobian includes derivatives for the gripper which does not affect the manipulator so these are ignored 250 | task_effort_.noalias() = jacobian_.block(0, 0, 6, num_controlled_dofs_).transpose() * (-task_k_matrix_ * taskspace_error_ 251 | -task_d_matrix_ * jacobian_.block(0, 0, 6, num_controlled_dofs_) * (nominal_theta_dot_prev_ - desired_state.velocities)) 252 | - joint_k_matrix_*(nominal_theta_prev_ - desired_positions_ - inverse_joint_stiffness_matrix_*gravity_) 253 | - joint_d_matrix_*(nominal_theta_dot_prev_ - desired_state.velocities); 254 | 255 | // Time step smaller than period.toSec() potentially because of too small rotor inertia matrix 256 | double const step_time {0.001}; 257 | // Sign of the current state effort seems to be negative of commanded 258 | nominal_theta_d_dot_.noalias() = inverse_rotor_inertia_matrix_*(task_effort_ + gravity_ + current_state.efforts); 259 | nominal_theta_dot_ = nominal_theta_dot_prev_ + nominal_theta_d_dot_*step_time; 260 | nominal_theta_ = nominal_theta_prev_ + nominal_theta_dot_*step_time; 261 | 262 | q_error_sum_ = integrate_error(nominal_theta_, current_theta_); 263 | 264 | nominal_friction_.noalias() = rotor_inertia_matrix_*friction_l_*((nominal_theta_dot_ - current_state.velocities) + 265 | friction_lp_*(nominal_theta_ - current_theta_) 266 | + friction_li_ * q_error_sum_); 267 | 268 | efforts_ = task_effort_ + nominal_friction_; 269 | 270 | // Apply gravity compensation if desired 271 | if (apply_gravity_) { efforts_ += gravity_; } 272 | 273 | nominal_theta_prev_ = nominal_theta_; 274 | nominal_theta_dot_prev_ = nominal_theta_dot_; 275 | 276 | return efforts_; 277 | } 278 | 279 | Eigen::VectorXd CompliantController::integrate_error(Eigen::VectorXd const& desired_q, 280 | Eigen::VectorXd const& current_q) 281 | { 282 | q_error_ = desired_q - current_q; 283 | q_error_sum_ += q_error_; 284 | // Clamp the integrated error 285 | q_error_sum_ = q_error_sum_.cwiseMin(q_error_max_).cwiseMax(-q_error_max_); 286 | return q_error_sum_; 287 | } 288 | 289 | Eigen::MatrixXd CompliantController::constructDiagonalMatrix(double const value, int const dim) { 290 | Eigen::VectorXd diag_elements {Eigen::VectorXd::Zero(dim)}; 291 | for (Eigen::Index i = 0; i < diag_elements.size(); ++i) { 292 | diag_elements[i] = value; 293 | } 294 | Eigen::MatrixXd matrix {Eigen::MatrixXd::Zero(dim, dim)}; 295 | matrix.diagonal() = diag_elements; 296 | return matrix; 297 | } 298 | 299 | bool CompliantController::checkMatrix(Eigen::MatrixXd const& matrix) const noexcept { 300 | if ((matrix.rows() != num_controlled_dofs_) || (matrix.cols() != num_controlled_dofs_)) { 301 | return false; 302 | } 303 | return true; 304 | } 305 | } 306 | } --------------------------------------------------------------------------------