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
4 
5 #include <cmath>
6 #include <memory>
7 #include <mutex>
8 #include <string>
9 
10 #include <franka/errors.h>
12 #include <franka_msgs/Errors.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)>(
33  message.cartesian_position_limits_violation =
34  static_cast<decltype(message.cartesian_position_limits_violation)>(
36  message.self_collision_avoidance_violation =
37  static_cast<decltype(message.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)>(
44  message.force_control_safety_violation =
45  static_cast<decltype(message.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)>(
53  message.max_path_pose_deviation_violation =
54  static_cast<decltype(message.max_path_pose_deviation_violation)>(
56  message.cartesian_velocity_profile_safety_violation =
57  static_cast<decltype(message.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)>(
62  message.joint_motion_generator_position_limits_violation =
63  static_cast<decltype(message.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)>(
68  message.joint_motion_generator_velocity_discontinuity =
69  static_cast<decltype(message.joint_motion_generator_velocity_discontinuity)>(
71  message.joint_motion_generator_acceleration_discontinuity =
72  static_cast<decltype(message.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)>(
77  message.cartesian_motion_generator_elbow_limit_violation =
78  static_cast<decltype(message.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)>(
83  message.cartesian_motion_generator_velocity_discontinuity =
84  static_cast<decltype(message.cartesian_motion_generator_velocity_discontinuity)>(
86  message.cartesian_motion_generator_acceleration_discontinuity =
87  static_cast<decltype(message.cartesian_motion_generator_acceleration_discontinuity)>(
89  message.cartesian_motion_generator_elbow_sign_inconsistent =
90  static_cast<decltype(message.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)>(
95  message.cartesian_motion_generator_joint_position_limits_violation =
96  static_cast<decltype(message.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)>(
101  message.cartesian_motion_generator_joint_velocity_discontinuity =
102  static_cast<decltype(message.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)>(
107  message.cartesian_position_motion_generator_invalid_frame =
108  static_cast<decltype(message.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)>(
113  message.controller_torque_discontinuity =
114  static_cast<decltype(message.controller_torque_discontinuity)>(
116  message.start_elbow_sign_inconsistent =
117  static_cast<decltype(message.start_elbow_sign_inconsistent)>(
119  message.communication_constraints_violation =
120  static_cast<decltype(message.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)>(
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 
139  ros::NodeHandle& root_node_handle,
140  ros::NodeHandle& controller_node_handle) {
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  }
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"));
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(
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(
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());
194  }
195  {
196  std::lock_guard<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>> lock(
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(
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);
236  }
237 }
238 
241  static_assert(
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++) {
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++) {
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++) {
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++) {
335  }
339 
340  for (size_t i = 0; i < robot_state_.I_load.size(); i++) {
344  }
345 
346  for (size_t i = 0; i < robot_state_.F_x_Cload.size(); i++) {
350  }
351 
353  publisher_franka_states_.msg_.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) {
361  publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_OTHER;
362  break;
363 
365  publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_IDLE;
366  break;
367 
369  publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_MOVE;
370  break;
371 
373  publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_GUIDING;
374  break;
375 
377  publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_REFLEX;
378  break;
379 
381  publisher_franka_states_.msg_.robot_mode =
382  franka_msgs::FrankaState::ROBOT_MODE_USER_STOPPED;
383  break;
384 
386  publisher_franka_states_.msg_.robot_mode =
387  franka_msgs::FrankaState::ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY;
388  break;
389  }
390 
394  }
395 }
396 
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++) {
405  publisher_joint_states_.msg_.position[i] = robot_state_.q[i];
406  publisher_joint_states_.msg_.velocity[i] = robot_state_.dq[i];
408  }
412  }
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++) {
423  }
427  }
428 }
429 
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;
442  }
443 }
444 
447  publisher_external_wrench_.msg_.header.frame_id = arm_id_ + "_K";
456  }
457 }
458 
459 } // namespace franka_control
460 
const bool & cartesian_motion_generator_joint_velocity_discontinuity
std::unique_ptr< franka_hw::FrankaStateHandle > franka_state_handle_
realtime_tools::RealtimePublisher< sensor_msgs::JointState > publisher_joint_states_desired_
const bool & cartesian_motion_generator_elbow_limit_violation
const bool & cartesian_motion_generator_joint_velocity_limits_violation
std::array< double, 16 > F_T_EE
const bool & force_control_safety_violation
std::array< double, 7 > dtau_J
std::array< double, 7 > tau_ext_hat_filtered
const bool & controller_torque_discontinuity
double toSec() const noexcept
std::array< double, 3 > F_x_Cload
Header header
const bool & joint_motion_generator_velocity_discontinuity
std::array< double, 3 > F_x_Ctotal
const bool & communication_constraints_violation
const bool & cartesian_position_motion_generator_start_pose_invalid
const bool & joint_reflex
Controller to publish the robot state as ROS topics.
const bool & self_collision_avoidance_violation
const bool & cartesian_motion_generator_start_elbow_invalid
const bool & cartesian_velocity_violation
void update(const ros::Time &time, const ros::Duration &period) override
Reads the current robot state from the franka_hw::FrankaStateInterface and publishes it...
std::array< double, 2 > elbow_c
const bool & cartesian_motion_generator_velocity_limits_violation
std::array< double, 7 > q_d
std::array< double, 6 > K_F_ext_hat_K
std::array< double, 6 > O_F_ext_hat_K
std::array< double, 16 > O_T_EE_d
const bool & joint_position_motion_generator_start_pose_invalid
std::array< double, 3 > F_x_Cee
std::array< double, 7 > joint_contact
const bool & joint_motion_generator_position_limits_violation
const bool & cartesian_motion_generator_joint_position_limits_violation
std::array< double, 2 > delbow_c
std::array< double, 7 > q
const bool & cartesian_motion_generator_velocity_discontinuity
const bool & cartesian_motion_generator_joint_acceleration_discontinuity
std::array< double, 7 > dtheta
const bool & max_path_pose_deviation_violation
const bool & cartesian_position_limits_violation
const bool & cartesian_position_motion_generator_invalid_frame
const bool & cartesian_motion_generator_acceleration_discontinuity
std::array< double, 16 > O_T_EE_c
const bool & cartesian_velocity_profile_safety_violation
std::array< double, 7 > dq
const bool & joint_p2p_insufficient_torque_for_planning
const bool & max_goal_pose_deviation_violation
std::array< double, 16 > O_T_EE
def message(msg, args, kwargs)
const bool & joint_motion_generator_acceleration_discontinuity
std::array< double, 9 > I_load
std::array< double, 7 > dq_d
void publishExternalWrench(const ros::Time &time)
const bool & joint_position_limits_violation
const bool & force_controller_desired_force_tolerance_violation
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.
std::array< double, 6 > O_ddP_EE_c
realtime_tools::RealtimePublisher< sensor_msgs::JointState > publisher_joint_states_
std::array< double, 6 > O_dP_EE_c
std::array< double, 2 > ddelbow_c
std::array< double, 7 > joint_collision
void init(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
const bool & power_limit_violation
std::array< double, 7 > ddq_d
#define ROS_INFO_STREAM(args)
realtime_tools::RealtimePublisher< tf2_msgs::TFMessage > publisher_transforms_
std::array< double, 7 > tau_J_d
const bool & cartesian_reflex
bool getParam(const std::string &key, std::string &s) const
std::array< double, 9 > I_total
std::array< double, 2 > elbow_d
std::array< double, 6 > cartesian_contact
RobotMode robot_mode
static Time now()
const bool & joint_motion_generator_velocity_limits_violation
const bool & instability_detected
#define ROS_ERROR_STREAM(args)
std::array< double, 9 > I_ee
franka_hw::FrankaStateInterface * franka_state_interface_
double control_command_success_rate
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
const bool & joint_velocity_violation
realtime_tools::RealtimePublisher< franka_msgs::FrankaState > publisher_franka_states_
const bool & cartesian_motion_generator_elbow_sign_inconsistent
#define ROS_ERROR(...)
std::array< double, 6 > O_dP_EE_d
std::array< double, 7 > tau_J
const bool & start_elbow_sign_inconsistent
const bool & tau_j_range_violation
std::array< double, 2 > elbow
realtime_tools::RealtimePublisher< geometry_msgs::WrenchStamped > publisher_external_wrench_
std::array< double, 16 > EE_T_K
ros::Time stamp
std::array< double, 6 > cartesian_collision
std::array< double, 7 > theta


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