Struct RobotState

Struct Documentation

struct RobotState

Describes the robot state.

Public Members

std::array<double, 16> O_T_EE = {}

OTEE Measured end effector pose in base frame. Pose is represented as a 4x4 matrix in column-major format.

std::array<double, 16> O_T_EE_d = {}

OTEEd Last desired end effector pose of motion generation in base frame. Pose is represented as a 4x4 matrix in column-major format.

std::array<double, 16> F_T_EE = {}

FTEE End effector frame pose in flange frame. Pose is represented as a 4x4 matrix in column-major format.

See also

F_T_NE

See also

NE_T_EE

See also

Robot for an explanation of the F, NE and EE frames.

std::array<double, 16> F_T_NE = {}

FTNE Nominal end effector frame pose in flange frame. Pose is represented as a 4x4 matrix in column-major format.

See also

F_T_EE

See also

NE_T_EE

See also

Robot for an explanation of the F, NE and EE frames.

std::array<double, 16> NE_T_EE = {}

NETEE End effector frame pose in nominal end effector frame. Pose is represented as a 4x4 matrix in column-major format.

See also

Robot::setEE to change this frame.

See also

F_T_EE

See also

F_T_NE

See also

Robot for an explanation of the F, NE and EE frames.

std::array<double, 16> EE_T_K = {}

EETK Stiffness frame pose in end effector frame. Pose is represented as a 4x4 matrix in column-major format.

See also K frame.

double m_ee = {}

mEE Configured mass of the end effector.

std::array<double, 9> I_ee = {}

IEE Configured rotational inertia matrix of the end effector load with respect to center of mass.

std::array<double, 3> F_x_Cee = {}

FxCEE Configured center of mass of the end effector load with respect to flange frame.

double m_load = {}

mload Configured mass of the external load.

std::array<double, 9> I_load = {}

Iload Configured rotational inertia matrix of the external load with respect to center of mass.

std::array<double, 3> F_x_Cload = {}

FxCload Configured center of mass of the external load with respect to flange frame.

double m_total = {}

mtotal Sum of the mass of the end effector and the external load.

std::array<double, 9> I_total = {}

Itotal Combined rotational inertia matrix of the end effector load and the external load with respect to the center of mass.

std::array<double, 3> F_x_Ctotal = {}

FxCtotal Combined center of mass of the end effector load and the external load with respect to flange frame.

std::array<double, 2> elbow = {}

Elbow configuration.

The values of the array are:

  • elbow[0]: Position of the 3rd joint in [rad].

  • elbow[1]: Flip direction of the elbow (4th joint):

    • +1 if q4>qelbowflip

    • 0 if q4==qelbowflip

    • -1 if q4<qelbowflip

with qelbowflip as specified in the robot interface specification page in the FCI Documentation.

std::array<double, 2> elbow_d = {}

Desired elbow configuration.

The values of the array are:

  • elbow_d[0]: Position of the 3rd joint in [rad].

  • elbow_d[1]: Flip direction of the elbow (4th joint):

    • +1 if q4>qelbowflip

    • 0 if q4==qelbowflip

    • -1 if q4<qelbowflip

with qelbowflip as specified in the robot interface specification page in the FCI Documentation.

std::array<double, 2> elbow_c = {}

Commanded elbow configuration.

The values of the array are:

  • elbow_c[0]: Position of the 3rd joint in [rad].

  • elbow_c[1]: Flip direction of the elbow (4th joint):

    • +1 if q4>qelbowflip

    • 0 if q4==qelbowflip

    • -1 if q4<qelbowflip

with qelbowflip as specified in the robot interface specification page in the FCI Documentation.

std::array<double, 2> delbow_c = {}

Commanded elbow velocity.

The values of the array are:

  • delbow_c[0] Velocity of the 3rd joint in rads

  • delbow_c[1] is always 0.

std::array<double, 2> ddelbow_c = {}

Commanded elbow acceleration.

The values of the array are:

  • ddelbow_c[0] Acceleration of the 3rd joint in rads2

  • ddelbow_c[1] is always 0.

std::array<double, 7> tau_J = {}

τJ Measured link-side joint torque sensor signals. Unit: [Nm]

std::array<double, 7> tau_J_d = {}

τJd Desired link-side joint torque sensor signals without gravity. Unit: [Nm]

std::array<double, 7> dtau_J = {}

