diff --git a/CMakeLists.txt b/CMakeLists.txt index 1cb8fa3d..cc7a1fdb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,7 @@ endif() if(BUILD_EXAMPLES) - foreach(example IN ITEMS linear grasping) + foreach(example IN ITEMS linear grasping linear_non_rt) add_executable(${example} "examples/${example}.cpp") target_link_libraries(${example} PRIVATE frankx) endforeach() diff --git a/examples/linear_non_rt.cpp b/examples/linear_non_rt.cpp new file mode 100644 index 00000000..28498262 --- /dev/null +++ b/examples/linear_non_rt.cpp @@ -0,0 +1,30 @@ +#include + + +using namespace frankx; + + +int main(int argc, char *argv[]) { + if (argc != 2) { + std::cerr << "Usage: " << argv[0] << " " << std::endl; + return -1; + } + + // Connect to the robot + Robot robot(argv[1], 0.1, true, true, franka::RealtimeConfig::kIgnore); + robot.automaticErrorRecovery(); + + // Reduce the acceleration and velocity dynamic + robot.setDynamicRel(0.15); + + // Define and move forwards + auto way = Affine(0.0, 0.2, 0.0); + auto motion_forward = LinearRelativeMotion(way); + robot.move(motion_forward); + + // And move backwards using the inverse motion + auto motion_backward = LinearRelativeMotion(way.inverse()); + robot.move(motion_backward); + + return 0; +} diff --git a/examples/linear_non_rt.py b/examples/linear_non_rt.py new file mode 100644 index 00000000..7a79201a --- /dev/null +++ b/examples/linear_non_rt.py @@ -0,0 +1,26 @@ +from argparse import ArgumentParser + +from frankx import Affine, LinearRelativeMotion, Robot, RealtimeConfig + + +if __name__ == '__main__': + parser = ArgumentParser() + parser.add_argument('--host', default='172.16.0.2', help='FCI IP of the robot') + args = parser.parse_args() + + # Connect to the robot + robot = Robot(args.host, realtime_config=RealtimeConfig.Ignore) + robot.set_default_behavior() + robot.recover_from_errors() + + # Reduce the acceleration and velocity dynamic + robot.set_dynamic_rel(0.15) + + # Define and move forwards + way = Affine(0.0, 0.2, 0.0) + motion_forward = LinearRelativeMotion(way) + robot.move(motion_forward) + + # And move backwards using the inverse motion + motion_backward = LinearRelativeMotion(way.inverse()) + robot.move(motion_backward) diff --git a/frankx/__init__.py b/frankx/__init__.py index a5684e67..aa5b905f 100644 --- a/frankx/__init__.py +++ b/frankx/__init__.py @@ -20,6 +20,7 @@ StopMotion, Waypoint, WaypointMotion, + RealtimeConfig, ) from .gripper import Gripper diff --git a/frankx/robot.py b/frankx/robot.py index 5e313fdf..093e0506 100644 --- a/frankx/robot.py +++ b/frankx/robot.py @@ -1,4 +1,5 @@ import base64 +import imp import json import hashlib from http.client import HTTPSConnection @@ -7,12 +8,13 @@ from threading import Thread from _frankx import Robot as _Robot +from _frankx import RealtimeConfig as _RealtimeConfig from .gripper import Gripper as _Gripper class Robot(_Robot): - def __init__(self, fci_ip, dynamic_rel=1.0, user=None, password=None, repeat_on_error=True, stop_at_python_signal=True): - super().__init__(fci_ip, dynamic_rel=dynamic_rel, repeat_on_error=repeat_on_error, stop_at_python_signal=stop_at_python_signal) + def __init__(self, fci_ip, dynamic_rel=1.0, user=None, password=None, repeat_on_error=True, stop_at_python_signal=True, realtime_config=_RealtimeConfig.Enforce): + super().__init__(fci_ip, dynamic_rel=dynamic_rel, repeat_on_error=repeat_on_error, stop_at_python_signal=stop_at_python_signal, realtime_config=realtime_config) self.hostname = fci_ip self.user = user self.password = password diff --git a/include/frankx/robot.hpp b/include/frankx/robot.hpp index 437177ab..741d3f58 100644 --- a/include/frankx/robot.hpp +++ b/include/frankx/robot.hpp @@ -69,7 +69,7 @@ class Robot: public franka::Robot { bool stop_at_python_signal {true}; //! Connects to a robot at the given FCI IP address. - explicit Robot(std::string fci_ip, double dynamic_rel = 1.0, bool repeat_on_error = true, bool stop_at_python_signal = true); + explicit Robot(std::string fci_ip, double dynamic_rel = 1.0, bool repeat_on_error = true, bool stop_at_python_signal = true, franka::RealtimeConfig realtime_config = franka::RealtimeConfig::kEnforce); void setDefaultBehavior(); void setDynamicRel(double dynamic_rel); diff --git a/src/frankx/python.cpp b/src/frankx/python.cpp index cc081c50..ca08ed38 100644 --- a/src/frankx/python.cpp +++ b/src/frankx/python.cpp @@ -214,6 +214,11 @@ PYBIND11_MODULE(_frankx, m) { .value("CartesianImpedance", franka::ControllerMode::kCartesianImpedance) .export_values(); + py::enum_(m, "RealtimeConfig") + .value("Enforce", franka::RealtimeConfig::kEnforce) + .value("Ignore", franka::RealtimeConfig::kIgnore) + .export_values(); + py::class_(m, "RobotState") .def_readonly("O_T_EE", &franka::RobotState::O_T_EE) .def_readonly("O_T_EE_d", &franka::RobotState::O_T_EE_d) @@ -260,7 +265,7 @@ PYBIND11_MODULE(_frankx, m) { .def_readonly("time", &franka::RobotState::time); py::class_(m, "Robot") - .def(py::init(), "fci_ip"_a, "dynamic_rel"_a = 1.0, "repeat_on_error"_a = true, "stop_at_python_signal"_a = true) + .def(py::init(), "fci_ip"_a, "dynamic_rel"_a = 1.0, "repeat_on_error"_a = true, "stop_at_python_signal"_a = true, "realtime_config"_a = franka::RealtimeConfig::kEnforce) .def_readonly_static("max_translation_velocity", &Robot::max_translation_velocity) .def_readonly_static("max_rotation_velocity", &Robot::max_rotation_velocity) .def_readonly_static("max_elbow_velocity", &Robot::max_elbow_velocity) diff --git a/src/frankx/robot.cpp b/src/frankx/robot.cpp index 6a93363f..1526ee68 100644 --- a/src/frankx/robot.cpp +++ b/src/frankx/robot.cpp @@ -5,7 +5,7 @@ namespace frankx { -Robot::Robot(std::string fci_ip, double dynamic_rel, bool repeat_on_error, bool stop_at_python_signal): franka::Robot(fci_ip), fci_ip(fci_ip), velocity_rel(dynamic_rel), acceleration_rel(dynamic_rel), jerk_rel(dynamic_rel), repeat_on_error(repeat_on_error), stop_at_python_signal(stop_at_python_signal) { } +Robot::Robot(std::string fci_ip, double dynamic_rel, bool repeat_on_error, bool stop_at_python_signal, franka::RealtimeConfig realtime_config): franka::Robot(fci_ip,realtime_config), fci_ip(fci_ip), velocity_rel(dynamic_rel), acceleration_rel(dynamic_rel), jerk_rel(dynamic_rel), repeat_on_error(repeat_on_error), stop_at_python_signal(stop_at_python_signal) { } void Robot::setDefaultBehavior() { setCollisionBehavior(