From 29ed2e398894ee7ab726575704cf4483c5cbeb0e Mon Sep 17 00:00:00 2001
From: Simon Gabl <simon.gabl@franka.de>
Date: Mon, 29 Jan 2018 11:47:25 +0100
Subject: [PATCH] Added tau_J_d, theta and dtheta to the FrankaState message.
---
CHANGELOG.md | 4 ++--
franka_control/src/franka_state_controller.cpp | 9 +++++++++
franka_msgs/msg/FrankaState.msg | 3 +++
3 files changed, 14 insertions(+), 2 deletions(-)
diff --git a/CHANGELOG.md b/CHANGELOG.md
index d1d879a..486286c 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 2fdcdba..fb6d22e 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 fea9184..7f8f376 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
--
GitLab