libfranka  0.14.1
FCI C++ API
robot_state.h
Go to the documentation of this file.
1 // Copyright (c) 2024 Franka Robotics GmbH
2 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
3 #pragma once
4 
5 #include <array>
6 #include <ostream>
7 
8 #include <franka/duration.h>
9 #include <franka/errors.h>
10 
16 namespace franka {
17 
21 enum class RobotMode {
22  kOther,
23  kIdle,
24  kMove,
25  kGuiding,
26  kReflex,
27  kUserStopped,
28  kAutomaticErrorRecovery
29 };
30 
34 struct RobotState {
40  std::array<double, 16> O_T_EE{}; // NOLINT(readability-identifier-naming)
41 
47  std::array<double, 16> O_T_EE_d{}; // NOLINT(readability-identifier-naming)
48 
58  std::array<double, 16> F_T_EE{}; // NOLINT(readability-identifier-naming)
59 
69  std::array<double, 16> F_T_NE{}; // NOLINT(readability-identifier-naming)
70 
81  std::array<double, 16> NE_T_EE{}; // NOLINT(readability-identifier-naming)
82 
90  std::array<double, 16> EE_T_K{}; // NOLINT(readability-identifier-naming)
91 
96  double m_ee{};
97 
102  std::array<double, 9> I_ee{}; // NOLINT(readability-identifier-naming)
103 
108  std::array<double, 3> F_x_Cee{}; // NOLINT(readability-identifier-naming)
109 
114  double m_load{};
115 
120  std::array<double, 9> I_load{}; // NOLINT(readability-identifier-naming)
121 
126  std::array<double, 3> F_x_Cload{}; // NOLINT(readability-identifier-naming)
127 
132  double m_total{};
133 
139  std::array<double, 9> I_total{}; // NOLINT(readability-identifier-naming)
140 
146  std::array<double, 3> F_x_Ctotal{}; // NOLINT(readability-identifier-naming)
147 
161  std::array<double, 2> elbow{};
162 
176  std::array<double, 2> elbow_d{};
177 
191  std::array<double, 2> elbow_c{};
192 
200  std::array<double, 2> delbow_c{};
201 
209  std::array<double, 2> ddelbow_c{};
210 
215  std::array<double, 7> tau_J{}; // NOLINT(readability-identifier-naming)
216 
221  std::array<double, 7> tau_J_d{}; // NOLINT(readability-identifier-naming)
222 
227  std::array<double, 7> dtau_J{}; // NOLINT(readability-identifier-naming)
228 
233  std::array<double, 7> q{};
234 
239  std::array<double, 7> q_d{};
240 
245  std::array<double, 7> dq{};
246 
251  std::array<double, 7> dq_d{};
252 
257  std::array<double, 7> ddq_d{};
258 
265  std::array<double, 7> joint_contact{};
266 
273  std::array<double, 6> cartesian_contact{};
274 
282  std::array<double, 7> joint_collision{};
283 
291  std::array<double, 6> cartesian_collision{};
292 
299  std::array<double, 7> tau_ext_hat_filtered{};
300 
309  std::array<double, 6> O_F_ext_hat_K{}; // NOLINT(readability-identifier-naming)
310 
319  std::array<double, 6> K_F_ext_hat_K{}; // NOLINT(readability-identifier-naming)
320 
326  std::array<double, 6> O_dP_EE_d{}; // NOLINT(readability-identifier-naming)
327 
335  std::array<double, 3> O_ddP_O{}; // NOLINT(readability-identifier-naming)
336 
342  std::array<double, 16> O_T_EE_c{}; // NOLINT(readability-identifier-naming)
343 
349  std::array<double, 6> O_dP_EE_c{}; // NOLINT(readability-identifier-naming)
350 
357  std::array<double, 6> O_ddP_EE_c{}; // NOLINT(readability-identifier-naming)
358 
363  std::array<double, 7> theta{};
364 
369  std::array<double, 7> dtheta{};
370 
375 
380 
389 
393  RobotMode robot_mode = RobotMode::kUserStopped;
394 
402 };
403 
413 std::ostream& operator<<(std::ostream& ostream, const franka::RobotState& robot_state);
414 
423 std::ostream& operator<<(std::ostream& ostream, RobotMode robot_mode);
424 
425 } // namespace franka
Represents a duration with millisecond resolution.
Definition: duration.h:19
Contains the franka::Duration type.
Contains the franka::Errors type.
std::ostream & operator<<(std::ostream &ostream, const Errors &errors)
Streams the errors as JSON array.
RobotMode
Describes the robot's current mode.
Definition: robot_state.h:21
Enumerates errors that can occur while controlling a franka::Robot.
Definition: errors.h:18
Describes the robot state.
Definition: robot_state.h:34
Errors last_motion_errors
Contains the errors that aborted the previous motion.
Definition: robot_state.h:379
std::array< double, 2 > elbow_c
Commanded elbow configuration.
Definition: robot_state.h:191
std::array< double, 16 > O_T_EE
Measured end effector pose in base frame.
Definition: robot_state.h:40
std::array< double, 6 > O_dP_EE_d
Desired end effector twist in base frame.
Definition: robot_state.h:326
std::array< double, 2 > ddelbow_c
Commanded elbow acceleration.
Definition: robot_state.h:209
std::array< double, 7 > dtheta
Motor velocity.
Definition: robot_state.h:369
std::array< double, 2 > elbow_d
Desired elbow configuration.
Definition: robot_state.h:176
std::array< double, 7 > joint_collision
Indicates which contact level is activated in which joint.
Definition: robot_state.h:282
std::array< double, 16 > O_T_EE_c
Last commanded end effector pose of motion generation in base frame.
Definition: robot_state.h:342
std::array< double, 16 > O_T_EE_d
Last desired end effector pose of motion generation in base frame.
Definition: robot_state.h:47
std::array< double, 2 > elbow
Elbow configuration.
Definition: robot_state.h:161
std::array< double, 3 > F_x_Cload
Configured center of mass of the external load with respect to flange frame.
Definition: robot_state.h:126
RobotMode robot_mode
Current robot mode.
Definition: robot_state.h:393
std::array< double, 6 > O_dP_EE_c
Last commanded end effector twist in base frame.
Definition: robot_state.h:349
std::array< double, 6 > cartesian_collision
Indicates which contact level is activated in which Cartesian dimension .
Definition: robot_state.h:291
std::array< double, 2 > delbow_c
Commanded elbow velocity.
Definition: robot_state.h:200
std::array< double, 6 > O_F_ext_hat_K
Estimated external wrench (force, torque) acting on stiffness frame, expressed relative to the base f...
Definition: robot_state.h:309
std::array< double, 9 > I_load
Configured rotational inertia matrix of the external load with respect to center of mass.
Definition: robot_state.h:120
std::array< double, 7 > ddq_d
Desired joint acceleration.
Definition: robot_state.h:257
std::array< double, 16 > F_T_EE
End effector frame pose in flange frame.
Definition: robot_state.h:58
std::array< double, 7 > q_d
Desired joint position.
Definition: robot_state.h:239
std::array< double, 7 > tau_J_d
Desired link-side joint torque sensor signals without gravity.
Definition: robot_state.h:221
std::array< double, 7 > joint_contact
Indicates which contact level is activated in which joint.
Definition: robot_state.h:265
std::array< double, 3 > F_x_Ctotal
Combined center of mass of the end effector load and the external load with respect to flange frame.
Definition: robot_state.h:146
std::array< double, 9 > I_ee
Configured rotational inertia matrix of the end effector load with respect to center of mass.
Definition: robot_state.h:102
std::array< double, 6 > cartesian_contact
Indicates which contact level is activated in which Cartesian dimension .
Definition: robot_state.h:273
double m_total
Sum of the mass of the end effector and the external load.
Definition: robot_state.h:132
std::array< double, 16 > F_T_NE
Nominal end effector frame pose in flange frame.
Definition: robot_state.h:69
std::array< double, 3 > F_x_Cee
Configured center of mass of the end effector load with respect to flange frame.
Definition: robot_state.h:108
std::array< double, 6 > K_F_ext_hat_K
Estimated external wrench (force, torque) acting on stiffness frame, expressed relative to the stiffn...
Definition: robot_state.h:319
double m_load
Configured mass of the external load.
Definition: robot_state.h:114
std::array< double, 7 > theta
Motor position.
Definition: robot_state.h:363
Duration time
Strictly monotonically increasing timestamp since robot start.
Definition: robot_state.h:401
std::array< double, 3 > O_ddP_O
Linear component of the acceleration of the robot's base, expressed in frame parallel to the base fra...
Definition: robot_state.h:335
Errors current_errors
Current error state.
Definition: robot_state.h:374
std::array< double, 16 > NE_T_EE
End effector frame pose in nominal end effector frame.
Definition: robot_state.h:81
std::array< double, 6 > O_ddP_EE_c
Last commanded end effector acceleration in base frame.
Definition: robot_state.h:357
std::array< double, 7 > tau_ext_hat_filtered
Low-pass filtered torques generated by external forces on the joints.
Definition: robot_state.h:299
std::array< double, 7 > tau_J
Measured link-side joint torque sensor signals.
Definition: robot_state.h:215
std::array< double, 9 > I_total
Combined rotational inertia matrix of the end effector load and the external load with respect to the...
Definition: robot_state.h:139
std::array< double, 7 > q
Measured joint position.
Definition: robot_state.h:233
std::array< double, 7 > dtau_J
Derivative of measured link-side joint torque sensor signals.
Definition: robot_state.h:227
std::array< double, 16 > EE_T_K
Stiffness frame pose in end effector frame.
Definition: robot_state.h:90
std::array< double, 7 > dq_d
Desired joint velocity.
Definition: robot_state.h:251
double control_command_success_rate
Percentage of the last 100 control commands that were successfully received by the robot.
Definition: robot_state.h:388
std::array< double, 7 > dq
Measured joint velocity.
Definition: robot_state.h:245
double m_ee
Configured mass of the end effector.
Definition: robot_state.h:96