Implemented Robot.currentJointState and Robot.currentJointVelocities

TimSchneider42 · May 28, 2024 · 6ba96d3 · 6ba96d3
1 parent 21616cc
commit 6ba96d3
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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) {