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  message.joint_move_in_wrong_direction =
132  static_cast<decltype(message.joint_move_in_wrong_direction)>(
133  error.joint_move_in_wrong_direction);
134 #ifdef ENABLE_BASE_ACCELERATION
135  message.cartesian_spline_motion_generator_violation =
136  static_cast<decltype(message.cartesian_spline_motion_generator_violation)>(
137  error.cartesian_spline_motion_generator_violation);
138  message.joint_via_motion_generator_planning_joint_limit_violation =
139  static_cast<decltype(message.joint_via_motion_generator_planning_joint_limit_violation)>(
140  error.joint_via_motion_generator_planning_joint_limit_violation);
141  message.base_acceleration_initialization_timeout =
142  static_cast<decltype(message.base_acceleration_initialization_timeout)>(
143  error.base_acceleration_initialization_timeout);
144  message.base_acceleration_invalid_reading =
145  static_cast<decltype(message.base_acceleration_invalid_reading)>(
146  error.base_acceleration_invalid_reading);
147 #endif
148  return message;
149 }
150 
151 } // anonymous namespace
152 
153 namespace franka_control {
154 
155 bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
156  ros::NodeHandle& root_node_handle,
157  ros::NodeHandle& controller_node_handle) {
158  franka_state_interface_ = robot_hardware->get<franka_hw::FrankaStateInterface>();
159  if (franka_state_interface_ == nullptr) {
160  ROS_ERROR("FrankaStateController: Could not get Franka state interface from hardware");
161  return false;
162  }
163  if (!controller_node_handle.getParam("arm_id", arm_id_)) {
164  ROS_ERROR("FrankaStateController: Could not get parameter arm_id");
165  return false;
166  }
167  double publish_rate(30.0);
168  if (!controller_node_handle.getParam("publish_rate", publish_rate)) {
169  ROS_INFO_STREAM("FrankaStateController: Did not find publish_rate. Using default "
170  << publish_rate << " [Hz].");
171  }
172  trigger_publish_ = franka_hw::TriggerRate(publish_rate);
173 
174  if (!controller_node_handle.getParam("joint_names", joint_names_) ||
175  joint_names_.size() != robot_state_.q.size()) {
176  ROS_ERROR(
177  "FrankaStateController: Invalid or no joint_names parameters provided, aborting "
178  "controller init!");
179  return false;
180  }
181 
182  try {
183  franka_state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
184  franka_state_interface_->getHandle(arm_id_ + "_robot"));
185  } catch (const hardware_interface::HardwareInterfaceException& ex) {
186  ROS_ERROR_STREAM("FrankaStateController: Exception getting franka state handle: " << ex.what());
187  return false;
188  }
189 
190  publisher_transforms_.init(root_node_handle, "/tf", 1);
191  publisher_franka_states_.init(controller_node_handle, "franka_states", 1);
192  publisher_joint_states_.init(controller_node_handle, "joint_states", 1);
193  publisher_joint_states_desired_.init(controller_node_handle, "joint_states_desired", 1);
194  publisher_external_wrench_.init(controller_node_handle, "F_ext", 1);
195 
196  {
197  std::lock_guard<realtime_tools::RealtimePublisher<sensor_msgs::JointState>> lock(
198  publisher_joint_states_);
199  publisher_joint_states_.msg_.name.resize(joint_names_.size());
200  publisher_joint_states_.msg_.position.resize(robot_state_.q.size());
201  publisher_joint_states_.msg_.velocity.resize(robot_state_.dq.size());
202  publisher_joint_states_.msg_.effort.resize(robot_state_.tau_J.size());
203  }
204  {
205  std::lock_guard<realtime_tools::RealtimePublisher<sensor_msgs::JointState>> lock(
206  publisher_joint_states_desired_);
207  publisher_joint_states_desired_.msg_.name.resize(joint_names_.size());
208  publisher_joint_states_desired_.msg_.position.resize(robot_state_.q_d.size());
209  publisher_joint_states_desired_.msg_.velocity.resize(robot_state_.dq_d.size());
210  publisher_joint_states_desired_.msg_.effort.resize(robot_state_.tau_J_d.size());
211  }
212  {
213  std::lock_guard<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>> lock(
214  publisher_transforms_);
215  publisher_transforms_.msg_.transforms.resize(3);
216  tf::Quaternion quaternion(0.0, 0.0, 0.0, 1.0);
217  tf::Vector3 translation(0.0, 0.0, 0.05);
218  tf::Transform transform(quaternion, translation);
219  tf::StampedTransform stamped_transform(transform, ros::Time::now(), arm_id_ + "_link8",
220  arm_id_ + "_NE");
221  geometry_msgs::TransformStamped transform_message;
222  transformStampedTFToMsg(stamped_transform, transform_message);
223  publisher_transforms_.msg_.transforms[0] = transform_message;
224 
225  translation = tf::Vector3(0.0, 0.0, 0.0);
226  transform = tf::Transform(quaternion, translation);
227  stamped_transform =
228  tf::StampedTransform(transform, ros::Time::now(), arm_id_ + "_NE", arm_id_ + "_EE");
229  transformStampedTFToMsg(stamped_transform, transform_message);
230  publisher_transforms_.msg_.transforms[1] = transform_message;
231 
232  stamped_transform =
233  tf::StampedTransform(transform, ros::Time::now(), arm_id_ + "_EE", arm_id_ + "_K");
234  transformStampedTFToMsg(stamped_transform, transform_message);
235  publisher_transforms_.msg_.transforms[2] = transform_message;
236  }
237  {
238  std::lock_guard<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped>> lock(
239  publisher_external_wrench_);
240  publisher_external_wrench_.msg_.header.frame_id = arm_id_ + "_K";
241  publisher_external_wrench_.msg_.wrench.force.x = 0.0;
242  publisher_external_wrench_.msg_.wrench.force.y = 0.0;
243  publisher_external_wrench_.msg_.wrench.force.z = 0.0;
244  publisher_external_wrench_.msg_.wrench.torque.x = 0.0;
245  publisher_external_wrench_.msg_.wrench.torque.y = 0.0;
246  publisher_external_wrench_.msg_.wrench.torque.z = 0.0;
247  }
248  return true;
249 }
250 
251 void FrankaStateController::update(const ros::Time& time, const ros::Duration& /* period */) {
252  if (trigger_publish_()) {
253  robot_state_ = franka_state_handle_->getRobotState();
254  publishFrankaStates(time);
255  publishTransforms(time);
256  publishExternalWrench(time);
257  publishJointStates(time);
258  sequence_number_++;
259  }
260 }
261 
262 void FrankaStateController::publishFrankaStates(const ros::Time& time) {
263  if (publisher_franka_states_.trylock()) {
264  static_assert(
265  sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.cartesian_contact),
266  "Robot state Cartesian members do not have same size");
267  static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.K_F_ext_hat_K),
268  "Robot state Cartesian members do not have same size");
269  static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_F_ext_hat_K),
270  "Robot state Cartesian members do not have same size");
271  static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_dP_EE_d),
272  "Robot state Cartesian members do not have same size");
273  static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_dP_EE_c),
274  "Robot state Cartesian members do not have same size");
275  static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_ddP_EE_c),
276  "Robot state Cartesian members do not have same size");
277  for (size_t i = 0; i < robot_state_.cartesian_collision.size(); i++) {
278  publisher_franka_states_.msg_.cartesian_collision[i] = robot_state_.cartesian_collision[i];
279  publisher_franka_states_.msg_.cartesian_contact[i] = robot_state_.cartesian_contact[i];
280  publisher_franka_states_.msg_.K_F_ext_hat_K[i] = robot_state_.K_F_ext_hat_K[i];
281  publisher_franka_states_.msg_.O_F_ext_hat_K[i] = robot_state_.O_F_ext_hat_K[i];
282  publisher_franka_states_.msg_.O_dP_EE_d[i] = robot_state_.O_dP_EE_d[i];
283  publisher_franka_states_.msg_.O_dP_EE_c[i] = robot_state_.O_dP_EE_c[i];
284  publisher_franka_states_.msg_.O_ddP_EE_c[i] = robot_state_.O_ddP_EE_c[i];
285  }
286 
287  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.q_d),
288  "Robot state joint members do not have same size");
289  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq),
290  "Robot state joint members do not have same size");
291  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq_d),
292  "Robot state joint members do not have same size");
293  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.ddq_d),
294  "Robot state joint members do not have same size");
295  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J),
296  "Robot state joint members do not have same size");
297  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtau_J),
298  "Robot state joint members do not have same size");
299  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J_d),
300  "Robot state joint members do not have same size");
301  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.theta),
302  "Robot state joint members do not have same size");
303  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtheta),
304  "Robot state joint members do not have same size");
305  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_collision),
306  "Robot state joint members do not have same size");
307  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_contact),
308  "Robot state joint members do not have same size");
309  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_ext_hat_filtered),
310  "Robot state joint members do not have same size");
311  for (size_t i = 0; i < robot_state_.q.size(); i++) {
312  publisher_franka_states_.msg_.q[i] = robot_state_.q[i];
313  publisher_franka_states_.msg_.q_d[i] = robot_state_.q_d[i];
314  publisher_franka_states_.msg_.dq[i] = robot_state_.dq[i];
315  publisher_franka_states_.msg_.dq_d[i] = robot_state_.dq_d[i];
316  publisher_franka_states_.msg_.ddq_d[i] = robot_state_.ddq_d[i];
317  publisher_franka_states_.msg_.tau_J[i] = robot_state_.tau_J[i];
318  publisher_franka_states_.msg_.dtau_J[i] = robot_state_.dtau_J[i];
319  publisher_franka_states_.msg_.tau_J_d[i] = robot_state_.tau_J_d[i];
320  publisher_franka_states_.msg_.theta[i] = robot_state_.theta[i];
321  publisher_franka_states_.msg_.dtheta[i] = robot_state_.dtheta[i];
322  publisher_franka_states_.msg_.joint_collision[i] = robot_state_.joint_collision[i];
323  publisher_franka_states_.msg_.joint_contact[i] = robot_state_.joint_contact[i];
324  publisher_franka_states_.msg_.tau_ext_hat_filtered[i] = robot_state_.tau_ext_hat_filtered[i];
325  }
326 
327  static_assert(sizeof(robot_state_.elbow) == sizeof(robot_state_.elbow_d),
328  "Robot state elbow configuration members do not have same size");
329  static_assert(sizeof(robot_state_.elbow) == sizeof(robot_state_.elbow_c),
330  "Robot state elbow configuration members do not have same size");
331  static_assert(sizeof(robot_state_.elbow) == sizeof(robot_state_.delbow_c),
332  "Robot state elbow configuration members do not have same size");
333  static_assert(sizeof(robot_state_.elbow) == sizeof(robot_state_.ddelbow_c),
334  "Robot state elbow configuration members do not have same size");
335 
336  for (size_t i = 0; i < robot_state_.elbow.size(); i++) {
337  publisher_franka_states_.msg_.elbow[i] = robot_state_.elbow[i];
338  publisher_franka_states_.msg_.elbow_d[i] = robot_state_.elbow_d[i];
339  publisher_franka_states_.msg_.elbow_c[i] = robot_state_.elbow_c[i];
340  publisher_franka_states_.msg_.delbow_c[i] = robot_state_.delbow_c[i];
341  publisher_franka_states_.msg_.ddelbow_c[i] = robot_state_.ddelbow_c[i];
342  }
343 
344  static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.F_T_EE),
345  "Robot state transforms do not have same size");
346  static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.F_T_NE),
347  "Robot state transforms do not have same size");
348  static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.NE_T_EE),
349  "Robot state transforms do not have same size");
350  static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.EE_T_K),
351  "Robot state transforms do not have same size");
352  static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.O_T_EE_d),
353  "Robot state transforms do not have same size");
354  static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.O_T_EE_c),
355  "Robot state transforms do not have same size");
356  for (size_t i = 0; i < robot_state_.O_T_EE.size(); i++) {
357  publisher_franka_states_.msg_.O_T_EE[i] = robot_state_.O_T_EE[i];
358  publisher_franka_states_.msg_.F_T_EE[i] = robot_state_.F_T_EE[i];
359  publisher_franka_states_.msg_.F_T_NE[i] = robot_state_.F_T_NE[i];
360  publisher_franka_states_.msg_.NE_T_EE[i] = robot_state_.NE_T_EE[i];
361  publisher_franka_states_.msg_.EE_T_K[i] = robot_state_.EE_T_K[i];
362  publisher_franka_states_.msg_.O_T_EE_d[i] = robot_state_.O_T_EE_d[i];
363  publisher_franka_states_.msg_.O_T_EE_c[i] = robot_state_.O_T_EE_c[i];
364  }
365  publisher_franka_states_.msg_.m_ee = robot_state_.m_ee;
366  publisher_franka_states_.msg_.m_load = robot_state_.m_load;
367  publisher_franka_states_.msg_.m_total = robot_state_.m_total;
368 
369  for (size_t i = 0; i < robot_state_.I_load.size(); i++) {
370  publisher_franka_states_.msg_.I_ee[i] = robot_state_.I_ee[i];
371  publisher_franka_states_.msg_.I_load[i] = robot_state_.I_load[i];
372  publisher_franka_states_.msg_.I_total[i] = robot_state_.I_total[i];
373  }
374 
375  for (size_t i = 0; i < robot_state_.F_x_Cload.size(); i++) {
376  publisher_franka_states_.msg_.F_x_Cee[i] = robot_state_.F_x_Cee[i];
377  publisher_franka_states_.msg_.F_x_Cload[i] = robot_state_.F_x_Cload[i];
378  publisher_franka_states_.msg_.F_x_Ctotal[i] = robot_state_.F_x_Ctotal[i];
379  }
380 #ifdef ENABLE_BASE_ACCELERATION
381  for (size_t i = 0; i < robot_state_.O_ddP_O.size(); i++) {
382  publisher_franka_states_.msg_.O_ddP_O[i] = robot_state_.O_ddP_O[i];
383  }
384 #else
385  publisher_franka_states_.msg_.O_ddP_O[2] = -9.81;
386 #endif
387 
388  publisher_franka_states_.msg_.time = robot_state_.time.toSec();
389  publisher_franka_states_.msg_.control_command_success_rate =
390  robot_state_.control_command_success_rate;
391  publisher_franka_states_.msg_.current_errors = errorsToMessage(robot_state_.current_errors);
392  publisher_franka_states_.msg_.last_motion_errors =
393  errorsToMessage(robot_state_.last_motion_errors);
394 
395  switch (robot_state_.robot_mode) {
396  case franka::RobotMode::kOther:
397  publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_OTHER;
398  break;
399 
400  case franka::RobotMode::kIdle:
401  publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_IDLE;
402  break;
403 
404  case franka::RobotMode::kMove:
405  publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_MOVE;
406  break;
407 
408  case franka::RobotMode::kGuiding:
409  publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_GUIDING;
410  break;
411 
412  case franka::RobotMode::kReflex:
413  publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_REFLEX;
414  break;
415 
416  case franka::RobotMode::kUserStopped:
417  publisher_franka_states_.msg_.robot_mode =
418  franka_msgs::FrankaState::ROBOT_MODE_USER_STOPPED;
419  break;
420 
421  case franka::RobotMode::kAutomaticErrorRecovery:
422  publisher_franka_states_.msg_.robot_mode =
423  franka_msgs::FrankaState::ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY;
424  break;
425  }
426 
427  publisher_franka_states_.msg_.header.seq = sequence_number_;
428  publisher_franka_states_.msg_.header.stamp = time;
429  publisher_franka_states_.unlockAndPublish();
430  }
431 }
432 
433 void FrankaStateController::publishJointStates(const ros::Time& time) {
434  if (publisher_joint_states_.trylock()) {
435  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq),
436  "Robot state joint members do not have same size");
437  static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J),
438  "Robot state joint members do not have same size");
439  for (size_t i = 0; i < robot_state_.q.size(); i++) {
440  publisher_joint_states_.msg_.name[i] = joint_names_[i];
441  publisher_joint_states_.msg_.position[i] = robot_state_.q[i];
442  publisher_joint_states_.msg_.velocity[i] = robot_state_.dq[i];
443  publisher_joint_states_.msg_.effort[i] = robot_state_.tau_J[i];
444  }
445  publisher_joint_states_.msg_.header.stamp = time;
446  publisher_joint_states_.msg_.header.seq = sequence_number_;
447  publisher_joint_states_.unlockAndPublish();
448  }
449  if (publisher_joint_states_desired_.trylock()) {
450  static_assert(sizeof(robot_state_.q_d) == sizeof(robot_state_.dq_d),
451  "Robot state joint members do not have same size");
452  static_assert(sizeof(robot_state_.q_d) == sizeof(robot_state_.tau_J_d),
453  "Robot state joint members do not have same size");
454  for (size_t i = 0; i < robot_state_.q_d.size(); i++) {
455  publisher_joint_states_desired_.msg_.name[i] = joint_names_[i];
456  publisher_joint_states_desired_.msg_.position[i] = robot_state_.q_d[i];
457  publisher_joint_states_desired_.msg_.velocity[i] = robot_state_.dq_d[i];
458  publisher_joint_states_desired_.msg_.effort[i] = robot_state_.tau_J_d[i];
459  }
460  publisher_joint_states_desired_.msg_.header.stamp = time;
461  publisher_joint_states_desired_.msg_.header.seq = sequence_number_;
462  publisher_joint_states_desired_.unlockAndPublish();
463  }
464 }
465 
466 void FrankaStateController::publishTransforms(const ros::Time& time) {
467  if (publisher_transforms_.trylock()) {
468  tf::StampedTransform stamped_transform(convertArrayToTf(robot_state_.F_T_NE), time,
469  arm_id_ + "_link8", arm_id_ + "_NE");
470  geometry_msgs::TransformStamped transform_message;
471  transformStampedTFToMsg(stamped_transform, transform_message);
472  publisher_transforms_.msg_.transforms[0] = transform_message;
473 
474  stamped_transform = tf::StampedTransform(convertArrayToTf(robot_state_.NE_T_EE), time,
475  arm_id_ + "_NE", arm_id_ + "_EE");
476  transformStampedTFToMsg(stamped_transform, transform_message);
477  publisher_transforms_.msg_.transforms[1] = transform_message;
478 
479  stamped_transform = tf::StampedTransform(convertArrayToTf(robot_state_.EE_T_K), time,
480  arm_id_ + "_EE", arm_id_ + "_K");
481  transformStampedTFToMsg(stamped_transform, transform_message);
482  publisher_transforms_.msg_.transforms[2] = transform_message;
483  publisher_transforms_.unlockAndPublish();
484  }
485 }
486 
487 void FrankaStateController::publishExternalWrench(const ros::Time& time) {
488  if (publisher_external_wrench_.trylock()) {
489  publisher_external_wrench_.msg_.header.frame_id = arm_id_ + "_K";
490  publisher_external_wrench_.msg_.header.stamp = time;
491  publisher_external_wrench_.msg_.wrench.force.x = robot_state_.K_F_ext_hat_K[0];
492  publisher_external_wrench_.msg_.wrench.force.y = robot_state_.K_F_ext_hat_K[1];
493  publisher_external_wrench_.msg_.wrench.force.z = robot_state_.K_F_ext_hat_K[2];
494  publisher_external_wrench_.msg_.wrench.torque.x = robot_state_.K_F_ext_hat_K[3];
495  publisher_external_wrench_.msg_.wrench.torque.y = robot_state_.K_F_ext_hat_K[4];
496  publisher_external_wrench_.msg_.wrench.torque.z = robot_state_.K_F_ext_hat_K[5];
497  publisher_external_wrench_.unlockAndPublish();
498  }
499 }
500 
501 } // namespace franka_control
502 
503 PLUGINLIB_EXPORT_CLASS(franka_control::FrankaStateController, controller_interface::ControllerBase)
tf::Matrix3x3
franka_control::FrankaStateController::publishExternalWrench
void publishExternalWrench(const ros::Time &time)
Definition: franka_state_controller.cpp:487
franka_control::FrankaStateController::sequence_number_
uint64_t sequence_number_
Definition: franka_state_controller.h:68
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
hardware_interface::HardwareInterfaceException::what
const char * what() const noexcept override
hardware_interface::InterfaceManager::get
T * get()
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
ros.h
franka_control::FrankaStateController::publisher_joint_states_
realtime_tools::RealtimePublisher< sensor_msgs::JointState > publisher_joint_states_
Definition: franka_state_controller.h:63
franka_control::FrankaStateController::franka_state_interface_
franka_hw::FrankaStateInterface * franka_state_interface_
Definition: franka_state_controller.h:58
franka_control::FrankaStateController::publisher_external_wrench_
realtime_tools::RealtimePublisher< geometry_msgs::WrenchStamped > publisher_external_wrench_
Definition: franka_state_controller.h:65
realtime_tools::RealtimePublisher::init
void init(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
franka_control::FrankaStateController::publishFrankaStates
void publishFrankaStates(const ros::Time &time)
Definition: franka_state_controller.cpp:262
franka_control::FrankaStateController::publishTransforms
void publishTransforms(const ros::Time &time)
Definition: franka_state_controller.cpp:466
tf::StampedTransform
hardware_interface.h
class_list_macros.h
franka_control::FrankaStateController::joint_names_
std::vector< std::string > joint_names_
Definition: franka_state_controller.h:69
controller_interface::ControllerBase
message
def message(msg, *args, **kwargs)
franka_control::FrankaStateController
Controller to publish the robot state as ROS topics.
Definition: franka_state_controller.h:23
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
franka_control::FrankaStateController::arm_id_
std::string arm_id_
Definition: franka_state_controller.h:56
realtime_tools::RealtimePublisher::unlockAndPublish
void unlockAndPublish()
hardware_interface::RobotHW
ROS_ERROR
#define ROS_ERROR(...)
realtime_tools::RealtimePublisher::msg_
Msg msg_
franka_control::FrankaStateController::trigger_publish_
franka_hw::TriggerRate trigger_publish_
Definition: franka_state_controller.h:66
franka_hw::FrankaStateInterface
realtime_tools::RealtimePublisher::trylock
bool trylock()
franka_control::FrankaStateController::publisher_joint_states_desired_
realtime_tools::RealtimePublisher< sensor_msgs::JointState > publisher_joint_states_desired_
Definition: franka_state_controller.h:64
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:155
Msg::header
Header header
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
tf::Transform
tf.h
hardware_interface::HardwareInterfaceException
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:251
transform_datatypes.h
ros::Time
franka_control::FrankaStateController::franka_state_handle_
std::unique_ptr< franka_hw::FrankaStateHandle > franka_state_handle_
Definition: franka_state_controller.h:59
franka_control::FrankaStateController::publishJointStates
void publishJointStates(const ros::Time &time)
Definition: franka_state_controller.cpp:433
franka_control::FrankaStateController::publisher_franka_states_
realtime_tools::RealtimePublisher< franka_msgs::FrankaState > publisher_franka_states_
Definition: franka_state_controller.h:62
franka_control::FrankaStateController::publisher_transforms_
realtime_tools::RealtimePublisher< tf2_msgs::TFMessage > publisher_transforms_
Definition: franka_state_controller.h:61
franka_hw::TriggerRate
Header::stamp
ros::Time stamp
ros::Duration
tf::Quaternion
franka_cartesian_command_interface.h
franka_control::FrankaStateController::robot_state_
franka::RobotState robot_state_
Definition: franka_state_controller.h:67
ros::NodeHandle
franka_control
Definition: franka_state_controller.h:18
ros::Time::now
static Time now()
error
def error(*args, **kwargs)
franka_state_controller.h

franka_control
Author(s): Franka Emika GmbH
autogenerated on Mon Sep 19 2022 02:33:24