cartesian_motion_controller.cpp
1 //////////////////////////////////////////////////////////////////////////////// 2 // Copyright 2019 FZI Research Center for Information Technology 3 // 4 // Redistribution and use in source and binary forms, with or without 5 // modification, are permitted provided that the following conditions are met: 6 // 7 // 1. Redistributions of source code must retain the above copyright notice, 8 // this list of conditions and the following disclaimer. 9 // 10 // 2. Redistributions in binary form must reproduce the above copyright notice, 11 // this list of conditions and the following disclaimer in the documentation 12 // and/or other materials provided with the distribution. 13 // 14 // 3. Neither the name of the copyright holder nor the names of its 15 // contributors may be used to endorse or promote products derived from this 16 // software without specific prior written permission. 17 // 18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 // POSSIBILITY OF SUCH DAMAGE. 29 //////////////////////////////////////////////////////////////////////////////// 30 31 //----------------------------------------------------------------------------- 32 /*!\file cartesian_motion_controller.cpp 33 * 34 * \author Stefan Scherzinger <scherzin@fzi.de> 35 * \date 2017/07/27 36 * 37 */ 38 //----------------------------------------------------------------------------- 39 40 #include <cartesian_motion_controller/cartesian_motion_controller.h> 41 42 #include <algorithm> 43 #include <cmath> 44 45 #include "cartesian_controller_base/Utility.h" 46 #include "controller_interface/controller_interface.hpp" 47 #include "rclcpp/clock.hpp" 48 #include "rclcpp/duration.hpp" 49 50 namespace cartesian_motion_controller 51 { 52 CartesianMotionController::CartesianMotionController() : Base::CartesianControllerBase() {} 53 54 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 55 CartesianMotionController::on_init() 56 { 57 const auto ret = Base::on_init(); 58 if (ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) 59 { 60 return ret; 61 } 62 63 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 64 } 65 66 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 67 CartesianMotionController::on_configure(const rclcpp_lifecycle::State & previous_state) 68 { 69 const auto ret = Base::on_configure(previous_state); 70 if (ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) 71 { 72 return ret; 73 } 74 75 m_target_frame_subscr = get_node()->create_subscription<geometry_msgs::msg::PoseStamped>( 76 get_node()->get_name() + std::string("/target_frame"), 3, 77 std::bind(&CartesianMotionController::targetFrameCallback, this, std::placeholders::_1)); 78 79 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 80 } 81 82 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 83 CartesianMotionController::on_activate(const rclcpp_lifecycle::State & previous_state) 84 { 85 Base::on_activate(previous_state); 86 87 // Reset simulation with real joint state 88 m_current_frame = Base::m_ik_solver->getEndEffectorPose(); 89 90 // Start where we are 91 m_target_frame = m_current_frame; 92 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 93 } 94 95 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 96 CartesianMotionController::on_deactivate(const rclcpp_lifecycle::State & previous_state) 97 { 98 Base::on_deactivate(previous_state); 99 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 100 } 101 102 controller_interface::return_type CartesianMotionController::update(const rclcpp::Time & time, 103 const rclcpp::Duration & period) 104 { 105 // Synchronize the internal model and the real robot 106 Base::m_ik_solver->synchronizeJointPositions(Base::m_joint_state_pos_handles); 107 108 // Forward Dynamics turns the search for the according joint motion into a 109 // control process. So, we control the internal model until we meet the 110 // Cartesian target motion. This internal control needs some simulation time 111 // steps. 112 for (int i = 0; i < Base::m_iterations; ++i) 113 { 114 // The internal 'simulation time' is deliberately independent of the outer 115 // control cycle. 116 auto internal_period = rclcpp::Duration::from_seconds(0.02); 117 118 // Compute the motion error = target - current. 119 ctrl::Vector6D error = computeMotionError(); 120 121 // Turn Cartesian error into joint motion 122 Base::computeJointControlCmds(error, internal_period); 123 } 124 125 // Write final commands to the hardware interface 126 Base::writeJointControlCmds(); 127 128 return controller_interface::return_type::OK; 129 } 130 131 ctrl::Vector6D CartesianMotionController::computeMotionError() 132 { 133 // Compute motion error wrt robot_base_link 134 m_current_frame = Base::m_ik_solver->getEndEffectorPose(); 135 136 // Transformation from target -> current corresponds to error = target - current 137 KDL::Frame error_kdl; 138 error_kdl.M = m_target_frame.M * m_current_frame.M.Inverse(); 139 error_kdl.p = m_target_frame.p - m_current_frame.p; 140 141 // Use Rodrigues Vector for a compact representation of orientation errors 142 // Only for angles within [0,Pi) 143 KDL::Vector rot_axis = KDL::Vector::Zero(); 144 double angle = error_kdl.M.GetRotAngle(rot_axis); // rot_axis is normalized 145 double distance = error_kdl.p.Normalize(); 146 147 // Clamp maximal tolerated error. 148 // The remaining error will be handled in the next control cycle. 149 // Note that this is also the maximal offset that the 150 // cartesian_compliance_controller can use to build up a restoring stiffness 151 // wrench. 152 const double max_angle = 1.0; 153 const double max_distance = 1.0; 154 angle = std::clamp(angle, -max_angle, max_angle); 155 distance = std::clamp(distance, -max_distance, max_distance); 156 157 // Scale errors to allowed magnitudes 158 rot_axis = rot_axis * angle; 159 error_kdl.p = error_kdl.p * distance; 160 161 // Reassign values 162 ctrl::Vector6D error; 163 error(0) = error_kdl.p.x(); 164 error(1) = error_kdl.p.y(); 165 error(2) = error_kdl.p.z(); 166 error(3) = rot_axis(0); 167 error(4) = rot_axis(1); 168 error(5) = rot_axis(2); 169 170 return error; 171 } 172 173 void CartesianMotionController::targetFrameCallback( 174 const geometry_msgs::msg::PoseStamped::SharedPtr target) 175 { 176 if (!this->isActive()) 177 { 178 return; 179 } 180 181 if (std::isnan(target->pose.position.x) || std::isnan(target->pose.position.y) || 182 std::isnan(target->pose.position.z) || std::isnan(target->pose.orientation.x) || 183 std::isnan(target->pose.orientation.y) || std::isnan(target->pose.orientation.z) || 184 std::isnan(target->pose.orientation.w)) 185 { 186 auto & clock = *get_node()->get_clock(); 187 RCLCPP_WARN_STREAM_THROTTLE(get_node()->get_logger(), clock, 3000, 188 "NaN detected in target pose. Ignoring input."); 189 return; 190 } 191 192 if (target->header.frame_id != Base::m_robot_base_link) 193 { 194 auto & clock = *get_node()->get_clock(); 195 RCLCPP_WARN_THROTTLE(get_node()->get_logger(), clock, 3000, 196 "Got target pose in wrong reference frame. Expected: %s but got %s", 197 Base::m_robot_base_link.c_str(), target->header.frame_id.c_str()); 198 return; 199 } 200 201 m_target_frame = KDL::Frame( 202 KDL::Rotation::Quaternion(target->pose.orientation.x, target->pose.orientation.y, 203 target->pose.orientation.z, target->pose.orientation.w), 204 KDL::Vector(target->pose.position.x, target->pose.position.y, target->pose.position.z)); 205 } 206 207 } // namespace cartesian_motion_controller 208 209 // Pluginlib 210 #include <pluginlib/class_list_macros.hpp> 211 212 PLUGINLIB_EXPORT_CLASS(cartesian_motion_controller::CartesianMotionController, 213 controller_interface::ControllerInterface)