diff --git a/README.md b/README.md index 3510e1ce..6cd20b9a 100644 --- a/README.md +++ b/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: diff --git a/frankx/__init__.py b/frankx/__init__.py index a5684e67..93aea9e8 100644 --- a/frankx/__init__.py +++ b/frankx/__init__.py @@ -20,6 +20,7 @@ StopMotion, Waypoint, WaypointMotion, + InvalidOperationException, ) from .gripper import Gripper diff --git a/include/frankx/motion_impedance_generator.hpp b/include/frankx/motion_impedance_generator.hpp index f0bf3f2d..7a17d8b2 100644 --- a/include/frankx/motion_impedance_generator.hpp +++ b/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)); diff --git a/include/frankx/motion_joint_generator.hpp b/include/frankx/motion_joint_generator.hpp index a73da60d..21730f70 100644 --- a/include/frankx/motion_joint_generator.hpp +++ b/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(period.toMSec(), 1); for (int i = 0; i < steps; i++) { result = trajectory_generator.update(input_para, output_para); diff --git a/include/frankx/motion_path_generator.hpp b/include/frankx/motion_path_generator.hpp index b141b956..7a97d904 100644 --- a/include/frankx/motion_path_generator.hpp +++ b/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(period.toMSec(), 1); trajectory_index += steps; if (trajectory_index >= trajectory.states.size()) { diff --git a/include/frankx/motion_waypoint_generator.hpp b/include/frankx/motion_waypoint_generator.hpp index acc3e5fb..f2bcd659 100644 --- a/include/frankx/motion_waypoint_generator.hpp +++ b/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; diff --git a/include/frankx/robot.hpp b/include/frankx/robot.hpp index 437177ab..2d79f13e 100644 --- a/include/frankx/robot.hpp +++ b/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 currentJointPositions(); + Affine currentPose(const Affine& frame = Affine(), const bool& read_once = true); + std::array currentJointPositions(const bool& read_once = true); Affine forwardKinematics(const std::array& q); std::array inverseKinematics(const Affine& target, const std::array& 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); diff --git a/src/frankx/python.cpp b/src/frankx/python.cpp index cc081c50..01a65386 100644 --- a/src/frankx/python.cpp +++ b/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()) .def("move", (bool (Robot::*)(ImpedanceMotion&, MotionData&)) &Robot::move, py::call_guard()) .def("move", (bool (Robot::*)(const Affine&, ImpedanceMotion&)) &Robot::move, py::call_guard()) diff --git a/src/frankx/robot.cpp b/src/frankx/robot.cpp index 6a93363f..df1c732d 100644 --- a/src/frankx/robot.cpp +++ b/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 Robot::currentJointPositions() { - auto state = readOnce(); +std::array 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 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 mg {this, frame, motion, data}; + asynchronous_state_ptr = &mg.asynchronous_state; + try { control(stateful(mg)); motion.is_active = false; @@ -115,6 +128,8 @@ bool Robot::move(JointMotion motion) { bool Robot::move(JointMotion motion, MotionData& data) { JointMotionGenerator mg {this, motion, data}; + asynchronous_state_ptr = &mg.asynchronous_state; + try { control(stateful(mg)); @@ -142,6 +157,8 @@ bool Robot::move(const Affine& frame, PathMotion motion) { bool Robot::move(const Affine& frame, PathMotion motion, MotionData& data) { PathMotionGenerator mg {this, frame, motion, data}; + asynchronous_state_ptr = &mg.asynchronous_state; + try { control(stateful(mg), controller_mode); @@ -170,6 +187,8 @@ bool Robot::move(const Affine& frame, WaypointMotion& motion, MotionData& data) WaypointMotionGenerator mg {this, frame, motion, data}; mg.input_para.target_position[0] = 0.01; + asynchronous_state_ptr = &mg.asynchronous_state; + try { control(stateful(mg), controller_mode);