services.cpp

Go to the documentation of this file.

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <franka_hw/services.h>
 
#include <mutex>
 
namespace franka_hw {
 
void setupServices(franka::Robot& robot,
                   std::mutex& robot_mutex,
                   ros::NodeHandle& node_handle,
                   ServiceContainer& services) {
  services
      .advertiseService<franka_msgs::SetJointImpedance>(
          node_handle, "set_joint_impedance",
          [&robot, &robot_mutex](auto&& req, auto&& res) {
            std::lock_guard<std::mutex> lock(robot_mutex);
            return franka_hw::setJointImpedance(robot, req, res);
          })
      .advertiseService<franka_msgs::SetCartesianImpedance>(
          node_handle, "set_cartesian_impedance",
          [&robot, &robot_mutex](auto&& req, auto&& res) {
            std::lock_guard<std::mutex> lock(robot_mutex);
            return franka_hw::setCartesianImpedance(robot, req, res);
          })
      .advertiseService<franka_msgs::SetEEFrame>(node_handle, "set_EE_frame",
                                                 [&robot, &robot_mutex](auto&& req, auto&& res) {
                                                   std::lock_guard<std::mutex> lock(robot_mutex);
                                                   return franka_hw::setEEFrame(robot, req, res);
                                                 })
      .advertiseService<franka_msgs::SetKFrame>(node_handle, "set_K_frame",
                                                [&robot, &robot_mutex](auto&& req, auto&& res) {
                                                  std::lock_guard<std::mutex> lock(robot_mutex);
                                                  return franka_hw::setKFrame(robot, req, res);
                                                })
      .advertiseService<franka_msgs::SetForceTorqueCollisionBehavior>(
          node_handle, "set_force_torque_collision_behavior",
          [&robot, &robot_mutex](auto&& req, auto&& res) {
            std::lock_guard<std::mutex> lock(robot_mutex);
            return franka_hw::setForceTorqueCollisionBehavior(robot, req, res);
          })
      .advertiseService<franka_msgs::SetFullCollisionBehavior>(
          node_handle, "set_full_collision_behavior",
          [&robot, &robot_mutex](auto&& req, auto&& res) {
            std::lock_guard<std::mutex> lock(robot_mutex);
            return franka_hw::setFullCollisionBehavior(robot, req, res);
          })
      .advertiseService<franka_msgs::SetLoad>(node_handle, "set_load",
                                              [&robot, &robot_mutex](auto&& req, auto&& res) {
                                                std::lock_guard<std::mutex> lock(robot_mutex);
                                                return franka_hw::setLoad(robot, req, res);
                                              });
}
 
void setCartesianImpedance(franka::Robot& robot,
                           const franka_msgs::SetCartesianImpedance::Request& req,
                           franka_msgs::SetCartesianImpedance::Response& /* res */) {
  std::array<double, 6> cartesian_stiffness;
  std::copy(req.cartesian_stiffness.cbegin(), req.cartesian_stiffness.cend(),
            cartesian_stiffness.begin());
  robot.setCartesianImpedance(cartesian_stiffness);
}
 
void setJointImpedance(franka::Robot& robot,
                       const franka_msgs::SetJointImpedance::Request& req,
                       franka_msgs::SetJointImpedance::Response& /* res */) {
  std::array<double, 7> joint_stiffness;
  std::copy(req.joint_stiffness.cbegin(), req.joint_stiffness.cend(), joint_stiffness.begin());
  robot.setJointImpedance(joint_stiffness);
}
 
void setEEFrame(franka::Robot& robot,
                const franka_msgs::SetEEFrame::Request& req,
                franka_msgs::SetEEFrame::Response& /* res */) {
  std::array<double, 16> NE_T_EE;  // NOLINT [readability-identifier-naming]
  std::copy(req.NE_T_EE.cbegin(), req.NE_T_EE.cend(), NE_T_EE.begin());
  robot.setEE(NE_T_EE);
}
 
void setKFrame(franka::Robot& robot,
               const franka_msgs::SetKFrame::Request& req,
               franka_msgs::SetKFrame::Response& /* res */) {
  std::array<double, 16> EE_T_K;  // NOLINT [readability-identifier-naming]
  std::copy(req.EE_T_K.cbegin(), req.EE_T_K.cend(), EE_T_K.begin());
  robot.setK(EE_T_K);
}
 
void setForceTorqueCollisionBehavior(
    franka::Robot& robot,
    const franka_msgs::SetForceTorqueCollisionBehavior::Request& req,
    franka_msgs::SetForceTorqueCollisionBehavior::Response& /* res */
) {
  std::array<double, 7> lower_torque_thresholds_nominal;
  std::copy(req.lower_torque_thresholds_nominal.cbegin(),
            req.lower_torque_thresholds_nominal.cend(), lower_torque_thresholds_nominal.begin());
  std::array<double, 7> upper_torque_thresholds_nominal;
  std::copy(req.upper_torque_thresholds_nominal.cbegin(),
            req.upper_torque_thresholds_nominal.cend(), upper_torque_thresholds_nominal.begin());
  std::array<double, 6> lower_force_thresholds_nominal;
  std::copy(req.lower_force_thresholds_nominal.cbegin(), req.lower_force_thresholds_nominal.cend(),
            lower_force_thresholds_nominal.begin());
  std::array<double, 6> upper_force_thresholds_nominal;
  std::copy(req.upper_force_thresholds_nominal.cbegin(), req.upper_force_thresholds_nominal.cend(),
            upper_force_thresholds_nominal.begin());
 
  robot.setCollisionBehavior(lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
                             lower_force_thresholds_nominal, upper_force_thresholds_nominal);
}
 
void setFullCollisionBehavior(franka::Robot& robot,
                              const franka_msgs::SetFullCollisionBehavior::Request& req,
                              franka_msgs::SetFullCollisionBehavior::Response& /* res */) {
  std::array<double, 7> lower_torque_thresholds_acceleration;
  std::copy(req.lower_torque_thresholds_acceleration.cbegin(),
            req.lower_torque_thresholds_acceleration.cend(),
            lower_torque_thresholds_acceleration.begin());
  std::array<double, 7> upper_torque_thresholds_acceleration;
  std::copy(req.upper_torque_thresholds_acceleration.cbegin(),
            req.upper_torque_thresholds_acceleration.cend(),
            upper_torque_thresholds_acceleration.begin());
  std::array<double, 7> lower_torque_thresholds_nominal;
  std::copy(req.lower_torque_thresholds_nominal.cbegin(),
            req.lower_torque_thresholds_nominal.cend(), lower_torque_thresholds_nominal.begin());
  std::array<double, 7> upper_torque_thresholds_nominal;
  std::copy(req.upper_torque_thresholds_nominal.cbegin(),
            req.upper_torque_thresholds_nominal.cend(), upper_torque_thresholds_nominal.begin());
  std::array<double, 6> lower_force_thresholds_acceleration;
  std::copy(req.lower_force_thresholds_acceleration.cbegin(),
            req.lower_force_thresholds_acceleration.cend(),
            lower_force_thresholds_acceleration.begin());
  std::array<double, 6> upper_force_thresholds_acceleration;
  std::copy(req.upper_force_thresholds_acceleration.cbegin(),
            req.upper_force_thresholds_acceleration.cend(),
            upper_force_thresholds_acceleration.begin());
  std::array<double, 6> lower_force_thresholds_nominal;
  std::copy(req.lower_force_thresholds_nominal.cbegin(), req.lower_force_thresholds_nominal.cend(),
            lower_force_thresholds_nominal.begin());
  std::array<double, 6> upper_force_thresholds_nominal;
  std::copy(req.upper_force_thresholds_nominal.cbegin(), req.upper_force_thresholds_nominal.cend(),
            upper_force_thresholds_nominal.begin());
  robot.setCollisionBehavior(lower_torque_thresholds_acceleration,
                             upper_torque_thresholds_acceleration, lower_torque_thresholds_nominal,
                             upper_torque_thresholds_nominal, lower_force_thresholds_acceleration,
                             upper_force_thresholds_acceleration, lower_force_thresholds_nominal,
                             upper_force_thresholds_nominal);
}
 
void setLoad(franka::Robot& robot,
             const franka_msgs::SetLoad::Request& req,
             franka_msgs::SetLoad::Response& /* res */) {
  double mass(req.mass);
  std::array<double, 3> F_x_center_load;  // NOLINT [readability-identifier-naming]
  std::copy(req.F_x_center_load.cbegin(), req.F_x_center_load.cend(), F_x_center_load.begin());
  std::array<double, 9> load_inertia;
  std::copy(req.load_inertia.cbegin(), req.load_inertia.cend(), load_inertia.begin());
 
  robot.setLoad(mass, F_x_center_load, load_inertia);
}
 
}  // namespace franka_hw