diff --git a/examples/Get_position_during_robot_mouvement.py b/examples/Get_position_during_robot_mouvement.py new file mode 100644 index 00000000..5796a6eb --- /dev/null +++ b/examples/Get_position_during_robot_mouvement.py @@ -0,0 +1,55 @@ +""" + +Example of use of get_O_T_EE method + +Author: Maxime Edeline +Date: 28/07/2022 +Version: 0.1.0 + +Context : +This work was done during an internship under the supervision of Guenhael Le Quilliec, +associate professor at "Laboratoire de Mécanique Gabriel Lamé" of Polytech Tours, in Tours, France. +The aim of this work was to be able to synchronise the movements of Franka Emika Robot (Panda) +with other systems (e.g. 3D printer head attached to the robot). +The preferred solution was to modify frankx library. +The present Python code is an example of use of the modifications added to frankx source code. +In this example, the position of the robot is simply printed while it is moving. +This position can be used to easily synchronise any other system during the movements of the robot. + +""" + + +from argparse import ArgumentParser +from frankx import Affine, PathMotion, Robot + +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) + robot.set_default_behavior() + robot.recover_from_errors() + + # Reduce the acceleration and velocity dynamic + robot.set_dynamic_rel(0.15) + + # Define and move forwards + motion = PathMotion([ + Affine(0.5, 0.0, 0.35), + Affine(0.5, 0.25, 0.55), + Affine(0.5, 0.5, 0.35), + ], blend_max_distance=0.05) + + + pthread = robot.move_async(motion) + + while pthread.is_alive() : + O_T_EE_async = robot.get_O_T_EE_async() + print(O_T_EE_async) + + print("Execution completed") + + + diff --git a/include/frankx/motion_path_generator.hpp b/include/frankx/motion_path_generator.hpp index b141b956..f9b6c8b4 100644 --- a/include/frankx/motion_path_generator.hpp +++ b/include/frankx/motion_path_generator.hpp @@ -27,6 +27,10 @@ struct PathMotionGenerator: public MotionGenerator { Affine frame; PathMotion motion; MotionData& data; + + //variables to collect O_T_EE_async + std::array * path_O_T_EE_async = new std::array(); + std::array * temp_O_T_EE_async; 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 @@ -64,7 +68,15 @@ struct PathMotionGenerator: public MotionGenerator { } s_current = trajectory.states[trajectory_index].s; - return CartesianPose(trajectory.path.q(s_current, frame), use_elbow); + + //determine the current CartesianPose + franka::CartesianPose current_cartesian_pose = CartesianPose(trajectory.path.q(s_current, frame), use_elbow); + + temp_O_T_EE_async = new std::array(current_cartesian_pose.O_T_EE); + *path_O_T_EE_async = *temp_O_T_EE_async; + + + return current_cartesian_pose; } }; diff --git a/include/frankx/robot.hpp b/include/frankx/robot.hpp index 437177ab..6855330d 100644 --- a/include/frankx/robot.hpp +++ b/include/frankx/robot.hpp @@ -101,6 +101,12 @@ class Robot: public franka::Robot { bool move(WaypointMotion& motion, MotionData& data); bool move(const Affine& frame, WaypointMotion& motion); bool move(const Affine& frame, WaypointMotion& motion, MotionData& data); + + //variable to save O_T_EE_async + std::array * robot_O_T_EE_async = nullptr; + + //getter for O_T_EE_async + std::array get_O_T_EE_async(); }; -} // namespace frankx +} // namespace frankx \ No newline at end of file diff --git a/src/frankx/python.cpp b/src/frankx/python.cpp index cc081c50..19cfde23 100644 --- a/src/frankx/python.cpp +++ b/src/frankx/python.cpp @@ -309,7 +309,8 @@ PYBIND11_MODULE(_frankx, m) { .def("move", (bool (Robot::*)(WaypointMotion&)) &Robot::move, py::call_guard()) .def("move", (bool (Robot::*)(WaypointMotion&, MotionData&)) &Robot::move, py::call_guard()) .def("move", (bool (Robot::*)(const Affine&, WaypointMotion&)) &Robot::move, py::call_guard()) - .def("move", (bool (Robot::*)(const Affine&, WaypointMotion&, MotionData&)) &Robot::move, py::call_guard(), "frame"_a, "waypoint_motion"_a, "motion_data"_a); + .def("move", (bool (Robot::*)(const Affine&, WaypointMotion&, MotionData&)) &Robot::move, py::call_guard(), "frame"_a, "waypoint_motion"_a, "motion_data"_a) + .def("get_O_T_EE_async",&Robot::get_O_T_EE_async); py::class_(m, "GripperState") .def_readonly("width", &franka::GripperState::width) diff --git a/src/frankx/robot.cpp b/src/frankx/robot.cpp index 6a93363f..0cd743db 100644 --- a/src/frankx/robot.cpp +++ b/src/frankx/robot.cpp @@ -141,6 +141,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}; + + Robot::robot_O_T_EE_async = mg.path_O_T_EE_async; try { control(stateful(mg), controller_mode); @@ -213,4 +215,16 @@ bool Robot::move(const Affine& frame, WaypointMotion& motion, MotionData& data) return true; } +std::array Robot::get_O_T_EE_async(){ + + if(robot_O_T_EE_async == nullptr){ + std::cout<<"It is not possible to get O_T_EE_async now" << std::endl; + return std::array(); + } + + else{ + return *robot_O_T_EE_async; + } +} + } // namepace frankx