force_example_controller.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 Franka Emika GmbH
2 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
4 
5 #include <cmath>
6 #include <memory>
7 
10 #include <ros/ros.h>
11 
12 #include <franka/robot_state.h>
13 
15 
17  ros::NodeHandle& node_handle) {
18  std::vector<std::string> joint_names;
19  std::string arm_id;
20  ROS_WARN(
21  "ForceExampleController: Make sure your robot's endeffector is in contact "
22  "with a horizontal surface before starting the controller!");
23  if (!node_handle.getParam("arm_id", arm_id)) {
24  ROS_ERROR("ForceExampleController: Could not read parameter arm_id");
25  return false;
26  }
27  if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) {
28  ROS_ERROR(
29  "ForceExampleController: Invalid or no joint_names parameters provided, aborting "
30  "controller init!");
31  return false;
32  }
33 
34  auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
35  if (model_interface == nullptr) {
36  ROS_ERROR_STREAM("ForceExampleController: Error getting model interface from hardware");
37  return false;
38  }
39  try {
40  model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
41  model_interface->getHandle(arm_id + "_model"));
44  "ForceExampleController: Exception getting model handle from interface: " << ex.what());
45  return false;
46  }
47 
48  auto* state_interface = robot_hw->get<franka_hw::FrankaStateInterface>();
49  if (state_interface == nullptr) {
50  ROS_ERROR_STREAM("ForceExampleController: Error getting state interface from hardware");
51  return false;
52  }
53  try {
54  state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
55  state_interface->getHandle(arm_id + "_robot"));
58  "ForceExampleController: Exception getting state handle from interface: " << ex.what());
59  return false;
60  }
61 
62  auto* effort_joint_interface = robot_hw->get<hardware_interface::EffortJointInterface>();
63  if (effort_joint_interface == nullptr) {
64  ROS_ERROR_STREAM("ForceExampleController: Error getting effort joint interface from hardware");
65  return false;
66  }
67  for (size_t i = 0; i < 7; ++i) {
68  try {
69  joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i]));
71  ROS_ERROR_STREAM("ForceExampleController: Exception getting joint handles: " << ex.what());
72  return false;
73  }
74  }
75 
77  ros::NodeHandle("dynamic_reconfigure_desired_mass_param_node");
78  dynamic_server_desired_mass_param_ = std::make_unique<
79  dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>>(
80 
83  boost::bind(&ForceExampleController::desiredMassParamCallback, this, _1, _2));
84 
85  return true;
86 }
87 
89  franka::RobotState robot_state = state_handle_->getRobotState();
90  std::array<double, 7> gravity_array = model_handle_->getGravity();
91  Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
92  Eigen::Map<Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
93  // Bias correction for the current external torque
94  tau_ext_initial_ = tau_measured - gravity;
95  tau_error_.setZero();
96 }
97 
98 void ForceExampleController::update(const ros::Time& /*time*/, const ros::Duration& period) {
99  franka::RobotState robot_state = state_handle_->getRobotState();
100  std::array<double, 42> jacobian_array =
101  model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
102  std::array<double, 7> gravity_array = model_handle_->getGravity();
103  Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
104  Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
105  Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_J_d( // NOLINT (readability-identifier-naming)
106  robot_state.tau_J_d.data());
107  Eigen::Map<Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
108 
109  Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7);
110  desired_force_torque.setZero();
111  desired_force_torque(2) = desired_mass_ * -9.81;
112  tau_ext = tau_measured - gravity - tau_ext_initial_;
113  tau_d << jacobian.transpose() * desired_force_torque;
114  tau_error_ = tau_error_ + period.toSec() * (tau_d - tau_ext);
115  // FF + PI control (PI gains are initially all 0)
116  tau_cmd = tau_d + k_p_ * (tau_d - tau_ext) + k_i_ * tau_error_;
117  tau_cmd << saturateTorqueRate(tau_cmd, tau_J_d);
118 
119  for (size_t i = 0; i < 7; ++i) {
120  joint_handles_[i].setCommand(tau_cmd(i));
121  }
122 
123  // Update signals changed online through dynamic reconfigure
125  k_p_ = filter_gain_ * target_k_p_ + (1 - filter_gain_) * k_p_;
126  k_i_ = filter_gain_ * target_k_i_ + (1 - filter_gain_) * k_i_;
127 }
128 
130  franka_example_controllers::desired_mass_paramConfig& config,
131  uint32_t /*level*/) {
132  target_mass_ = config.desired_mass;
133  target_k_p_ = config.k_p;
134  target_k_i_ = config.k_i;
135 }
136 
137 Eigen::Matrix<double, 7, 1> ForceExampleController::saturateTorqueRate(
138  const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
139  const Eigen::Matrix<double, 7, 1>& tau_J_d) { // NOLINT (readability-identifier-naming)
140  Eigen::Matrix<double, 7, 1> tau_d_saturated{};
141  for (size_t i = 0; i < 7; i++) {
142  double difference = tau_d_calculated[i] - tau_J_d[i];
143  tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax);
144  }
145  return tau_d_saturated;
146 }
147 
148 } // namespace franka_example_controllers
149 
std::string arm_id
bool init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &node_handle) override
Eigen::Matrix< double, 7, 1 > saturateTorqueRate(const Eigen::Matrix< double, 7, 1 > &tau_d_calculated, const Eigen::Matrix< double, 7, 1 > &tau_J_d)
std::array< std::string, 7 > joint_names
#define ROS_WARN(...)
void desiredMassParamCallback(franka_example_controllers::desired_mass_paramConfig &config, uint32_t level)
std::vector< hardware_interface::JointHandle > joint_handles_
std::unique_ptr< dynamic_reconfigure::Server< franka_example_controllers::desired_mass_paramConfig > > dynamic_server_desired_mass_param_
std::unique_ptr< franka_hw::FrankaModelHandle > model_handle_
std::array< double, 7 > tau_J_d
bool getParam(const std::string &key, std::string &s) const
std::unique_ptr< franka_hw::FrankaStateHandle > state_handle_
PLUGINLIB_EXPORT_CLASS(moveit_fake_controller_manager::MoveItFakeControllerManager, moveit_controller_manager::MoveItControllerManager)
#define ROS_ERROR_STREAM(args)
#define ROS_ERROR(...)
std::array< double, 7 > tau_J
void update(const ros::Time &, const ros::Duration &period) override


franka_example_controllers
Author(s): Franka Emika GmbH
autogenerated on Fri Oct 23 2020 03:47:17