From 87fb1c20ea9fbf74a6d7db7dfa88cd760f7e6ee5 Mon Sep 17 00:00:00 2001
From: Carolin Nowak <cano@mmmi.sdu.dk>
Date: Mon, 13 Jan 2025 15:47:46 +0100
Subject: [PATCH] Added python bindings and example for operational space
 controller.py

---
 .../python/operational_space_controller.py    | 75 +++++++++++++++++++
 python/sdu_controllers/__init__.py            |  3 +
 python/sdu_controllers/_sdu_controllers.cpp   | 16 ++++
 3 files changed, 94 insertions(+)
 create mode 100644 examples/ur_examples/python/operational_space_controller.py

diff --git a/examples/ur_examples/python/operational_space_controller.py b/examples/ur_examples/python/operational_space_controller.py
new file mode 100644
index 0000000..a34fa07
--- /dev/null
+++ b/examples/ur_examples/python/operational_space_controller.py
@@ -0,0 +1,75 @@
+import numpy as np
+import time
+import csv
+from numpy import genfromtxt
+from scipy.spatial.transform import Rotation
+import sdu_controllers
+
+
+my_data = genfromtxt('examples/data/cartesian_trajectory_safe.csv', delimiter=',')
+
+freq = 500.0
+dt = 1.0 / freq
+
+Kp_pos_val = 500
+Kp_orient_val = 100
+Kd_pos_val = 2 * np.sqrt(Kp_pos_val)
+Kd_orient_val = 2 * np.sqrt(Kp_orient_val)
+N_val = 1
+
+Kp = np.diag([Kp_pos_val, Kp_pos_val, Kp_pos_val, Kp_orient_val, Kp_orient_val, Kp_orient_val])
+Kd = np.diag([Kd_pos_val, Kd_pos_val, Kd_pos_val, Kd_orient_val, Kd_orient_val, Kd_orient_val])
+# N = 
+
+ur_robot = sdu_controllers.URRobotModel()
+osc_controller = sdu_controllers.OperationalSpaceController(Kp, Kd, ur_robot)
+inv_dyn_jnt_space = sdu_controllers.InverseDynamicsJointSpace(ur_robot)
+fwd_dyn = sdu_controllers.ForwardDynamics(ur_robot)
+
+q = np.array([0.0, -1.5707, -1.5707, -1.5707, 1.5707, 0.0])
+dq = np.zeros(6)
+
+output_data = []
+start_total = time.time()
+for row in my_data:
+    x_d = np.array(row[0:6])
+    dx_d = np.array(row[6:12])
+    ddx_d = np.array(row[12:18])
+
+    q_meas = q
+    dq_meas = dq
+    # 
+
+    # Controller 
+    osc_controller.step(x_d, dx_d, ddx_d, q_meas, dq_meas)
+    y = osc_controller.get_output()
+    print('y:', y)
+    tau = inv_dyn_jnt_space.inverse_dynamics(y, q_meas, dq_meas)
+    print('tau', tau)
+
+    # Simulation
+    ddq = fwd_dyn.forward_dynamics(q, dq, tau)
+    # integrate to get velocity
+    dq += ddq * dt
+    # integrate to get position
+    q += dq * dt
+
+    print('q:', q)
+    T = sdu_controllers.forward_kinematics(q, ur_robot)
+    pos = T[0:3, 3]
+    print('pos:', pos)
+    rot_mat = T[0:3, 0:3]
+    rpy_zyz = Rotation.from_matrix(rot_mat).as_euler('zyz', degrees=False)
+    rpy_zyz = rpy_zyz[[2, 1, 0]]
+    print('rpy_zyz:', rpy_zyz)
+
+    output_data.append(np.hstack([q, pos, rpy_zyz]))
+
+with open("cartesian_output_python.csv", "w") as f_stream:
+    csv_writer = csv.writer(f_stream)
+    csv_writer.writerows(output_data)
+
+del ur_robot
+del osc_controller
+del inv_dyn_jnt_space
+del fwd_dyn
\ No newline at end of file
diff --git a/python/sdu_controllers/__init__.py b/python/sdu_controllers/__init__.py
index a573330..60da867 100644
--- a/python/sdu_controllers/__init__.py
+++ b/python/sdu_controllers/__init__.py
@@ -3,9 +3,12 @@
 from sdu_controllers._sdu_controllers import URRobotModel
 from sdu_controllers._sdu_controllers import BreedingBlanketHandlingRobotModel
 from sdu_controllers._sdu_controllers import PDController
+from sdu_controllers._sdu_controllers import OperationalSpaceController
 from sdu_controllers._sdu_controllers import AdmittanceControllerPosition
 from sdu_controllers._sdu_controllers import InverseDynamicsJointSpace
 from sdu_controllers._sdu_controllers import ForwardDynamics
+from sdu_controllers._sdu_controllers import forward_kinematics
+
 
 # Export the version given in project metadata
 from importlib import metadata
diff --git a/python/sdu_controllers/_sdu_controllers.cpp b/python/sdu_controllers/_sdu_controllers.cpp
index c20abe3..de92b06 100644
--- a/python/sdu_controllers/_sdu_controllers.cpp
+++ b/python/sdu_controllers/_sdu_controllers.cpp
@@ -3,7 +3,9 @@
 #include <nanobind/stl/shared_ptr.h>
 
 #include <sdu_controllers/controllers/pd_controller.hpp>
+#include <sdu_controllers/controllers/operational_space_controller.hpp>
 #include <sdu_controllers/controllers/admittance_controller_position.hpp>
+#include <sdu_controllers/kinematics/forward_kinematics.hpp>
 #include <sdu_controllers/math/forward_dynamics.hpp>
 #include <sdu_controllers/math/inverse_dynamics_joint_space.hpp>
 #include <sdu_controllers/models/breeding_blanket_handling_robot_model.hpp>
@@ -60,6 +62,17 @@ namespace sdu_controllers
         .def("get_output", &controllers::PDController::get_output)
         .def("reset", &controllers::PDController::reset);
 
+    nb::class_<controllers::OperationalSpaceController>(m, "OperationalSpaceController")
+        .def(
+            nb::init<Eigen::MatrixXd, Eigen::MatrixXd, std::shared_ptr<models::RobotModel>>(),
+            nb::arg("Kp"), 
+            nb::arg("Kd"), 
+            nb::arg("robot_model")
+        )
+        .def("step", &controllers::OperationalSpaceController::step)
+        .def("get_output", &controllers::OperationalSpaceController::get_output)
+        .def("reset", &controllers::OperationalSpaceController::reset);
+
     nb::class_<controllers::AdmittanceControllerPosition>(m, "AdmittanceControllerPosition")
         .def(nb::init<const double>(), nb::arg("frequency"))
         .def("step", &controllers::AdmittanceControllerPosition::step)
@@ -81,6 +94,9 @@ namespace sdu_controllers
     nb::class_<math::ForwardDynamics>(m, "ForwardDynamics")
         .def(nb::init<std::shared_ptr<models::RobotModel>>())
         .def("forward_dynamics", &math::ForwardDynamics::forward_dynamics);
+
+    // kinematics
+    m.def("forward_kinematics", &kinematics::forward_kinematics); 
   }
 
 }  // namespace sdu_controllers