franka_state_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
3 #include <franka_control/franka_state_controller.h>
4 
5 #include <cmath>
6 #include <memory>
7 #include <mutex>
8 #include <string>
9 
10 #include <franka/errors.h>
11 #include <franka_hw/franka_cartesian_command_interface.h>
12 #include <franka_msgs/Errors.h>
13 #include <hardware_interface/hardware_interface.h>
14 #include <pluginlib/class_list_macros.h>
15 #include <ros/ros.h>
16 #include <tf/tf.h>
17 #include <tf/transform_datatypes.h>
18 
19 namespace {
20 
21 tf::Transform convertArrayToTf(const std::array<double, 16>& transform) {
22  tf::Matrix3x3 rotation(transform[0], transform[4], transform[8], transform[1], transform[5],
23  transform[9], transform[2], transform[6], transform[10]);
24  tf::Vector3 translation(transform[12], transform[13], transform[14]);
25  return tf::Transform(rotation, translation);
26 }
27 
28 franka_msgs::Errors errorsToMessage(const franka::Errors& error) {
29  franka_msgs::Errors message;
30  message.joint_position_limits_violation =
31  static_cast<decltype(message.joint_position_limits_violation)>(
32  error.joint_position_limits_violation);
33  message.cartesian_position_limits_violation =
34  static_cast<decltype(message.cartesian_position_limits_violation)>(
35  error.cartesian_position_limits_violation);
36  message.self_collision_avoidance_violation =
37  static_cast<decltype(message.self_collision_avoidance_violation)>(
38  error.self_collision_avoidance_violation);
39  message.joint_velocity_violation =
40  static_cast<decltype(message.joint_velocity_violation)>(error.joint_velocity_violation);
41  message.cartesian_velocity_violation =
42  static_cast<decltype(message.cartesian_velocity_violation)>(
43  error.cartesian_velocity_violation);
44  message.force_control_safety_violation =
45  static_cast<decltype(message.force_control_safety_violation)>(
46  error.force_control_safety_violation);
47  message.joint_reflex = static_cast<decltype(message.joint_reflex)>(error.joint_reflex);
48  message.cartesian_reflex =
49  static_cast<decltype(message.cartesian_reflex)>(error.cartesian_reflex);
50  message.max_goal_pose_deviation_violation =
51  static_cast<decltype(message.max_goal_pose_deviation_violation)>(
52  error.max_goal_pose_deviation_violation);
53  message.max_path_pose_deviation_violation =
54  static_cast<decltype(message.max_path_pose_deviation_violation)>(
55  error.max_path_pose_deviation_violation);
56  message.cartesian_velocity_profile_safety_violation =
57  static_cast<decltype(message.cartesian_velocity_profile_safety_violation)>(
58  error.cartesian_velocity_profile_safety_violation);
59  message.joint_position_motion_generator_start_pose_invalid =
60  static_cast<decltype(message.joint_position_motion_generator_start_pose_invalid)>(
61  error.joint_position_motion_generator_start_pose_invalid);
62  message.joint_motion_generator_position_limits_violation =
63  static_cast<decltype(message.joint_motion_generator_position_limits_violation)>(
64  error.joint_motion_generator_position_limits_violation);
65  message.joint_motion_generator_velocity_limits_violation =
66  static_cast<decltype(message.joint_motion_generator_velocity_limits_violation)>(
67  error.joint_motion_generator_velocity_limits_violation);
68  message.joint_motion_generator_velocity_discontinuity =
69  static_cast<decltype(message.joint_motion_generator_velocity_discontinuity)>(
70  error.joint_motion_generator_velocity_discontinuity);
71  message.joint_motion_generator_acceleration_discontinuity =
72  static_cast<decltype(message.joint_motion_generator_acceleration_discontinuity)>(
73  error.joint_motion_generator_acceleration_discontinuity);
74  message.cartesian_position_motion_generator_start_pose_invalid =
75  static_cast<decltype(message.cartesian_position_motion_generator_start_pose_invalid)>(
76  error.cartesian_position_motion_generator_start_pose_invalid);
77  message.cartesian_motion_generator_elbow_limit_violation =
78  static_cast<decltype(message.cartesian_motion_generator_elbow_limit_violation)>(
79  error.cartesian_motion_generator_elbow_limit_violation);
80  message.cartesian_motion_generator_velocity_limits_violation =
81  static_cast<decltype(message.cartesian_motion_generator_velocity_limits_violation)>(
82  error.cartesian_motion_generator_velocity_limits_violation);
83  message.cartesian_motion_generator_velocity_discontinuity =
84  static_cast<decltype(message.cartesian_motion_generator_velocity_discontinuity)>(
85  error.cartesian_motion_generator_velocity_discontinuity);
86  message.cartesian_motion_generator_acceleration_discontinuity =
87  static_cast<decltype(message.cartesian_motion_generator_acceleration_discontinuity)>(
88  error.cartesian_motion_generator_acceleration_discontinuity);
89  message.cartesian_motion_generator_elbow_sign_inconsistent =
90  static_cast<decltype(message.cartesian_motion_generator_elbow_sign_inconsistent)>(
91  error.cartesian_motion_generator_elbow_sign_inconsistent);
92  message.cartesian_motion_generator_start_elbow_invalid =
93  static_cast<decltype(message.cartesian_motion_generator_start_elbow_invalid)>(
94  error.cartesian_motion_generator_start_elbow_invalid);
95  message.cartesian_motion_generator_joint_position_limits_violation =
96  static_cast<decltype(message.cartesian_motion_generator_joint_position_limits_violation)>(
97  error.cartesian_motion_generator_joint_position_limits_violation);
98  message.cartesian_motion_generator_joint_velocity_limits_violation =
99  static_cast<decltype(message.cartesian_motion_generator_joint_velocity_limits_violation)>(
100  error.cartesian_motion_generator_joint_velocity_limits_violation);
101  message.cartesian_motion_generator_joint_velocity_discontinuity =
102  static_cast<decltype(message.cartesian_motion_generator_joint_velocity_discontinuity)>(
103  error.cartesian_motion_generator_joint_velocity_discontinuity);
104  message.cartesian_motion_generator_joint_acceleration_discontinuity =
105  static_cast<decltype(message.cartesian_motion_generator_joint_acceleration_discontinuity)>(
106  error.cartesian_motion_generator_joint_acceleration_discontinuity);
107  message.cartesian_position_motion_generator_invalid_frame =
108  static_cast<decltype(message.cartesian_position_motion_generator_invalid_frame)>(
109  error.cartesian_position_motion_generator_invalid_frame);
110  message.force_controller_desired_force_tolerance_violation =
111  static_cast<decltype(message.force_controller_desired_force_tolerance_violation)>(
112  error.force_controller_desired_force_tolerance_violation);
113  message.controller_torque_discontinuity =
114  static_cast<decltype(message.controller_torque_discontinuity)>(
115  error.controller_torque_discontinuity);
116  message.start_elbow_sign_inconsistent =
117  static_cast<decltype(message.start_elbow_sign_inconsistent)>(
118  error.start_elbow_sign_inconsistent);
119  message.communication_constraints_violation =
120  static_cast<decltype(message.communication_constraints_violation)>(
121  error.communication_constraints_violation);
122  message.power_limit_violation =
123  static_cast<decltype(message.power_limit_violation)>(error.power_limit_violation);
124  message.joint_p2p_insufficient_torque_for_planning =
125  static_cast<decltype(message.joint_p2p_insufficient_torque_for_planning)>(
126  error.joint_p2p_insufficient_torque_for_planning);
127  message.tau_j_range_violation =
128  static_cast<decltype(message.tau_j_range_violation)>(error.tau_j_range_violation);
129  message.instability_detected =
130  static_cast<decltype(message.instability_detected)>(error.instability_detected);
131  return message;
132 }
133 
134 } // anonymous namespace
135 
136 namespace franka_control {
137 
138 bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
139  ros::NodeHandle& root_node_handle,
140  ros::NodeHandle& controller_node_handle) {
141  franka_state_interface_ = robot_hardware->get<franka_hw::FrankaStateInterface>();
142  if (franka_state_interface_ == nullptr) {
143  ROS_ERROR("FrankaStateController: Could not get Franka state interface from hardware");
144  return false;
145  }
146  if (!controller_node_handle.getParam("arm_id", arm_id_)) {
147  ROS_ERROR("FrankaStateController: Could not get parameter arm_id");
148  return false;
149  }
150  double publish_rate(30.0);
151  if (!controller_node_handle.getParam("publish_rate", publish_rate)) {
152  ROS_INFO_STREAM("FrankaStateController: Did not find publish_rate. Using default "
153  << publish_rate << " [Hz].");
154  }
155  trigger_publish_ = franka_hw::TriggerRate(publish_rate);
156 
157  if (!controller_node_handle.getParam("joint_names", joint_names_) ||
158  joint_names_.size() != robot_state_.q.size()) {
159  ROS_ERROR(
160  "FrankaStateController: Invalid or no joint_names parameters provided, aborting "
161  "controller init!");
162  return false;
163  }
164 
165  try {
166  franka_state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
167  franka_state_interface_->getHandle(arm_id_ + "_robot"));
168  } catch (const hardware_interface::HardwareInterfaceException& ex) {
169  ROS_ERROR_STREAM("FrankaStateController: Exception getting franka state handle: " << ex.what());
170  return false;
171  }
172 
173  publisher_transforms_.init(root_node_handle, "/tf", 1);
174  publisher_franka_states_.init(controller_node_handle, "franka_states", 1);
175  publisher_joint_states_.init(controller_node_handle, "joint_states", 1);
176  publisher_joint_states_desired_.init(controller_node_handle, "joint_states_desired", 1);
177  publisher_external_wrench_.init(controller_node_handle, "F_ext", 1);
178 
179  {
180  std::lock_guard<realtime_tools::RealtimePublisher<sensor_msgs::JointState>> lock(
181  publisher_joint_states_);
182  publisher_joint_states_.msg_.name.resize(joint_names_.size());
183  publisher_joint_states_.msg_.position.resize(robot_state_.q.size());
184  publisher_joint_states_.msg_.velocity.resize(robot_state_.dq.size());
185  publisher_joint_states_.msg_.effort.resize(robot_state_.tau_J.size());
186  }
187  {
188  std::lock_guard<realtime_tools::RealtimePublisher<sensor_msgs::JointState>> lock(
189  publisher_joint_states_desired_);
190  publisher_joint_states_desired_.msg_.name.resize(joint_names_.size());
191  publisher_joint_states_desired_.msg_.position.resize(robot_state_.q_d.size());
192  publisher_joint_states_desired_.msg_.velocity.resize(robot_state_.dq_d.size());
193  publisher_joint_states_desired_.msg_.effort.resize(robot_state_.tau_J_d.size());
194  }
195  {
196  std::lock_guard<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>> lock(
197  publisher_transforms_);
198  publisher_transforms_.msg_.transforms.resize(2);
199  tf::Quaternion quaternion(0.0, 0.0, 0.0, 1.0);
200  tf::Vector3 translation(0.0, 0.0, 0.05);
201  tf::Transform transform(quaternion, translation);
202  tf::StampedTransform stamped_transform(transform, ros::Time::now(), arm_id_ + "_link8",
203  arm_id_ + "_EE");
204  geometry_msgs::TransformStamped transform_message;
205  transformStampedTFToMsg(stamped_transform, transform_message);
206  publisher_transforms_.msg_.transforms[0] = transform_message;
207  translation = tf::Vector3(0.0, 0.0, 0.0);
208  transform = tf::Transform(quaternion, translation);
209  stamped_transform =
210  tf::StampedTransform(transform, ros::Time::now(), arm_id_ + "_EE", arm_id_ + "_K");
211  transformStampedTFToMsg(stamped_transform, transform_message);
212  publisher_transforms_.msg_.transforms[1] = transform_message;
213  }
214  {
215  std::lock_guard<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped>> lock(
216  publisher_external_wrench_);
217  publisher_external_wrench_.msg_.header.frame_id = arm_id_ + "_K";
218  publisher_external_wrench_.msg_.wrench.force.x = 0.0;
219  publisher_external_wrench_.msg_.wrench.force.y = 0.0;
220  publisher_external_wrench_.msg_.wrench.force.z = 0.0;
221  publisher_external_wrench_.msg_.wrench.torque.x = 0.0;
222  publisher_external_wrench_.msg_.wrench.torque.y = 0.0;
223  publisher_external_wrench_.msg_.wrench.torque.z = 0.0;
224  }
225  return true;
226 }
227 
228 void FrankaStateController::update(const ros::Time& time, const ros::Duration& /* period */) {
229  if (trigger_publish_()) {
230  robot_state_ = franka_state_handle_->getRobotState();
231  publishFrankaStates(time);
232  publishTransforms(time);
233  publishExternalWrench(time);
234  publishJointStates(time);
235  sequence_number_++;
236  }
237 }
238 
239 void FrankaStateController::publishFrankaStates(const ros::Time& time) {
240  if (publisher_franka_states_.trylock()) {
241  static_assert(
242  sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.cartesian_contact),
243  "Robot state Cartesian members do not have same size");
244  static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.K_F_ext_hat_K),
245  "Robot state Cartesian members do not have same size");
246  static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_F_ext_hat_K),
247  "Robot state Cartesian members do not have same size");
248  static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_dP_EE_d),
249  "Robot state Cartesian members do not have same size");
250  static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_dP_EE_c),
251  "Robot state Cartesian members do not have same size");
252  static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_ddP_EE_c),
253  "Robot state Cartesian members do not have same size");
254  for (size_t i = 0; i < robot_state_.cartesian_collision.size(); i++) {
255  publisher_franka_states_.msg_.cartesian_collision[i] = robot_state_.cartesian_collision[i];
256  publisher_franka_states_.msg_.cartesian_contact[i] = robot_state_.cartesian_contact[i];
257  publisher_franka_states_.msg_.K_F_ext_hat_K[i] = robot_state_.K_F_ext_hat_K[i];
258  publisher_franka_states_.msg_.O_F_ext_hat_K[i] = robot_state_.O_F_ext_hat_K[i];
259  publisher_franka_states_.msg_.O_dP_EE_d[i] = robot_state_.O_dP_EE_d[i];
260  publisher_franka_states_.msg_.O_dP_EE_c[i] = robot_state_.O_dP_EE_c[i];
261  publisher_franka_states_.msg_.O_ddP_EE_c[i] = robot_state_.O_ddP_EE_c[i];
262  }
263 
264  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.q_d),
265  "Robot state joint members do not have same size");
266  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq),
267  "Robot state joint members do not have same size");
268  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq_d),
269  "Robot state joint members do not have same size");
270  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.ddq_d),
271  "Robot state joint members do not have same size");
272  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J),
273  "Robot state joint members do not have same size");
274  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtau_J),
275  "Robot state joint members do not have same size");
276  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J_d),
277  "Robot state joint members do not have same size");
278  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.theta),
279  "Robot state joint members do not have same size");
280  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtheta),
281  "Robot state joint members do not have same size");
282  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_collision),
283  "Robot state joint members do not have same size");
284  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_contact),
285  "Robot state joint members do not have same size");
286  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_ext_hat_filtered),
287  "Robot state joint members do not have same size");
288  for (size_t i = 0; i < robot_state_.q.size(); i++) {
289  publisher_franka_states_.msg_.q[i] = robot_state_.q[i];
290  publisher_franka_states_.msg_.q_d[i] = robot_state_.q_d[i];
291  publisher_franka_states_.msg_.dq[i] = robot_state_.dq[i];
292  publisher_franka_states_.msg_.dq_d[i] = robot_state_.dq_d[i];
293  publisher_franka_states_.msg_.ddq_d[i] = robot_state_.ddq_d[i];
294  publisher_franka_states_.msg_.tau_J[i] = robot_state_.tau_J[i];
295  publisher_franka_states_.msg_.dtau_J[i] = robot_state_.dtau_J[i];
296  publisher_franka_states_.msg_.tau_J_d[i] = robot_state_.tau_J_d[i];
297  publisher_franka_states_.msg_.theta[i] = robot_state_.theta[i];
298  publisher_franka_states_.msg_.dtheta[i] = robot_state_.dtheta[i];
299  publisher_franka_states_.msg_.joint_collision[i] = robot_state_.joint_collision[i];
300  publisher_franka_states_.msg_.joint_contact[i] = robot_state_.joint_contact[i];
301  publisher_franka_states_.msg_.tau_ext_hat_filtered[i] = robot_state_.tau_ext_hat_filtered[i];
302  }
303 
304  static_assert(sizeof(robot_state_.elbow) == sizeof(robot_state_.elbow_d),
305  "Robot state elbow configuration members do not have same size");
306  static_assert(sizeof(robot_state_.elbow) == sizeof(robot_state_.elbow_c),
307  "Robot state elbow configuration members do not have same size");
308  static_assert(sizeof(robot_state_.elbow) == sizeof(robot_state_.delbow_c),
309  "Robot state elbow configuration members do not have same size");
310  static_assert(sizeof(robot_state_.elbow) == sizeof(robot_state_.ddelbow_c),
311  "Robot state elbow configuration members do not have same size");
312 
313  for (size_t i = 0; i < robot_state_.elbow.size(); i++) {
314  publisher_franka_states_.msg_.elbow[i] = robot_state_.elbow[i];
315  publisher_franka_states_.msg_.elbow_d[i] = robot_state_.elbow_d[i];
316  publisher_franka_states_.msg_.elbow_c[i] = robot_state_.elbow_c[i];
317  publisher_franka_states_.msg_.delbow_c[i] = robot_state_.delbow_c[i];
318  publisher_franka_states_.msg_.ddelbow_c[i] = robot_state_.ddelbow_c[i];
319  }
320 
321  static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.F_T_EE),
322  "Robot state transforms do not have same size");
323  static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.EE_T_K),
324  "Robot state transforms do not have same size");
325  static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.O_T_EE_d),
326  "Robot state transforms do not have same size");
327  static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.O_T_EE_c),
328  "Robot state transforms do not have same size");
329  for (size_t i = 0; i < robot_state_.O_T_EE.size(); i++) {
330  publisher_franka_states_.msg_.O_T_EE[i] = robot_state_.O_T_EE[i];
331  publisher_franka_states_.msg_.F_T_EE[i] = robot_state_.F_T_EE[i];
332  publisher_franka_states_.msg_.EE_T_K[i] = robot_state_.EE_T_K[i];
333  publisher_franka_states_.msg_.O_T_EE_d[i] = robot_state_.O_T_EE_d[i];
334  publisher_franka_states_.msg_.O_T_EE_c[i] = robot_state_.O_T_EE_c[i];
335  }
336  publisher_franka_states_.msg_.m_ee = robot_state_.m_ee;
337  publisher_franka_states_.msg_.m_load = robot_state_.m_load;
338  publisher_franka_states_.msg_.m_total = robot_state_.m_total;
339 
340  for (size_t i = 0; i < robot_state_.I_load.size(); i++) {
341  publisher_franka_states_.msg_.I_ee[i] = robot_state_.I_ee[i];
342  publisher_franka_states_.msg_.I_load[i] = robot_state_.I_load[i];
343  publisher_franka_states_.msg_.I_total[i] = robot_state_.I_total[i];
344  }
345 
346  for (size_t i = 0; i < robot_state_.F_x_Cload.size(); i++) {
347  publisher_franka_states_.msg_.F_x_Cee[i] = robot_state_.F_x_Cee[i];
348  publisher_franka_states_.msg_.F_x_Cload[i] = robot_state_.F_x_Cload[i];
349  publisher_franka_states_.msg_.F_x_Ctotal[i] = robot_state_.F_x_Ctotal[i];
350  }
351 
352  publisher_franka_states_.msg_.time = robot_state_.time.toSec();
353  publisher_franka_states_.msg_.control_command_success_rate =
354  robot_state_.control_command_success_rate;
355  publisher_franka_states_.msg_.current_errors = errorsToMessage(robot_state_.current_errors);
356  publisher_franka_states_.msg_.last_motion_errors =
357  errorsToMessage(robot_state_.last_motion_errors);
358 
359  switch (robot_state_.robot_mode) {
360  case franka::RobotMode::kOther:
361  publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_OTHER;
362  break;
363 
364  case franka::RobotMode::kIdle:
365  publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_IDLE;
366  break;
367 
368  case franka::RobotMode::kMove:
369  publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_MOVE;
370  break;
371 
372  case franka::RobotMode::kGuiding:
373  publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_GUIDING;
374  break;
375 
376  case franka::RobotMode::kReflex:
377  publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_REFLEX;
378  break;
379 
380  case franka::RobotMode::kUserStopped:
381  publisher_franka_states_.msg_.robot_mode =
382  franka_msgs::FrankaState::ROBOT_MODE_USER_STOPPED;
383  break;
384 
385  case franka::RobotMode::kAutomaticErrorRecovery:
386  publisher_franka_states_.msg_.robot_mode =
387  franka_msgs::FrankaState::ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY;
388  break;
389  }
390 
391  publisher_franka_states_.msg_.header.seq = sequence_number_;
392  publisher_franka_states_.msg_.header.stamp = time;
393  publisher_franka_states_.unlockAndPublish();
394  }
395 }
396 
397 void FrankaStateController::publishJointStates(const ros::Time& time) {
398  if (publisher_joint_states_.trylock()) {
399  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq),
400  "Robot state joint members do not have same size");
401  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J),
402  "Robot state joint members do not have same size");
403  for (size_t i = 0; i < robot_state_.q.size(); i++) {
404  publisher_joint_states_.msg_.name[i] = joint_names_[i];
405  publisher_joint_states_.msg_.position[i] = robot_state_.q[i];
406  publisher_joint_states_.msg_.velocity[i] = robot_state_.dq[i];
407  publisher_joint_states_.msg_.effort[i] = robot_state_.tau_J[i];
408  }
409  publisher_joint_states_.msg_.header.stamp = time;
410  publisher_joint_states_.msg_.header.seq = sequence_number_;
411  publisher_joint_states_.unlockAndPublish();
412  }
413  if (publisher_joint_states_desired_.trylock()) {
414  static_assert(sizeof(robot_state_.q_d) == sizeof(robot_state_.dq_d),
415  "Robot state joint members do not have same size");
416  static_assert(sizeof(robot_state_.q_d) == sizeof(robot_state_.tau_J_d),
417  "Robot state joint members do not have same size");
418  for (size_t i = 0; i < robot_state_.q_d.size(); i++) {
419  publisher_joint_states_desired_.msg_.name[i] = joint_names_[i];
420  publisher_joint_states_desired_.msg_.position[i] = robot_state_.q_d[i];
421  publisher_joint_states_desired_.msg_.velocity[i] = robot_state_.dq_d[i];
422  publisher_joint_states_desired_.msg_.effort[i] = robot_state_.tau_J_d[i];
423  }
424  publisher_joint_states_desired_.msg_.header.stamp = time;
425  publisher_joint_states_desired_.msg_.header.seq = sequence_number_;
426  publisher_joint_states_desired_.unlockAndPublish();
427  }
428 }
429 
430 void FrankaStateController::publishTransforms(const ros::Time& time) {
431  if (publisher_transforms_.trylock()) {
432  tf::StampedTransform stamped_transform(convertArrayToTf(robot_state_.F_T_EE), time,
433  arm_id_ + "_link8", arm_id_ + "_EE");
434  geometry_msgs::TransformStamped transform_message;
435  transformStampedTFToMsg(stamped_transform, transform_message);
436  publisher_transforms_.msg_.transforms[0] = transform_message;
437  stamped_transform = tf::StampedTransform(convertArrayToTf(robot_state_.EE_T_K), time,
438  arm_id_ + "_EE", arm_id_ + "_K");
439  transformStampedTFToMsg(stamped_transform, transform_message);
440  publisher_transforms_.msg_.transforms[1] = transform_message;
441  publisher_transforms_.unlockAndPublish();
442  }
443 }
444 
445 void FrankaStateController::publishExternalWrench(const ros::Time& time) {
446  if (publisher_external_wrench_.trylock()) {
447  publisher_external_wrench_.msg_.header.frame_id = arm_id_ + "_K";
448  publisher_external_wrench_.msg_.header.stamp = time;
449  publisher_external_wrench_.msg_.wrench.force.x = robot_state_.K_F_ext_hat_K[0];
450  publisher_external_wrench_.msg_.wrench.force.y = robot_state_.K_F_ext_hat_K[1];
451  publisher_external_wrench_.msg_.wrench.force.z = robot_state_.K_F_ext_hat_K[2];
452  publisher_external_wrench_.msg_.wrench.torque.x = robot_state_.K_F_ext_hat_K[3];
453  publisher_external_wrench_.msg_.wrench.torque.y = robot_state_.K_F_ext_hat_K[4];
454  publisher_external_wrench_.msg_.wrench.torque.z = robot_state_.K_F_ext_hat_K[5];
455  publisher_external_wrench_.unlockAndPublish();
456  }
457 }
458 
459 } // namespace franka_control
460 
461 PLUGINLIB_EXPORT_CLASS(franka_control::FrankaStateController, controller_interface::ControllerBase)
franka::Errors::cartesian_motion_generator_joint_velocity_discontinuity
const bool & cartesian_motion_generator_joint_velocity_discontinuity
franka_control::FrankaStateController::franka_state_handle_
std::unique_ptr< franka_hw::FrankaStateHandle > franka_state_handle_
Definition: franka_state_controller.h:59
franka_control::FrankaStateController::publisher_joint_states_desired_
realtime_tools::RealtimePublisher< sensor_msgs::JointState > publisher_joint_states_desired_
Definition: franka_state_controller.h:64
franka::Errors::cartesian_motion_generator_elbow_limit_violation
const bool & cartesian_motion_generator_elbow_limit_violation
franka::Errors::cartesian_motion_generator_joint_velocity_limits_violation
const bool & cartesian_motion_generator_joint_velocity_limits_violation
franka::RobotState::F_T_EE
std::array< double, 16 > F_T_EE
hardware_interface::HardwareInterfaceException::what
virtual const char * what() const
franka_control::FrankaStateController::sequence_number_
uint64_t sequence_number_
Definition: franka_state_controller.h:68
franka::Errors::force_control_safety_violation
const bool & force_control_safety_violation
franka::RobotState::dtau_J
std::array< double, 7 > dtau_J
franka::RobotState::tau_ext_hat_filtered
std::array< double, 7 > tau_ext_hat_filtered
franka::Errors::controller_torque_discontinuity
const bool & controller_torque_discontinuity
realtime_tools::RealtimePublisher::msg_
Msg msg_
ros::NodeHandle
franka::Duration::toSec
double toSec() const noexcept
franka::RobotState::F_x_Cload
std::array< double, 3 > F_x_Cload
Msg::header
Header header
franka_control::FrankaStateController::publishJointStates
void publishJointStates(const ros::Time &time)
Definition: franka_state_controller.cpp:397
franka::Errors::joint_motion_generator_velocity_discontinuity
const bool & joint_motion_generator_velocity_discontinuity
franka::RobotState::F_x_Ctotal
std::array< double, 3 > F_x_Ctotal
franka::Errors::communication_constraints_violation
const bool & communication_constraints_violation
franka::Errors::cartesian_position_motion_generator_start_pose_invalid
const bool & cartesian_position_motion_generator_start_pose_invalid
franka::Errors::joint_reflex
const bool & joint_reflex
hardware_interface::HardwareInterfaceException
franka_control::FrankaStateController
Controller to publish the robot state as ROS topics.
Definition: franka_state_controller.h:23
franka_control::FrankaStateController::trigger_publish_
franka_hw::TriggerRate trigger_publish_
Definition: franka_state_controller.h:66
franka::Errors::self_collision_avoidance_violation
const bool & self_collision_avoidance_violation
franka::Errors::cartesian_motion_generator_start_elbow_invalid
const bool & cartesian_motion_generator_start_elbow_invalid
franka::Errors::cartesian_velocity_violation
const bool & cartesian_velocity_violation
franka_control::FrankaStateController::update
void update(const ros::Time &time, const ros::Duration &period) override
Reads the current robot state from the franka_hw::FrankaStateInterface and publishes it...
Definition: franka_state_controller.cpp:228
realtime_tools::RealtimePublisher::unlockAndPublish
void unlockAndPublish()
franka::RobotState::elbow_c
std::array< double, 2 > elbow_c
franka_hw::FrankaStateInterface
franka::Errors::cartesian_motion_generator_velocity_limits_violation
const bool & cartesian_motion_generator_velocity_limits_violation
franka::RobotState::q_d
std::array< double, 7 > q_d
franka_control::FrankaStateController::robot_state_
franka::RobotState robot_state_
Definition: franka_state_controller.h:67
franka::RobotState::K_F_ext_hat_K
std::array< double, 6 > K_F_ext_hat_K
franka::RobotState::O_F_ext_hat_K
std::array< double, 6 > O_F_ext_hat_K
franka::RobotState::O_T_EE_d
std::array< double, 16 > O_T_EE_d
franka::Errors::joint_position_motion_generator_start_pose_invalid
const bool & joint_position_motion_generator_start_pose_invalid
transform_datatypes.h
realtime_tools::RealtimePublisher::trylock
bool trylock()
franka::RobotState::F_x_Cee
std::array< double, 3 > F_x_Cee
franka::RobotState::joint_contact
std::array< double, 7 > joint_contact
franka::Errors::joint_motion_generator_position_limits_violation
const bool & joint_motion_generator_position_limits_violation
franka_control::FrankaStateController::publishFrankaStates
void publishFrankaStates(const ros::Time &time)
Definition: franka_state_controller.cpp:239
franka::Errors::cartesian_motion_generator_joint_position_limits_violation
const bool & cartesian_motion_generator_joint_position_limits_violation
franka::RobotState::delbow_c
std::array< double, 2 > delbow_c
franka::RobotState::q
std::array< double, 7 > q
franka::Errors::cartesian_motion_generator_velocity_discontinuity
const bool & cartesian_motion_generator_velocity_discontinuity
franka::Errors::cartesian_motion_generator_joint_acceleration_discontinuity
const bool & cartesian_motion_generator_joint_acceleration_discontinuity
franka::RobotState::dtheta
std::array< double, 7 > dtheta
controller_interface::ControllerBase
franka_control::FrankaStateController::publishTransforms
void publishTransforms(const ros::Time &time)
Definition: franka_state_controller.cpp:430
franka_control::FrankaStateController::joint_names_
std::vector< std::string > joint_names_
Definition: franka_state_controller.h:69
franka_state_controller.h
class_list_macros.h
franka::Errors::max_path_pose_deviation_violation
const bool & max_path_pose_deviation_violation
franka::Errors::cartesian_position_limits_violation
const bool & cartesian_position_limits_violation
franka::Errors::cartesian_position_motion_generator_invalid_frame
const bool & cartesian_position_motion_generator_invalid_frame
franka::Errors::cartesian_motion_generator_acceleration_discontinuity
const bool & cartesian_motion_generator_acceleration_discontinuity
hardware_interface::InterfaceManager::get
T * get()
franka::RobotState::O_T_EE_c
std::array< double, 16 > O_T_EE_c
franka::Errors::cartesian_velocity_profile_safety_violation
const bool & cartesian_velocity_profile_safety_violation
franka::RobotState::dq
std::array< double, 7 > dq
franka::Errors::joint_p2p_insufficient_torque_for_planning
const bool & joint_p2p_insufficient_torque_for_planning
franka::Errors::max_goal_pose_deviation_violation
const bool & max_goal_pose_deviation_violation
franka::RobotState::O_T_EE
std::array< double, 16 > O_T_EE
message
def message(msg, args, kwargs)
franka::Errors::joint_motion_generator_acceleration_discontinuity
const bool & joint_motion_generator_acceleration_discontinuity
franka::RobotState::I_load
std::array< double, 9 > I_load
franka::RobotState::dq_d
std::array< double, 7 > dq_d
franka_control::FrankaStateController::publishExternalWrench
void publishExternalWrench(const ros::Time &time)
Definition: franka_state_controller.cpp:445
franka::Errors::joint_position_limits_violation
const bool & joint_position_limits_violation
franka::Errors::force_controller_desired_force_tolerance_violation
const bool & force_controller_desired_force_tolerance_violation
franka_hw::TriggerRate
franka_control::FrankaStateController::arm_id_
std::string arm_id_
Definition: franka_state_controller.h:56
franka_control::FrankaStateController::init
bool init(hardware_interface::RobotHW *robot_hardware, ros::NodeHandle &root_node_handle, ros::NodeHandle &controller_node_handle) override
Initializes the controller with interfaces and publishers.
Definition: franka_state_controller.cpp:138
franka::RobotState::O_ddP_EE_c
std::array< double, 6 > O_ddP_EE_c
franka_control::FrankaStateController::publisher_joint_states_
realtime_tools::RealtimePublisher< sensor_msgs::JointState > publisher_joint_states_
Definition: franka_state_controller.h:63
franka::RobotState::O_dP_EE_c
std::array< double, 6 > O_dP_EE_c
franka::RobotState::ddelbow_c
std::array< double, 2 > ddelbow_c
franka::RobotState::joint_collision
std::array< double, 7 > joint_collision
realtime_tools::RealtimePublisher::init
void init(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
franka::Errors::power_limit_violation
const bool & power_limit_violation
franka::RobotState::ddq_d
std::array< double, 7 > ddq_d
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
franka_control::FrankaStateController::publisher_transforms_
realtime_tools::RealtimePublisher< tf2_msgs::TFMessage > publisher_transforms_
Definition: franka_state_controller.h:61
franka::RobotState::tau_J_d
std::array< double, 7 > tau_J_d
franka::Errors::cartesian_reflex
const bool & cartesian_reflex
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
franka_cartesian_command_interface.h
franka::RobotState::I_total
std::array< double, 9 > I_total
franka::RobotState::elbow_d
std::array< double, 2 > elbow_d
franka::RobotState::cartesian_contact
std::array< double, 6 > cartesian_contact
franka::RobotState::robot_mode
RobotMode robot_mode
ros::Time::now
static Time now()
franka::Errors::joint_motion_generator_velocity_limits_violation
const bool & joint_motion_generator_velocity_limits_violation
hardware_interface.h
franka::Errors::instability_detected
const bool & instability_detected
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
franka::RobotState::I_ee
std::array< double, 9 > I_ee
hardware_interface::RobotHW
franka_control::FrankaStateController::franka_state_interface_
franka_hw::FrankaStateInterface * franka_state_interface_
Definition: franka_state_controller.h:58
franka::RobotState::control_command_success_rate
double control_command_success_rate
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
franka::Errors::joint_velocity_violation
const bool & joint_velocity_violation
franka_control::FrankaStateController::publisher_franka_states_
realtime_tools::RealtimePublisher< franka_msgs::FrankaState > publisher_franka_states_
Definition: franka_state_controller.h:62
franka::Errors::cartesian_motion_generator_elbow_sign_inconsistent
const bool & cartesian_motion_generator_elbow_sign_inconsistent
ROS_ERROR
#define ROS_ERROR(...)
franka::RobotState::O_dP_EE_d
std::array< double, 6 > O_dP_EE_d
franka::RobotState::tau_J
std::array< double, 7 > tau_J
franka::Errors::start_elbow_sign_inconsistent
const bool & start_elbow_sign_inconsistent
franka::Errors::tau_j_range_violation
const bool & tau_j_range_violation
franka::RobotState::elbow
std::array< double, 2 > elbow
franka_control
Definition: franka_state_controller.h:18
franka_control::FrankaStateController::publisher_external_wrench_
realtime_tools::RealtimePublisher< geometry_msgs::WrenchStamped > publisher_external_wrench_
Definition: franka_state_controller.h:65
franka::RobotState::EE_T_K
std::array< double, 16 > EE_T_K
Header::stamp
ros::Time stamp
franka::RobotState::cartesian_collision
std::array< double, 6 > cartesian_collision
franka::RobotState::theta
std::array< double, 7 > theta

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