diff --git a/CHANGELOG.md b/CHANGELOG.md
index d1d879a8706e76f77119a42025a5cfe61cc43167..486286c1f37e4d6f7dbb86fd77053a5bd5399fb1 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -4,8 +4,8 @@
 
   * Added missing run-time dependencies to `franka_description` and `franka_control`
   * Added epsilon to grasp action
-  * Added `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal` and `I_total`
-    to the robot state
+  * Added `tau_J_d`, `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal`, `I_total`,
+    `theta` and `dtheta` to `franka::RobotState`
   * Updated and improved examples
   * Fixed includes for Eigen3 in `franka_example_controllers`
   * Fixed gripper state publishing in `franka_gripper_node`
diff --git a/franka_control/src/franka_state_controller.cpp b/franka_control/src/franka_state_controller.cpp
index 2fdcdbad2d65c137d1ca0915d46cc73b139a53e8..fb6d22e08cb38eb622cbb187279e20faaeed5c79 100644
--- a/franka_control/src/franka_state_controller.cpp
+++ b/franka_control/src/franka_state_controller.cpp
@@ -255,6 +255,12 @@ void FrankaStateController::publishFrankaStates(const ros::Time& time) {
                   "Robot state joint members do not have same size");
     static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtau_J),
                   "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J_d),
+                  "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.theta),
+                  "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtheta),
+                  "Robot state joint members do not have same size");
     static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_collision),
                   "Robot state joint members do not have same size");
     static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_contact),
@@ -268,6 +274,9 @@ void FrankaStateController::publishFrankaStates(const ros::Time& time) {
       publisher_franka_states_.msg_.dq_d[i] = robot_state_.dq_d[i];
       publisher_franka_states_.msg_.tau_J[i] = robot_state_.tau_J[i];
       publisher_franka_states_.msg_.dtau_J[i] = robot_state_.dtau_J[i];
+      publisher_franka_states_.msg_.tau_J_d[i] = robot_state_.tau_J_d[i];
+      publisher_franka_states_.msg_.theta[i] = robot_state_.theta[i];
+      publisher_franka_states_.msg_.dtheta[i] = robot_state_.dtheta[i];
       publisher_franka_states_.msg_.joint_collision[i] = robot_state_.joint_collision[i];
       publisher_franka_states_.msg_.joint_contact[i] = robot_state_.joint_contact[i];
       publisher_franka_states_.msg_.tau_ext_hat_filtered[i] = robot_state_.tau_ext_hat_filtered[i];
diff --git a/franka_msgs/msg/FrankaState.msg b/franka_msgs/msg/FrankaState.msg
index fea9184f82ca538953f37027a330cbf132d9eda5..7f8f376a1d21fa6166e05ee4952995bbf1b3b2d5 100644
--- a/franka_msgs/msg/FrankaState.msg
+++ b/franka_msgs/msg/FrankaState.msg
@@ -5,8 +5,11 @@ float64[7] q
 float64[7] q_d
 float64[7] dq
 float64[7] dq_d
+float64[7] theta
+float64[7] dtheta
 float64[7] tau_J
 float64[7] dtau_J
+float64[7] tau_J_d
 float64[6] K_F_ext_hat_K
 float64[2] elbow
 float64[2] elbow_d