τJ˙ Derivative of measured link-side joint torque sensor signals. Unit: [Nms]

std::array<double, 7> q = {}

q Measured joint position. Unit: [rad]

std::array<double, 7> q_d = {}

qd Desired joint position. Unit: [rad]

std::array<double, 7> dq = {}

q˙ Measured joint velocity. Unit: [rads]

std::array<double, 7> dq_d = {}

q˙d Desired joint velocity. Unit: [rads]

std::array<double, 7> ddq_d = {}

q¨d Desired joint acceleration. Unit: [rads2]

std::array<double, 7> joint_contact = {}

Indicates which contact level is activated in which joint. After contact disappears, value turns to zero.

See also

Robot::setCollisionBehavior for setting sensitivity values.

std::array<double, 6> cartesian_contact = {}

Indicates which contact level is activated in which Cartesian dimension (x,y,z,R,P,Y). After contact disappears, the value turns to zero.

See also

Robot::setCollisionBehavior for setting sensitivity values.

std::array<double, 7> joint_collision = {}

Indicates which contact level is activated in which joint. After contact disappears, the value stays the same until a reset command is sent.

See also

Robot::setCollisionBehavior for setting sensitivity values.

See also

Robot::automaticErrorRecovery for performing a reset after a collision.

std::array<double, 6> cartesian_collision = {}

Indicates which contact level is activated in which Cartesian dimension (x,y,z,R,P,Y). After contact disappears, the value stays the same until a reset command is sent.

See also

Robot::setCollisionBehavior for setting sensitivity values.

See also

Robot::automaticErrorRecovery for performing a reset after a collision.

std::array<double, 7> tau_ext_hat_filtered = {}

τ^ext Low-pass filtered torques generated by external forces on the joints. It does not include configured end-effector and load nor the mass and dynamics of the robot. tau_ext_hat_filtered is the error between tau_J and the expected torques given by the robot model. Unit: [Nm].

std::array<double, 6> O_F_ext_hat_K = {}

OFK,ext Estimated external wrench (force, torque) acting on stiffness frame, expressed relative to the base frame. Forces applied by the robot to the environment are positive, while forces applied by the environment on the robot are negative. Becomes [0,0,0,0,0,0] when near or in a singularity. See also Stiffness frame K. Unit: [N,N,N,Nm,Nm,Nm].

std::array<double, 6> K_F_ext_hat_K = {}

KFK,ext Estimated external wrench (force, torque) acting on stiffness frame, expressed relative to the stiffness frame. Forces applied by the robot to the environment are positive, while forces applied by the environment on the robot are negative. Becomes [0,0,0,0,0,0] when near or in a singularity. See also Stiffness frame K. Unit: [N,N,N,Nm,Nm,Nm].

std::array<double, 6> O_dP_EE_d = {}

OdPEEd Desired end effector twist in base frame. Unit: [ms,ms,ms,rads,rads,rads].

std::array<double, 3> O_ddP_O = {}

OddPO Linear component of the acceleration of the robot’s base, expressed in frame parallel to the base frame, i.e. the base’s translational acceleration. If the base is resting this shows the direction of the gravity vector. It is harcoded for now to {0, 0, -9.81}.

std::array<double, 16> O_T_EE_c = {}

OTEEc Last commanded end effector pose of motion generation in base frame. Pose is represented as a 4x4 matrix in column-major format.

std::array<double, 6> O_dP_EE_c = {}

OdPEEc Last commanded end effector twist in base frame. Unit: [ms,ms,ms,rads,rads,rads].

std::array<double, 6> O_ddP_EE_c = {}

OddPEEc Last commanded end effector acceleration in base frame. Unit: [ms2,ms2,ms2,rads2,rads2,rads2].

std::array<double, 7> theta = {}

θ Motor position. Unit: [rad]

std::array<double, 7> dtheta = {}

θ˙ Motor velocity. Unit: [rads]

Errors current_errors = {}

Current error state.

Errors last_motion_errors = {}

Contains the errors that aborted the previous motion.

double control_command_success_rate = {}

Percentage of the last 100 control commands that were successfully received by the robot.

Shows a value of zero if no control or motion generator loop is currently running.

Range: [0,1].

RobotMode robot_mode = RobotMode::kUserStopped

Current robot mode.

Duration time = {}

Strictly monotonically increasing timestamp since robot start.

Inside of control loops time_step parameter of Robot::control can be used instead.