From 6ba96d3322b523223051751d2681c3d7fcc0b1c4 Mon Sep 17 00:00:00 2001
From: Tim Schneider <mail@tim-schneider.me>
Date: Tue, 28 May 2024 02:04:54 +0200
Subject: [PATCH] Implemented Robot.currentJointState and
 Robot.currentJointVelocities

---
 include/franky/robot.hpp |  7 ++++++-
 python/python.cpp        |  2 ++
 src/robot.cpp            | 11 ++++++++++-
 3 files changed, 18 insertions(+), 2 deletions(-)

diff --git a/include/franky/robot.hpp b/include/franky/robot.hpp
index 0ba257d8..c3fd1c0c 100644
--- a/include/franky/robot.hpp
+++ b/include/franky/robot.hpp
@@ -22,7 +22,8 @@
 #include "franky/scope_guard.hpp"
 #include "franky/control_signal_type.hpp"
 #include "franky/util.hpp"
-#include "relative_dynamics_factor.hpp"
+#include "franky/relative_dynamics_factor.hpp"
+#include "franky/joint_state.hpp"
 
 namespace franky {
 
@@ -136,8 +137,12 @@ class Robot : public franka::Robot {
             RobotVelocity(franka::CartesianVelocities(s.O_dP_EE_c, s.delbow_c))};
   }
 
+  [[nodiscard]] JointState currentJointState();
+
   [[nodiscard]] Vector7d currentJointPositions();
 
+  [[nodiscard]] Vector7d currentJointVelocities();
+
   [[nodiscard]] franka::RobotState state();
 
   [[nodiscard]] RelativeDynamicsFactor relative_dynamics_factor();
diff --git a/python/python.cpp b/python/python.cpp
index 53868672..bc1fa87d 100644
--- a/python/python.cpp
+++ b/python/python.cpp
@@ -985,6 +985,8 @@ PYBIND11_MODULE(_franky, m) {
       .def_property_readonly("current_pose", &Robot::currentPose)
       .def_property_readonly("current_cartesian_velocity", &Robot::currentCartesianVelocity)
       .def_property_readonly("current_cartesian_state", &Robot::currentCartesianState)
+      .def_property_readonly("current_joint_state", &Robot::currentJointState)
+      .def_property_readonly("current_joint_velocities", &Robot::currentJointVelocities)
       .def_property_readonly("current_joint_positions", &Robot::currentJointPositions)
       .def_property_readonly("state", &Robot::state)
       .def_property_readonly("is_in_control", &Robot::is_in_control)
diff --git a/src/robot.cpp b/src/robot.cpp
index ca4d3278..da6c3910 100644
--- a/src/robot.cpp
+++ b/src/robot.cpp
@@ -24,8 +24,17 @@ bool Robot::recoverFromErrors() {
   return !hasErrors();
 }
 
+JointState Robot::currentJointState() {
+  auto s = state();
+  return {Eigen::Map<const Vector7d>(state().q.data()), Eigen::Map<const Vector7d>(state().dq.data())};
+}
+
 Vector7d Robot::currentJointPositions() {
-  return Eigen::Map<const Vector7d>(state().q.data());
+  return currentJointState().position();
+}
+
+Vector7d Robot::currentJointVelocities() {
+  return currentJointState().velocity();
 }
 
 Affine Robot::forwardKinematics(const Vector7d &q) {