12 #include <franka_msgs/Errors.h> 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]);
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 =
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 =
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 =
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 =
129 message.instability_detected =
143 ROS_ERROR(
"FrankaStateController: Could not get Franka state interface from hardware");
147 ROS_ERROR(
"FrankaStateController: Could not get parameter arm_id");
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].");
160 "FrankaStateController: Invalid or no joint_names parameters provided, aborting " 169 ROS_ERROR_STREAM(
"FrankaStateController: Exception getting franka state handle: " << ex.
what());
180 std::lock_guard<realtime_tools::RealtimePublisher<sensor_msgs::JointState>> lock(
188 std::lock_guard<realtime_tools::RealtimePublisher<sensor_msgs::JointState>> lock(
196 std::lock_guard<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>> lock(
204 geometry_msgs::TransformStamped transform_message;
205 transformStampedTFToMsg(stamped_transform, transform_message);
211 transformStampedTFToMsg(stamped_transform, transform_message);
215 std::lock_guard<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped>> lock(
243 "Robot state Cartesian members do not have same size");
245 "Robot state Cartesian members do not have same size");
247 "Robot state Cartesian members do not have same size");
249 "Robot state Cartesian members do not have same size");
251 "Robot state Cartesian members do not have same size");
253 "Robot state Cartesian members do not have same size");
265 "Robot state joint members do not have same size");
267 "Robot state joint members do not have same size");
269 "Robot state joint members do not have same size");
271 "Robot state joint members do not have same size");
273 "Robot state joint members do not have same size");
275 "Robot state joint members do not have same size");
277 "Robot state joint members do not have same size");
279 "Robot state joint members do not have same size");
281 "Robot state joint members do not have same size");
283 "Robot state joint members do not have same size");
285 "Robot state joint members do not have same size");
287 "Robot state joint members do not have same size");
305 "Robot state elbow configuration members do not have same size");
307 "Robot state elbow configuration members do not have same size");
309 "Robot state elbow configuration members do not have same size");
311 "Robot state elbow configuration members do not have same size");
322 "Robot state transforms do not have same size");
324 "Robot state transforms do not have same size");
326 "Robot state transforms do not have same size");
328 "Robot state transforms do not have same size");
382 franka_msgs::FrankaState::ROBOT_MODE_USER_STOPPED;
387 franka_msgs::FrankaState::ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY;
400 "Robot state joint members do not have same size");
402 "Robot state joint members do not have same size");
415 "Robot state joint members do not have same size");
417 "Robot state joint members do not have same size");
434 geometry_msgs::TransformStamped transform_message;
435 transformStampedTFToMsg(stamped_transform, transform_message);
439 transformStampedTFToMsg(stamped_transform, transform_message);
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
virtual const char * what() const
uint64_t sequence_number_
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
void publishJointStates(const ros::Time &time)
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.
franka_hw::TriggerRate trigger_publish_
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
franka::RobotState robot_state_
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
void publishFrankaStates(const ros::Time &time)
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
void publishTransforms(const ros::Time &time)
std::vector< std::string > joint_names_
const bool & max_path_pose_deviation_violation
Errors last_motion_errors
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
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
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
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
std::array< double, 6 > cartesian_collision
std::array< double, 7 > theta