Get O_T_EE while moving

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")
+
+
+

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<double, 16> * path_O_T_EE_async = new std::array<double,16>();
+    std::array<double, 16> * 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<double,16>(current_cartesian_pose.O_T_EE);
+        *path_O_T_EE_async = *temp_O_T_EE_async;
+
+
+        return current_cartesian_pose;
     }
 };
 

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<double,16> * robot_O_T_EE_async = nullptr;
+
+    //getter for O_T_EE_async
+    std::array<double,16> get_O_T_EE_async();
 };
 
-} // namespace frankx
+} // namespace frankx

src/frankx/python.cpp

@@ -309,7 +309,8 @@ PYBIND11_MODULE(_frankx, m) {
         .def("move", (bool (Robot::*)(WaypointMotion&)) &Robot::move, py::call_guard<py::gil_scoped_release>())
         .def("move", (bool (Robot::*)(WaypointMotion&, MotionData&)) &Robot::move, py::call_guard<py::gil_scoped_release>())
         .def("move", (bool (Robot::*)(const Affine&, WaypointMotion&)) &Robot::move, py::call_guard<py::gil_scoped_release>())
-        .def("move", (bool (Robot::*)(const Affine&, WaypointMotion&, MotionData&)) &Robot::move, py::call_guard<py::gil_scoped_release>(), "frame"_a, "waypoint_motion"_a, "motion_data"_a);
+        .def("move", (bool (Robot::*)(const Affine&, WaypointMotion&, MotionData&)) &Robot::move, py::call_guard<py::gil_scoped_release>(), "frame"_a, "waypoint_motion"_a, "motion_data"_a)
+        .def("get_O_T_EE_async",&Robot::get_O_T_EE_async);
 
     py::class_<franka::GripperState>(m, "GripperState")
         .def_readonly("width", &franka::GripperState::width)

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<Robot> mg {this, frame, motion, data};
+
+    Robot::robot_O_T_EE_async = mg.path_O_T_EE_async;
 
     try {
         control(stateful<franka::CartesianPose>(mg), controller_mode);
@@ -213,4 +215,16 @@ bool Robot::move(const Affine& frame, WaypointMotion& motion, MotionData& data)
     return true;
 }
 
+std::array<double,16> 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<double,16>();
+    }
+
+    else{
+        return *robot_O_T_EE_async;
+    }
+}
+
 } // namepace frankx