Full robot state while moving

README.md

@@ -168,6 +168,49 @@ current_pose = robot.current_pose()
 ```
 
 
+### Robot State
+
+The robot state can be retrieved by calling the following methods:
+
+* `get_state`: Return an object of the `frankx.RobotState` class which contains most of the same attributes, under the same name, as the libfranka [franka::RobotState](https://frankaemika.github.io/libfranka/structfranka_1_1RobotState.html) definition.
+
+* `current_pose`: Return a 3D Affine transformation object of the measured end effector pose in base frame (alias for [franka::RobotState::O_T_EE](https://frankaemika.github.io/libfranka/structfranka_1_1RobotState.html#a193781d47722b32925e0ea7ac415f442)) affected or not by another Affine transformation.
+
+* `current_joint_positions`: Return a sequence of the manipulator arm's 7-joint positions (alias for [franka::RobotState::q](https://frankaemika.github.io/libfranka/structfranka_1_1RobotState.html#ade3335d1ac2f6c44741a916d565f7091)).
+
+```.py
+robot = Robot("172.16.0.2")
+
+# Get the current state
+state = robot.get_state()
+pose = robot.current_pose()
+joint_positions = robot.current_joint_positions()
+```
+
+However, invoking these methods as-is throws an exception (`_frankx.InvalidOperationException: libfranka robot: Cannot perform this operation while another control or read operation is running`) when the robot is moving. In this case, the methods must be invoked by setting the `read_once` argument to False (default to True) to retrieve the state.
+```.py
+robot = Robot("172.16.0.2")
+
+# Get the current state handling the read exception when the robot is in motion
+try:
+    robot_state = robot.get_state(read_once=True)
+except frankx.InvalidOperationException:
+    robot_state = robot.get_state(read_once=False)
+
+# Get the current pose handling the read exception when the robot is in motion
+try:
+    pose = robot.current_pose(read_once=True)
+except frankx.InvalidOperationException:
+    pose = robot.current_pose(read_once=False)
+
+# Get the current joint positions handling the read exception when the robot is in motion
+try:
+    joint_positions = robot.current_joint_positions(read_once=True)
+except frankx.InvalidOperationException:
+    joint_positions = robot.current_joint_positions(read_once=False)
+```
+
+
 ### Motion Types
 
 Frankx defines five different motion types. In python, you can use them as follows:

frankx/__init__.py

@@ -20,6 +20,7 @@
     StopMotion,
     Waypoint,
     WaypointMotion,
+    InvalidOperationException,
 )
 
 from .gripper import Gripper

include/frankx/motion_impedance_generator.hpp

@@ -27,6 +27,7 @@ struct ImpedanceMotionGenerator: public MotionGenerator {
     Affine frame;
     ImpedanceMotion& motion;
     MotionData& data;
+    franka::RobotState asynchronous_state;
 
     explicit ImpedanceMotionGenerator(RobotType* robot, const Affine& frame, ImpedanceMotion& motion, MotionData& data): robot(robot), frame(frame), motion(motion), data(data) {
         if (motion.type == ImpedanceMotion::Type::Joint) {
@@ -113,6 +114,8 @@ struct ImpedanceMotionGenerator: public MotionGenerator {
         }
 #endif
 
+        asynchronous_state = franka::RobotState(robot_state);
+
         if (motion.should_finish) {
             motion.is_active = false;
             return franka::MotionFinished(franka::Torques(tau_d_array));

include/frankx/motion_joint_generator.hpp

@@ -26,6 +26,7 @@ struct JointMotionGenerator: public MotionGenerator {
 
     JointMotion motion;
     MotionData& data;
+    franka::RobotState asynchronous_state;
 
     explicit JointMotionGenerator(RobotType* robot, JointMotion motion, MotionData& data): robot(robot), motion(motion), data(data) { }
 
@@ -57,6 +58,8 @@ struct JointMotionGenerator: public MotionGenerator {
         }
 #endif
 
+        asynchronous_state = franka::RobotState(robot_state);
+
         const int steps = std::max<int>(period.toMSec(), 1);
         for (int i = 0; i < steps; i++) {
             result = trajectory_generator.update(input_para, output_para);

include/frankx/motion_path_generator.hpp

@@ -27,6 +27,7 @@ struct PathMotionGenerator: public MotionGenerator {
     Affine frame;
     PathMotion motion;
     MotionData& data;
+    franka::RobotState asynchronous_state;
 
     explicit PathMotionGenerator(RobotType* robot, const Affine& frame, PathMotion motion, MotionData& data): robot(robot), frame(frame), motion(motion), data(data) {
         // Insert current pose into beginning of path
@@ -56,6 +57,8 @@ struct PathMotionGenerator: public MotionGenerator {
         }
 #endif
 
+        asynchronous_state = franka::RobotState(robot_state);
+
         const int steps = std::max<int>(period.toMSec(), 1);
         trajectory_index += steps;
         if (trajectory_index >= trajectory.states.size()) {

include/frankx/motion_waypoint_generator.hpp

@@ -33,6 +33,7 @@ struct WaypointMotionGenerator: public MotionGenerator {
     Affine frame;
     WaypointMotion& motion;
     MotionData& data;
+    franka::RobotState asynchronous_state;
 
     bool set_target_at_zero_time {true};
 
@@ -89,6 +90,8 @@ struct WaypointMotionGenerator: public MotionGenerator {
             init(robot_state, period);
         }
 
+        asynchronous_state = franka::RobotState(robot_state);
+
         for (auto& reaction : data.reactions) {
             if (reaction.has_fired) {
                 continue;

include/frankx/robot.hpp

@@ -77,12 +77,13 @@ class Robot: public franka::Robot {
     bool hasErrors();
     bool recoverFromErrors();
 
-    Affine currentPose(const Affine& frame = Affine());
-    std::array<double, 7> currentJointPositions();
+    Affine currentPose(const Affine& frame = Affine(), const bool& read_once = true);
+    std::array<double, 7> currentJointPositions(const bool& read_once = true);
     Affine forwardKinematics(const std::array<double, 7>& q);
     std::array<double, 7> inverseKinematics(const Affine& target, const std::array<double, 7>& q0);
 
-    ::franka::RobotState get_state();
+    ::franka::RobotState* asynchronous_state_ptr;
+    ::franka::RobotState get_state(const bool& read_once = true);
 
     bool move(ImpedanceMotion& motion);
     bool move(ImpedanceMotion& motion, MotionData& data);

src/frankx/python.cpp

@@ -292,10 +292,11 @@ PYBIND11_MODULE(_frankx, m) {
         .def("has_errors", &Robot::hasErrors)
         .def("recover_from_errors", &Robot::recoverFromErrors)
         .def("read_once", &Robot::readOnce)
-        .def("current_pose", &Robot::currentPose, "frame"_a = Affine())
+        .def("current_pose", &Robot::currentPose, "frame"_a = Affine(), "read_once"_a = true)
+        .def("current_joint_positions", &Robot::currentJointPositions, "read_once"_a = true)
         .def("forward_kinematics", &Robot::forwardKinematics, "q"_a)
         .def("inverse_kinematics", &Robot::inverseKinematics, "target"_a, "q0"_a)
-        .def("get_state", &Robot::get_state)
+        .def("get_state", &Robot::get_state, "read_once"_a = true)
         .def("move", (bool (Robot::*)(ImpedanceMotion&)) &Robot::move, py::call_guard<py::gil_scoped_release>())
         .def("move", (bool (Robot::*)(ImpedanceMotion&, MotionData&)) &Robot::move, py::call_guard<py::gil_scoped_release>())
         .def("move", (bool (Robot::*)(const Affine&, ImpedanceMotion&)) &Robot::move, py::call_guard<py::gil_scoped_release>())

src/frankx/robot.cpp

@@ -49,13 +49,21 @@ bool Robot::recoverFromErrors() {
     return !hasErrors();
 }
 
-Affine Robot::currentPose(const Affine& frame) {
-    auto state = readOnce();
+Affine Robot::currentPose(const Affine& frame, const bool& read_once) {
+    ::franka::RobotState state;
+    if (read_once)
+        state = readOnce();
+    else
+        state = ::franka::RobotState(*asynchronous_state_ptr);
     return Affine(state.O_T_EE) * frame;
 }
 
-std::array<double, 7> Robot::currentJointPositions() {
-    auto state = readOnce();
+std::array<double, 7> Robot::currentJointPositions(const bool& read_once) {
+    ::franka::RobotState state;
+    if (read_once)
+        state = readOnce();
+    else
+        state = ::franka::RobotState(*asynchronous_state_ptr);
     return state.q;
 }
 
@@ -74,8 +82,11 @@ std::array<double, 7> Robot::inverseKinematics(const Affine& target, const std::
     return result;
 }
 
-::franka::RobotState Robot::get_state() {
-    return readOnce();
+::franka::RobotState Robot::get_state(const bool& read_once) {
+    if (read_once)
+        return readOnce();
+    else
+        return ::franka::RobotState(*asynchronous_state_ptr);
 }
 
 bool Robot::move(ImpedanceMotion& motion) {
@@ -94,6 +105,8 @@ bool Robot::move(const Affine& frame, ImpedanceMotion& motion) {
 bool Robot::move(const Affine& frame, ImpedanceMotion& motion, MotionData& data) {
     ImpedanceMotionGenerator<Robot> mg {this, frame, motion, data};
 
+    asynchronous_state_ptr = &mg.asynchronous_state;
+
     try {
         control(stateful<franka::Torques>(mg));
         motion.is_active = false;
@@ -115,6 +128,8 @@ bool Robot::move(JointMotion motion) {
 bool Robot::move(JointMotion motion, MotionData& data) {
     JointMotionGenerator<Robot> mg {this, motion, data};
 
+    asynchronous_state_ptr = &mg.asynchronous_state;
+
     try {
         control(stateful<franka::JointPositions>(mg));
 
@@ -142,6 +157,8 @@ bool Robot::move(const Affine& frame, PathMotion motion) {
 bool Robot::move(const Affine& frame, PathMotion motion, MotionData& data) {
     PathMotionGenerator<Robot> mg {this, frame, motion, data};
 
+    asynchronous_state_ptr = &mg.asynchronous_state;
+
     try {
         control(stateful<franka::CartesianPose>(mg), controller_mode);
 
@@ -170,6 +187,8 @@ bool Robot::move(const Affine& frame, WaypointMotion& motion, MotionData& data)
     WaypointMotionGenerator<Robot> mg {this, frame, motion, data};
     mg.input_para.target_position[0] = 0.01;
 
+    asynchronous_state_ptr = &mg.asynchronous_state;
+
     try {
         control(stateful<franka::CartesianPose>(mg), controller_mode);