cartesian_controller_base.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_controller_base.cpp 33 * 34 * \author Stefan Scherzinger <scherzin@fzi.de> 35 * \date 2017/07/27 36 * 37 */ 38 //----------------------------------------------------------------------------- 39 40 #include <cartesian_controller_base/cartesian_controller_base.h> 41 #include <urdf/model.h> 42 #include <urdf_model/joint.h> 43 44 #include <cmath> 45 #include <kdl/jntarray.hpp> 46 #include <kdl/tree.hpp> 47 #include <kdl_parser/kdl_parser.hpp> 48 49 #include "controller_interface/controller_interface.hpp" 50 #include "controller_interface/helpers.hpp" 51 #include "geometry_msgs/msg/detail/pose_stamped__struct.hpp" 52 #include "geometry_msgs/msg/detail/twist_stamped__struct.hpp" 53 #include "hardware_interface/types/hardware_interface_type_values.hpp" 54 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" 55 56 namespace cartesian_controller_base 57 { 58 CartesianControllerBase::CartesianControllerBase() {} 59 60 controller_interface::InterfaceConfiguration 61 CartesianControllerBase::command_interface_configuration() const 62 { 63 controller_interface::InterfaceConfiguration conf; 64 conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; 65 conf.names.reserve(m_joint_names.size() * m_cmd_interface_types.size()); 66 for (const auto & type : m_cmd_interface_types) 67 { 68 for (const auto & joint_name : m_joint_names) 69 { 70 conf.names.push_back(joint_name + std::string("/").append(type)); 71 } 72 } 73 return conf; 74 } 75 76 controller_interface::InterfaceConfiguration 77 CartesianControllerBase::state_interface_configuration() const 78 { 79 controller_interface::InterfaceConfiguration conf; 80 conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; 81 conf.names.reserve(m_joint_names.size()); // Only position 82 for (const auto & joint_name : m_joint_names) 83 { 84 conf.names.push_back(joint_name + "/position"); 85 } 86 return conf; 87 } 88 89 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 90 CartesianControllerBase::on_init() 91 { 92 if (!m_initialized) 93 { 94 auto_declare<std::string>("ik_solver", "forward_dynamics"); 95 auto_declare<std::string>("robot_description", ""); 96 auto_declare<std::string>("robot_base_link", ""); 97 auto_declare<std::string>("end_effector_link", ""); 98 auto_declare<std::vector<std::string>>("joints", std::vector<std::string>()); 99 auto_declare<std::vector<std::string>>("command_interfaces", std::vector<std::string>()); 100 auto_declare<double>("solver.error_scale", 1.0); 101 auto_declare<int>("solver.iterations", 1); 102 auto_declare<bool>("solver.publish_state_feedback", false); 103 m_initialized = true; 104 } 105 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 106 } 107 108 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 109 CartesianControllerBase::on_configure(const rclcpp_lifecycle::State & previous_state) 110 { 111 if (m_configured) 112 { 113 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 114 } 115 116 // Load user specified inverse kinematics solver 117 std::string ik_solver = get_node()->get_parameter("ik_solver").as_string(); 118 m_solver_loader.reset(new pluginlib::ClassLoader<IKSolver>( 119 "cartesian_controller_base", "cartesian_controller_base::IKSolver")); 120 try 121 { 122 m_ik_solver = m_solver_loader->createSharedInstance(ik_solver); 123 } 124 catch (pluginlib::PluginlibException & ex) 125 { 126 RCLCPP_ERROR(get_node()->get_logger(), ex.what()); 127 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 128 } 129 130 // Get kinematics specific configuration 131 urdf::Model robot_model; 132 KDL::Tree robot_tree; 133 134 #if defined CARTESIAN_CONTROLLERS_JAZZY 135 m_robot_description = this->get_robot_description(); 136 #else 137 m_robot_description = get_node()->get_parameter("robot_description").as_string(); 138 #endif 139 140 if (m_robot_description.empty()) 141 { 142 RCLCPP_ERROR(get_node()->get_logger(), "robot_description is empty"); 143 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 144 } 145 m_robot_base_link = get_node()->get_parameter("robot_base_link").as_string(); 146 if (m_robot_base_link.empty()) 147 { 148 RCLCPP_ERROR(get_node()->get_logger(), "robot_base_link is empty"); 149 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 150 } 151 m_end_effector_link = get_node()->get_parameter("end_effector_link").as_string(); 152 if (m_end_effector_link.empty()) 153 { 154 RCLCPP_ERROR(get_node()->get_logger(), "end_effector_link is empty"); 155 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 156 } 157 158 // Build a kinematic chain of the robot 159 if (!robot_model.initString(m_robot_description)) 160 { 161 RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse urdf model from 'robot_description'"); 162 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 163 } 164 if (!kdl_parser::treeFromUrdfModel(robot_model, robot_tree)) 165 { 166 RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse KDL tree from urdf model"); 167 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 168 } 169 if (!robot_tree.getChain(m_robot_base_link, m_end_effector_link, m_robot_chain)) 170 { 171 const std::string error = 172 "" 173 "Failed to parse robot chain from urdf model. " 174 "Do robot_base_link and end_effector_link exist?"; 175 RCLCPP_ERROR(get_node()->get_logger(), error.c_str()); 176 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 177 } 178 179 // Get names of actuated joints 180 m_joint_names = get_node()->get_parameter("joints").as_string_array(); 181 if (m_joint_names.empty()) 182 { 183 RCLCPP_ERROR(get_node()->get_logger(), "joints array is empty"); 184 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 185 } 186 187 // Parse joint limits 188 KDL::JntArray upper_pos_limits(m_joint_names.size()); 189 KDL::JntArray lower_pos_limits(m_joint_names.size()); 190 for (size_t i = 0; i < m_joint_names.size(); ++i) 191 { 192 if (!robot_model.getJoint(m_joint_names[i])) 193 { 194 RCLCPP_ERROR(get_node()->get_logger(), "Joint %s does not appear in robot_description", 195 m_joint_names[i].c_str()); 196 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 197 } 198 if (robot_model.getJoint(m_joint_names[i])->type == urdf::Joint::CONTINUOUS) 199 { 200 upper_pos_limits(i) = std::nan("0"); 201 lower_pos_limits(i) = std::nan("0"); 202 } 203 else 204 { 205 // Non-existent urdf limits are zero initialized 206 upper_pos_limits(i) = robot_model.getJoint(m_joint_names[i])->limits->upper; 207 lower_pos_limits(i) = robot_model.getJoint(m_joint_names[i])->limits->lower; 208 } 209 } 210 211 // Initialize solvers 212 m_ik_solver->init(get_node(), m_robot_chain, upper_pos_limits, lower_pos_limits); 213 KDL::Tree tmp("not_relevant"); 214 tmp.addChain(m_robot_chain, "not_relevant"); 215 m_forward_kinematics_solver.reset(new KDL::TreeFkSolverPos_recursive(tmp)); 216 m_iterations = get_node()->get_parameter("solver.iterations").as_int(); 217 m_error_scale = get_node()->get_parameter("solver.error_scale").as_double(); 218 219 // Initialize Cartesian pd controllers 220 m_spatial_controller.init(get_node()); 221 222 // Check command interfaces. 223 // We support position, velocity, or both. 224 m_cmd_interface_types = get_node()->get_parameter("command_interfaces").as_string_array(); 225 if (m_cmd_interface_types.empty()) 226 { 227 RCLCPP_ERROR(get_node()->get_logger(), "No command_interfaces specified"); 228 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 229 } 230 for (const auto & type : m_cmd_interface_types) 231 { 232 if (type != hardware_interface::HW_IF_POSITION && type != hardware_interface::HW_IF_VELOCITY) 233 { 234 RCLCPP_ERROR(get_node()->get_logger(), 235 "Unsupported command interface: %s. Choose position or velocity", type.c_str()); 236 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 237 } 238 } 239 240 // Controller-internal state publishing 241 m_feedback_pose_publisher = 242 std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped>>( 243 get_node()->create_publisher<geometry_msgs::msg::PoseStamped>( 244 std::string(get_node()->get_name()) + "/current_pose", 3)); 245 246 m_feedback_twist_publisher = 247 std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::TwistStamped>>( 248 get_node()->create_publisher<geometry_msgs::msg::TwistStamped>( 249 std::string(get_node()->get_name()) + "/current_twist", 3)); 250 251 m_configured = true; 252 253 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 254 } 255 256 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 257 CartesianControllerBase::on_deactivate(const rclcpp_lifecycle::State & previous_state) 258 { 259 stopCurrentMotion(); 260 261 if (m_active) 262 { 263 m_joint_cmd_pos_handles.clear(); 264 m_joint_cmd_vel_handles.clear(); 265 m_joint_state_pos_handles.clear(); 266 this->release_interfaces(); 267 m_active = false; 268 } 269 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 270 } 271 272 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 273 CartesianControllerBase::on_activate(const rclcpp_lifecycle::State & previous_state) 274 { 275 if (m_active) 276 { 277 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 278 } 279 280 // Get command handles. 281 for (const auto & type : m_cmd_interface_types) 282 { 283 if (!controller_interface::get_ordered_interfaces(command_interfaces_, m_joint_names, type, 284 (type == hardware_interface::HW_IF_POSITION) 285 ? m_joint_cmd_pos_handles 286 : m_joint_cmd_vel_handles)) 287 { 288 RCLCPP_ERROR(get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", 289 m_joint_names.size(), type.c_str(), 290 (type == hardware_interface::HW_IF_POSITION) ? m_joint_cmd_pos_handles.size() 291 : m_joint_cmd_vel_handles.size()); 292 return CallbackReturn::ERROR; 293 } 294 } 295 296 // Get state handles. 297 if (!controller_interface::get_ordered_interfaces(state_interfaces_, m_joint_names, 298 hardware_interface::HW_IF_POSITION, 299 m_joint_state_pos_handles)) 300 { 301 RCLCPP_ERROR(get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", 302 m_joint_names.size(), hardware_interface::HW_IF_POSITION, 303 m_joint_state_pos_handles.size()); 304 return CallbackReturn::ERROR; 305 } 306 307 // Copy joint state to internal simulation 308 if (!m_ik_solver->setStartState(m_joint_state_pos_handles)) 309 { 310 RCLCPP_ERROR(get_node()->get_logger(), "Could not set start state"); 311 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; 312 }; 313 m_ik_solver->updateKinematics(); 314 315 // Provide safe command buffers with starting where we are 316 computeJointControlCmds(ctrl::Vector6D::Zero(), rclcpp::Duration::from_seconds(0)); 317 writeJointControlCmds(); 318 319 m_active = true; 320 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 321 } 322 323 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 324 CartesianControllerBase::on_shutdown(const rclcpp_lifecycle::State & previous_state) 325 { 326 stopCurrentMotion(); 327 328 if (m_active) 329 { 330 m_joint_cmd_pos_handles.clear(); 331 m_joint_cmd_vel_handles.clear(); 332 m_joint_state_pos_handles.clear(); 333 this->release_interfaces(); 334 m_active = false; 335 } 336 return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; 337 } 338 339 void CartesianControllerBase::writeJointControlCmds() 340 { 341 if (get_node()->get_parameter("solver.publish_state_feedback").as_bool()) 342 { 343 publishStateFeedback(); 344 } 345 346 auto nan_in = [](const auto & values) -> bool 347 { 348 for (const auto & value : values) 349 { 350 if (std::isnan(value)) 351 { 352 return true; 353 } 354 } 355 return false; 356 }; 357 358 if (nan_in(m_simulated_joint_motion.positions) || nan_in(m_simulated_joint_motion.velocities)) 359 { 360 RCLCPP_ERROR( 361 get_node()->get_logger(), 362 "NaN detected in internal model. It's unlikely to recover from this. Shutting down."); 363 get_node()->shutdown(); 364 return; 365 } 366 367 // Write all available types. 368 for (const auto & type : m_cmd_interface_types) 369 { 370 if (type == hardware_interface::HW_IF_POSITION) 371 { 372 for (size_t i = 0; i < m_joint_names.size(); ++i) 373 { 374 m_joint_cmd_pos_handles[i].get().set_value(m_simulated_joint_motion.positions[i]); 375 } 376 } 377 if (type == hardware_interface::HW_IF_VELOCITY) 378 { 379 for (size_t i = 0; i < m_joint_names.size(); ++i) 380 { 381 m_joint_cmd_vel_handles[i].get().set_value(m_simulated_joint_motion.velocities[i]); 382 } 383 } 384 } 385 } 386 387 void CartesianControllerBase::computeJointControlCmds(const ctrl::Vector6D & error, 388 const rclcpp::Duration & period) 389 { 390 // PD controlled system input 391 m_error_scale = get_node()->get_parameter("solver.error_scale").as_double(); 392 m_cartesian_input = m_error_scale * m_spatial_controller(error, period); 393 394 // Simulate one step forward 395 m_simulated_joint_motion = m_ik_solver->getJointControlCmds(period, m_cartesian_input); 396 397 m_ik_solver->updateKinematics(); 398 } 399 400 ctrl::Vector6D CartesianControllerBase::displayInBaseLink(const ctrl::Vector6D & vector, 401 const std::string & from) 402 { 403 // Adjust format 404 KDL::Wrench wrench_kdl; 405 for (int i = 0; i < 6; ++i) 406 { 407 wrench_kdl(i) = vector[i]; 408 } 409 410 KDL::Frame transform_kdl; 411 m_forward_kinematics_solver->JntToCart(m_ik_solver->getPositions(), transform_kdl, from); 412 413 // Rotate into new reference frame 414 wrench_kdl = transform_kdl.M * wrench_kdl; 415 416 // Reassign 417 ctrl::Vector6D out; 418 for (int i = 0; i < 6; ++i) 419 { 420 out[i] = wrench_kdl(i); 421 } 422 423 return out; 424 } 425 426 ctrl::Matrix6D CartesianControllerBase::displayInBaseLink(const ctrl::Matrix6D & tensor, 427 const std::string & from) 428 { 429 // Get rotation to base 430 KDL::Frame R_kdl; 431 m_forward_kinematics_solver->JntToCart(m_ik_solver->getPositions(), R_kdl, from); 432 433 // Adjust format 434 ctrl::Matrix3D R; 435 R << R_kdl.M.data[0], R_kdl.M.data[1], R_kdl.M.data[2], R_kdl.M.data[3], R_kdl.M.data[4], 436 R_kdl.M.data[5], R_kdl.M.data[6], R_kdl.M.data[7], R_kdl.M.data[8]; 437 438 // Treat diagonal blocks as individual 2nd rank tensors. 439 // Display in base frame. 440 ctrl::Matrix6D tmp = ctrl::Matrix6D::Zero(); 441 tmp.topLeftCorner<3, 3>() = R * tensor.topLeftCorner<3, 3>() * R.transpose(); 442 tmp.bottomRightCorner<3, 3>() = R * tensor.bottomRightCorner<3, 3>() * R.transpose(); 443 444 return tmp; 445 } 446 447 ctrl::Vector6D CartesianControllerBase::displayInTipLink(const ctrl::Vector6D & vector, 448 const std::string & to) 449 { 450 // Adjust format 451 KDL::Wrench wrench_kdl; 452 for (int i = 0; i < 6; ++i) 453 { 454 wrench_kdl(i) = vector[i]; 455 } 456 457 KDL::Frame transform_kdl; 458 m_forward_kinematics_solver->JntToCart(m_ik_solver->getPositions(), transform_kdl, to); 459 460 // Rotate into new reference frame 461 wrench_kdl = transform_kdl.M.Inverse() * wrench_kdl; 462 463 // Reassign 464 ctrl::Vector6D out; 465 for (int i = 0; i < 6; ++i) 466 { 467 out[i] = wrench_kdl(i); 468 } 469 470 return out; 471 } 472 473 void CartesianControllerBase::publishStateFeedback() 474 { 475 // End-effector pose 476 auto pose = m_ik_solver->getEndEffectorPose(); 477 if (m_feedback_pose_publisher->trylock()) 478 { 479 m_feedback_pose_publisher->msg_.header.stamp = get_node()->now(); 480 m_feedback_pose_publisher->msg_.header.frame_id = m_robot_base_link; 481 m_feedback_pose_publisher->msg_.pose.position.x = pose.p.x(); 482 m_feedback_pose_publisher->msg_.pose.position.y = pose.p.y(); 483 m_feedback_pose_publisher->msg_.pose.position.z = pose.p.z(); 484 485 pose.M.GetQuaternion(m_feedback_pose_publisher->msg_.pose.orientation.x, 486 m_feedback_pose_publisher->msg_.pose.orientation.y, 487 m_feedback_pose_publisher->msg_.pose.orientation.z, 488 m_feedback_pose_publisher->msg_.pose.orientation.w); 489 490 m_feedback_pose_publisher->unlockAndPublish(); 491 } 492 493 // End-effector twist 494 auto twist = m_ik_solver->getEndEffectorVel(); 495 if (m_feedback_twist_publisher->trylock()) 496 { 497 m_feedback_twist_publisher->msg_.header.stamp = get_node()->now(); 498 m_feedback_twist_publisher->msg_.header.frame_id = m_robot_base_link; 499 m_feedback_twist_publisher->msg_.twist.linear.x = twist[0]; 500 m_feedback_twist_publisher->msg_.twist.linear.y = twist[1]; 501 m_feedback_twist_publisher->msg_.twist.linear.z = twist[2]; 502 m_feedback_twist_publisher->msg_.twist.angular.x = twist[3]; 503 m_feedback_twist_publisher->msg_.twist.angular.y = twist[4]; 504 m_feedback_twist_publisher->msg_.twist.angular.z = twist[5]; 505 506 m_feedback_twist_publisher->unlockAndPublish(); 507 } 508 } 509 510 } // namespace cartesian_controller_base