From c74d708e28daca516e24aafccedf3573facaeda4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Fri, 2 Feb 2024 08:41:10 +0100
Subject: [PATCH 01/32] Added build instructions for Windows.

---
 README.md | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 2829b71..13c979c 100644
--- a/README.md
+++ b/README.md
@@ -42,14 +42,26 @@ print(robot.operational_config)
 ```
 
 ### Setup and build ###
+For building libromocc, you need to have CMake installed. 
 
 ```bash
 git clone https://github.com/SINTEFMedtek/libromocc.git
 cd libromocc
 mkdir build
 cd build
+```
+
+#### Windows ####
+```bash
+cmake .. -G "Visual Studio 16 2019" -A x64 # Change generator string to reflect your VS version
+cmake --build . --config Release -j 4
+```
+
+#### Ubuntu ####
+
+```bash
 cmake ..
-make -j8
+make -j 4
 ```
 
 ### Usage ###

From 2895a406b43644f32d409108f1b936fe0c466232 Mon Sep 17 00:00:00 2001
From: androst <a.ostvik@gmail.com>
Date: Fri, 2 Feb 2024 14:52:51 +0100
Subject: [PATCH 02/32] Fixed bug with acceleration and velocity scaling. Issue
 when using joint controllers.

---
 source/romocc/manipulators/ur/UrMessageEncoder.cpp | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

diff --git a/source/romocc/manipulators/ur/UrMessageEncoder.cpp b/source/romocc/manipulators/ur/UrMessageEncoder.cpp
index 823e119..ce515ad 100644
--- a/source/romocc/manipulators/ur/UrMessageEncoder.cpp
+++ b/source/romocc/manipulators/ur/UrMessageEncoder.cpp
@@ -1,6 +1,5 @@
 #include "UrMessageEncoder.h"
 #include <stdio.h>
-#include <string.h>
 #include <iostream>
 
 namespace romocc
@@ -9,18 +8,21 @@ namespace romocc
 std::string UrMessageEncoder::moveCommand(MotionType typeOfMovement, Eigen::RowVectorXd targetConfiguration, double acc,
                                       double vel, double t, double rad)
 {
-    acc = acc/1000; // mm -> m
-    vel = vel/1000;
-    rad = rad/1000;
-
     if(typeOfMovement==MotionType::movej)
         return movej(targetConfiguration,acc,vel,t,rad);
     else if(typeOfMovement==MotionType::movep){
+        acc = acc/1000; // mm -> m
+        vel = vel/1000;
+        rad = rad/1000;
+
         targetConfiguration(Eigen::seq(0, 2)) = targetConfiguration(Eigen::seq(0, 2))/1000;
         return movep(targetConfiguration,acc,vel,t,rad);
     }
-    else if(typeOfMovement==MotionType::speedl)
+    else if(typeOfMovement==MotionType::speedl){
+        acc = acc/1000; // mm -> m
+        targetConfiguration = targetConfiguration/1000; // mm -> m
         return speedl(targetConfiguration,acc,t);
+    }
     else if(typeOfMovement == MotionType::speedj)
         return speedj(targetConfiguration,acc,t);
     else if(typeOfMovement == MotionType::stopj)

From 4b5e4440f75820d91f745a2161a135a613e554d4 Mon Sep 17 00:00:00 2001
From: androst <a.ostvik@gmail.com>
Date: Fri, 2 Feb 2024 14:54:12 +0100
Subject: [PATCH 03/32] Fixed issue with version comparison, was not
 considering if major is identical for minor check.

---
 source/romocc/manipulators/ur/UrMessageEncoder.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/source/romocc/manipulators/ur/UrMessageEncoder.cpp b/source/romocc/manipulators/ur/UrMessageEncoder.cpp
index ce515ad..00a8d3e 100644
--- a/source/romocc/manipulators/ur/UrMessageEncoder.cpp
+++ b/source/romocc/manipulators/ur/UrMessageEncoder.cpp
@@ -123,7 +123,7 @@ bool UrMessageEncoder::compareVersions(std::string version_0, std::string versio
     if(major_0 > major_1)
         return true;
     else{
-        if(minor_0 > minor_1)
+        if(major_0 == major_1 & minor_0 > minor_1)
             return true;
     }
     return false;

From 117962349c07948701c811ace9e3231c02365b4c Mon Sep 17 00:00:00 2001
From: androst <a.ostvik@gmail.com>
Date: Fri, 2 Feb 2024 14:56:09 +0100
Subject: [PATCH 04/32] Changed some functions to be private for improved
 control and documentation of Python side.

---
 source/pyromocc/source/pyromocc.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/source/pyromocc/source/pyromocc.cpp b/source/pyromocc/source/pyromocc.cpp
index 2f42990..c8d9b6d 100644
--- a/source/pyromocc/source/pyromocc.cpp
+++ b/source/pyromocc/source/pyromocc.cpp
@@ -24,16 +24,16 @@ PYBIND11_MODULE(pyromocc, m) {
     pybind11::class_<Robot> robot(m, "RobotBase");
     robot.def(py::init<>())
         .def("configure", &Robot::configure)
-        .def("connect", &Robot::connect, "Connects to the robot.")
         .def("get_state", &Robot::getCurrentState)
-        .def("stop_move", &Robot::stopMove);
+        .def("stop_move", &Robot::stopMove)
+        .def("_connect", &Robot::connect, "Connects to the robot.");
 
-    robot.def("movej", [](Robot& self, Eigen::Ref<const Eigen::RowVectorXd> target, double acc, double vel,
+    robot.def("_movej", [](Robot& self, Eigen::Ref<const Eigen::RowVectorXd> target, double acc, double vel,
             double time = 0, double blendRad = 0, bool wait=false){
             self.move(romocc::MotionType::movej, target, acc, vel, time, blendRad, wait);
     });
 
-    robot.def("movep", [](Robot& self, Eigen::Ref<const Eigen::MatrixXd> pose, double acc, double vel,
+    robot.def("_movep", [](Robot& self, Eigen::Ref<const Eigen::MatrixXd> pose, double acc, double vel,
             double time = 0, double blendRad = 0, bool wait=false){
         if(pose.rows() == 4 && pose.cols() == 4){
             Eigen::Affine3d transform;
@@ -44,16 +44,16 @@ PYBIND11_MODULE(pyromocc, m) {
         }
     });
 
-    robot.def("speedj", [](Robot& self, Eigen::Ref<const Eigen::RowVectorXd> target, double acc, double time=5){
+    robot.def("_speedj", [](Robot& self, Eigen::Ref<const Eigen::RowVectorXd> target, double acc=500, double time=0.5){
         self.move(romocc::MotionType::speedj, target, acc, 0, time);
     });
-    robot.def("speedl", [](Robot& self, Eigen::Ref<const Eigen::RowVectorXd> target, double acc, double time=5){
+    robot.def("_speedl", [](Robot& self, Eigen::Ref<const Eigen::RowVectorXd> target, double acc=500, double time=0.5){
         self.move(romocc::MotionType::speedl, target, acc, 0, time);
     });
-    robot.def("stopj", [](Robot& self, double acc){
+    robot.def("_stopj", [](Robot& self, double acc){
         self.stopMove(romocc::MotionType::stopj, acc);
     });
-    robot.def("stopl", [](Robot& self, double acc){
+    robot.def("_stopl", [](Robot& self, double acc){
         self.stopMove(romocc::MotionType::stopl, acc);
     });
 

From 48bc34d3ce6da664e4f79659fd45ac3cde1d0cd3 Mon Sep 17 00:00:00 2001
From: androst <a.ostvik@gmail.com>
Date: Fri, 2 Feb 2024 15:06:20 +0100
Subject: [PATCH 05/32] Added simple spacemouse object and example.

---
 .../pyromocc/examples/spacemouse_control.py   | 18 ++++++++++
 source/pyromocc/pyromocc/tools/__init__.py    |  1 +
 source/pyromocc/pyromocc/tools/spacemouse.py  | 35 +++++++++++++++++++
 3 files changed, 54 insertions(+)
 create mode 100644 source/pyromocc/examples/spacemouse_control.py
 create mode 100644 source/pyromocc/pyromocc/tools/spacemouse.py

diff --git a/source/pyromocc/examples/spacemouse_control.py b/source/pyromocc/examples/spacemouse_control.py
new file mode 100644
index 0000000..f328ca9
--- /dev/null
+++ b/source/pyromocc/examples/spacemouse_control.py
@@ -0,0 +1,18 @@
+import time
+
+from pyromocc.tools import SpaceMouse
+from pyromocc import Robot
+
+robot = Robot(ip="localhost", port=30003)
+robot.connect()
+
+space_mouse = SpaceMouse()
+space_mouse.connect()
+
+
+while True:
+    raw_state = space_mouse.get_operational_config(calibrated=True)
+    target_speed = [raw_state[0]*100, raw_state[1]*100, raw_state[2]*100,
+                    raw_state[3]*100, raw_state[4]*100, raw_state[5]*100]
+    robot.speedl(target_speed, 100, 0.5)
+    time.sleep(0.01)
diff --git a/source/pyromocc/pyromocc/tools/__init__.py b/source/pyromocc/pyromocc/tools/__init__.py
index 6b36b8b..484bf3f 100644
--- a/source/pyromocc/pyromocc/tools/__init__.py
+++ b/source/pyromocc/pyromocc/tools/__init__.py
@@ -1 +1,2 @@
 from .robotiq_gripper import RobotiqGripper
+from .spacemouse import SpaceMouse
diff --git a/source/pyromocc/pyromocc/tools/spacemouse.py b/source/pyromocc/pyromocc/tools/spacemouse.py
new file mode 100644
index 0000000..a1a97aa
--- /dev/null
+++ b/source/pyromocc/pyromocc/tools/spacemouse.py
@@ -0,0 +1,35 @@
+import time
+import warnings
+
+try:
+    import pyspacemouse
+except ImportError:
+    warnings.warn("pyspacemouse not installed. Please install it to use the SpaceMouse object.")
+
+
+class SpaceMouse(object):
+    def __init__(self):
+        self.calibration_state = None
+        self.is_connected = False
+
+    def connect(self):
+        self.is_connected = pyspacemouse.open()
+
+        # Warmup
+        t0 = time.time()
+        while time.time()-t0 < 1.0:
+            self.calibration_state = pyspacemouse.read()
+
+    @staticmethod
+    def get_state(self):
+        return pyspacemouse.read()
+
+    def get_operational_config(self, calibrated=False):
+        state = self.get_state()
+
+        if calibrated:
+            return [state.x-self.calibration_state.x, state.y-self.calibration_state.y,
+                    state.z-self.calibration_state.z, state.roll-self.calibration_state.roll,
+                    state.pitch-self.calibration_state.pitch, state.yaw-self.calibration_state.yaw]
+        else:
+            return [state.x, state.y, state.z, state.roll, state.pitch, state.yaw]

From bd16ea9173a5cf06a5d704c5fcb307b221a23d62 Mon Sep 17 00:00:00 2001
From: androst <a.ostvik@gmail.com>
Date: Fri, 2 Feb 2024 16:04:54 +0100
Subject: [PATCH 06/32] Added default values to motion functions.

---
 source/pyromocc/source/pyromocc.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/source/pyromocc/source/pyromocc.cpp b/source/pyromocc/source/pyromocc.cpp
index c8d9b6d..c0426d5 100644
--- a/source/pyromocc/source/pyromocc.cpp
+++ b/source/pyromocc/source/pyromocc.cpp
@@ -44,16 +44,16 @@ PYBIND11_MODULE(pyromocc, m) {
         }
     });
 
-    robot.def("_speedj", [](Robot& self, Eigen::Ref<const Eigen::RowVectorXd> target, double acc=500, double time=0.5){
+    robot.def("_speedj", [](Robot& self, Eigen::Ref<const Eigen::RowVectorXd> target, double acc=M_PI_4, double time=0.5){
         self.move(romocc::MotionType::speedj, target, acc, 0, time);
     });
     robot.def("_speedl", [](Robot& self, Eigen::Ref<const Eigen::RowVectorXd> target, double acc=500, double time=0.5){
         self.move(romocc::MotionType::speedl, target, acc, 0, time);
     });
-    robot.def("_stopj", [](Robot& self, double acc){
+    robot.def("_stopj", [](Robot& self, double acc=M_PI_2){
         self.stopMove(romocc::MotionType::stopj, acc);
     });
-    robot.def("_stopl", [](Robot& self, double acc){
+    robot.def("_stopl", [](Robot& self, double acc=500){
         self.stopMove(romocc::MotionType::stopl, acc);
     });
 

From 490c839f6fc95ddc6cce471199fee003fd499b9f Mon Sep 17 00:00:00 2001
From: androst <a.ostvik@gmail.com>
Date: Fri, 2 Feb 2024 16:06:39 +0100
Subject: [PATCH 07/32] Moved move functions to python object. Improved
 documentation. Removed unit definition to avoid confusion.

---
 source/pyromocc/pyromocc/robot.py | 215 +++++++++++++++++++++++++-----
 1 file changed, 184 insertions(+), 31 deletions(-)

diff --git a/source/pyromocc/pyromocc/robot.py b/source/pyromocc/pyromocc/robot.py
index bb89b12..267fa15 100644
--- a/source/pyromocc/pyromocc/robot.py
+++ b/source/pyromocc/pyromocc/robot.py
@@ -1,3 +1,6 @@
+import time
+import warnings
+
 from .pyromocc import *
 from typing import Union
 
@@ -13,25 +16,50 @@ class Robot(RobotBase):
         The robot IP address.
     port: int
         Port used for communicating.
-    manipulator: str, {'UR5', 'UR10'}
-        Manipulator type. Currently supports UR5 and UR10 from universal robotics. Defaults to 'UR5'.
+    manipulator: str, {'UR3', 'UR5', 'UR10', 'UR3e', 'UR5e', 'UR10e'}
+        Manipulator type. Currently, supports UR3, UR5, and UR10 from universal robotics. Defaults to 'UR5'.
     sw_version: str
         Controller software version. Format 'major.minor'. Defaults to "5.3".
 
     Attributes
     ----------
     pose : ndarray
-        Returns the instant 4x4 homogeneous matrix from base to end-effector
+        Returns the instant 4x4 homogeneous matrix from base to end-effector. The pose is in mm.
     pose_aa : list
-        A list of 6 values representing the axis-angle representation and translation of the end-effector wrt base
+        A list of 6 values representing the axis-angle representation and translation of the flange wrt base. The pose
+        is in mm.
+    joint_config : ndarray
+        Returns the instant joint configuration of the robot.
+    joint_velocity : ndarray
+        Returns the instant joint velocity of the robot.
+    operational_config : ndarray
+        Returns the instant operational configuration of the robot.
+    operational_velocity : ndarray
+        Returns the instant operational velocity of the robot.
+    operational_force : ndarray
+        Returns the instant operational force of the robot.
+
+
+    Methods
+    -------
+    connect()
+        Triggers robot connection process. Checks and confirms connection validity within a 5-second loop.
+        Raises a warning if the robot is not connected after 5 seconds. Returns connection status.
+    move_to_pose(pose, acceleration, velocity, wait=False, pose_units="mm")
+        Move to the specified pose. Pose is a 4x4 homogeneous transform.
+    translate(vec, acceleration, velocity, wait=False)
+        Translate the end-effector by the specified vector (x, y, z).
+    movep(pose, acceleration, velocity, time=0, blend_radius=0, wait=False)
+        Move the robot to the specified pose.
+
     """
 
-    def __init__(self, ip: str, port: int = 30003, manipulator: str = None, units="mm", sw_version="3.15"):
+    def __init__(self, ip: str, port: int = 30003, manipulator: str = None, sw_version="3.15"):
         RobotBase.__init__(self)
         self.ip = ip
         self.port = port
         self.sw_version = sw_version
-        self.units = units  # default unit is mm (millimetre)
+        self.is_connected = False
 
         if manipulator is None:
             manipulator = "UR5"
@@ -41,22 +69,24 @@ def __init__(self, ip: str, port: int = 30003, manipulator: str = None, units="m
         self.manipulator = Manipulator(manipulator_type, self.sw_version)
         self.configure(self.manipulator, self.ip, self.port)
 
-    @staticmethod
-    def _string_to_manipulator_type(manipulator):
-        if manipulator == 'UR3':
-            return ManipulatorType.UR3
-        elif manipulator == 'UR3e':
-            return ManipulatorType.UR3e
-        elif manipulator == 'UR5':
-            return ManipulatorType.UR5
-        elif manipulator == 'UR5e':
-            return ManipulatorType.UR5e
-        elif manipulator == 'UR10':
-            return ManipulatorType.UR10
-        elif manipulator == 'UR10e':
-            return ManipulatorType.UR10e
+    def connect(self):
+        """ Triggers robot connection process. Checks and confirms connection validity within a 5-second loop.
+        Raises a warning if the robot is not connected after 5 seconds. Returns connection status.
+        """
 
-    def move_to_pose(self, pose, acceleration, velocity, wait=False):
+        self.is_connected = self._connect()
+
+        t0 = time.time()
+        while not self._has_valid_state() or time.time()-t0 > 5.0:
+            time.sleep(0.1)
+            self.is_connected = self._has_valid_state()
+
+        if not self.is_connected:
+            warnings.warn("Robot not connected. Please check the IP and port.")
+
+        return self.is_connected
+
+    def move_to_pose(self, pose, acceleration, velocity, wait=False, pose_units="mm"):
         """
         Parameters
         ----------
@@ -68,18 +98,20 @@ def move_to_pose(self, pose, acceleration, velocity, wait=False):
             Velocity of motion
         wait: bool
             Wait for the motion to finish before returning
+        pose_units: str
+            Units of the pose, 'mm' or 'm'. Default is 'mm'.
         """
 
-        if self.units == "mm":
+        if pose_units == "mm":
             self.movep(pose, acceleration, velocity, 0, 0, wait)
-        elif self.units == "m":
+        elif pose_units == "m":
             # scale to mm before calling movep
             pose[:3, 3] *= 1000
             acceleration *= 1000
             velocity *= 1000
             self.movep(pose, acceleration, velocity, 0, 0, wait)
         else:
-            raise NotImplemented("Unit {} not supported.".format(self.units))
+            raise NotImplemented("Unit {} not supported.".format(pose_units))
 
     def translate(self, vec, acceleration, velocity, wait=False):
         """
@@ -87,7 +119,7 @@ def translate(self, vec, acceleration, velocity, wait=False):
 
         Parameters
         ----------
-        vec: ndarray, sequence (list, tuple)
+        vec: np.ndarray, sequence (list, tuple)
             Translate with the relative vector (x, y, z)
         acceleration: float
             Acceleration to velocity value
@@ -100,6 +132,101 @@ def translate(self, vec, acceleration, velocity, wait=False):
         pose[:3, 3] += vec
         self.movep(pose, acceleration, velocity, 0, 0, wait)
 
+    def movep(self, pose, acceleration, velocity, time=0, blend_radius=0, wait=False):
+        """
+        Move the robot to the specified pose.
+
+        Parameters
+        ----------
+        pose: np.ndarray, Sequence
+            Move to the specified pose. Pose is a 4x4 homogeneous transform or a Sequence of 6 values representing the
+            axis-angle representation and translation of the flange wrt base.
+        acceleration: float
+            Acceleration to velocity value
+        velocity: float
+            Velocity of motion (unit: mm/s)
+        time: float
+            Time to complete the motion. If 0, the robot will move with the specified velocity and acceleration.
+        blend_radius: float
+            Blend radius for the motion.
+        wait: bool
+            Wait for the motion to finish before returning.
+        """
+        self._movep(pose, acceleration, velocity, time, blend_radius, wait)
+
+    def movej(self, joint_config, acceleration, velocity, time=0, blend_radius=0, wait=False):
+        """
+        Move the robot to the specified joint configuration.
+
+        Parameters
+        ----------
+        joint_config: np.ndarray, Sequence
+            Move to the specified joint configuration.
+        acceleration: float
+            Acceleration to velocity value (rad/s^2)
+        velocity: float
+            Velocity of motion (rad/s)
+        time: float
+            Time to complete the motion. If 0, the robot will move with the specified velocity and acceleration.
+        blend_radius: float
+            Blend radius for the motion.
+        wait: bool
+            Wait for the motion to finish before returning.
+        """
+        self._movej(joint_config, acceleration, velocity, time, blend_radius, wait)
+
+    def speedl(self, velocity, acceleration, time=0.5):
+        """
+        Set the linear speed of the robot.
+
+        Parameters
+        ----------
+        velocity: np.ndarray, Sequence
+            Linear velocity (mm/s)
+        acceleration: float
+            Linear acceleration (mm/s^2)
+        time: float
+            Time to keep motion regardless of the velocity is reached.
+        """
+        self._speedl(velocity, acceleration, time)
+
+    def speedj(self, velocity, acceleration, time=0.5):
+        """
+        Set the joint speed of the robot.
+
+        Parameters
+        ----------
+        velocity: np.ndarray, Sequence
+            Joint velocity (rad/s)
+        acceleration: float
+            Joint acceleration (rad/s^2)
+        time: float
+            Time to keep motion regardless of the velocity is reached.
+        """
+        self._speedj(velocity, acceleration, time)
+
+    def stopl(self, acceleration=500):
+        """
+        Stops the robot with linear motion and given acceleration.
+
+        Parameters
+        ----------
+        acceleration: float
+            Linear acceleration (mm/s^2)
+        """
+        self._stopl(acceleration)
+
+    def stopj(self, acceleration=np.pi/4):
+        """
+        Stops the robot with joint motion and given joint acceleration.
+
+        Parameters
+        ----------
+        acceleration: float
+            Joint acceleration (rad/s^2)
+        """
+        self._stopj(acceleration)
+
     @property
     def joint_config(self):
         return self.get_state().get_joint_config()
@@ -123,13 +250,17 @@ def operational_force(self):
     @property
     def pose(self):
         pose = np.copy(self.get_state().get_pose())
-        if self.units == "mm":
+        return pose
+
+    def get_pose(self, pose_units="mm"):
+        pose = np.copy(self.get_state().get_pose())
+        if pose_units == "mm":
             return pose
-        elif self.units == "m":
+        elif pose_units == "m":
             pose[:3, 3] /= 1000
             return pose
         else:
-            raise NotImplemented("Unit {} not supported.".format(self.units))
+            raise NotImplemented("Unit {} not supported.".format(pose_units))
 
     @property
     def pose_aa(self):
@@ -208,16 +339,38 @@ def inverse_kinematics(self, pose):
         return joint_config
 
     def jacobian(self):
+        """ Returns the Jacobian matrix of the robot."""
         return self.get_state().get_jacobian()
 
     def inverse_jacobian(self):
+        """Returns the inverse Jacobian matrix of the robot."""
         return self.get_state().get_inverse_jacobian()
 
     def send_program(self, program: Union[str, bytes]):
-        """
-        Sends program to the robot in URScript format.
-        """
+        """ Sends program to the robot in URScript format. """
         program.strip()
         if not isinstance(program, bytes):
             program = program.encode()
         self._send_program(program)
+
+    def _has_valid_state(self):
+        """ Checks if robot is in a valid state by evaluating the joint configuration."""
+        joint_config = self.joint_config
+        return not np.any(np.abs(joint_config) > 2*np.pi)
+
+    @staticmethod
+    def _string_to_manipulator_type(manipulator):
+        """ Converts user provided manipulator type string to enumerated type. """
+
+        if manipulator == 'UR3':
+            return ManipulatorType.UR3
+        elif manipulator == 'UR3e':
+            return ManipulatorType.UR3e
+        elif manipulator == 'UR5':
+            return ManipulatorType.UR5
+        elif manipulator == 'UR5e':
+            return ManipulatorType.UR5e
+        elif manipulator == 'UR10':
+            return ManipulatorType.UR10
+        elif manipulator == 'UR10e':
+            return ManipulatorType.UR10e

From 2658c5e9c1ace886167bf467d17646311936d47a Mon Sep 17 00:00:00 2001
From: androst <a.ostvik@gmail.com>
Date: Fri, 2 Feb 2024 16:06:55 +0100
Subject: [PATCH 08/32] Added more tests on Python side.

---
 source/pyromocc/tests/pyromocc/test_robot.py | 57 ++++++++++++++++----
 1 file changed, 46 insertions(+), 11 deletions(-)

diff --git a/source/pyromocc/tests/pyromocc/test_robot.py b/source/pyromocc/tests/pyromocc/test_robot.py
index 614fc52..70c1387 100644
--- a/source/pyromocc/tests/pyromocc/test_robot.py
+++ b/source/pyromocc/tests/pyromocc/test_robot.py
@@ -1,17 +1,52 @@
-from unittest import TestCase
 import time
+import numpy as np
+
+from unittest import TestCase
 
-from pyromocc.robot import Robot
-from pyromocc.utilities.dummy_robot import DummyRobot
+from pyromocc import Robot
 
 
 class TestRobot(TestCase):
     def setUp(self) -> None:
-        self.fake_robot = DummyRobot()
-
-    def test_init_robot(self):
-        self.fake_robot.run()
-        robot = Robot(ip="localhost", port=30003)
-        robot.connect()
-        time.sleep(3)
-        self.fake_robot.close()
+        self.robot = Robot(ip="localhost", port=30003)
+        self.robot.connect()
+
+    def test_robot_connect(self):
+        self.assertTrue(self.robot.is_connected)
+
+    def test_operational_config(self):
+        assert self.robot.operational_config is not None
+
+    def test_robot_movej(self):
+        target_q = [-np.pi/2, -np.pi/2, -np.pi/2, -np.pi/2, np.pi/2, 0]
+        self.robot.movej(target_q, np.pi/4, np.pi/4, wait=True)
+        print(self.robot.joint_config)
+        self.assertTrue(np.allclose(self.robot.joint_config, target_q, atol=0.1))
+
+    def test_robot_movep(self):
+        target_pose_aa = [-100, -450, 280, 0, -np.pi/2, 0]
+        self.robot.movep(target_pose_aa, 100, 100, wait=True)
+        print(self.robot.operational_config)
+        self.assertTrue(np.allclose(self.robot.operational_config, target_pose_aa, atol=0.1))
+
+    def test_robot_speedl(self):
+        target_speed = [10, 0, 0, 0, 0, 0]
+        self.robot.speedl(target_speed, 100, 3)
+
+    def test_robot_speedj(self):
+        target_speed_joints = [-np.pi/2, 0, 0, 0, 0, 0]
+        self.robot.speedj(target_speed_joints, np.pi, time=1)
+
+    def test_robot_speedl_stopl(self):
+        target_speed = [0, 0, 50, 0, 0, 0]
+        self.robot.speedl(target_speed, 500, 5)
+        t0 = time.time()
+        while time.time()-t0 < 2:
+            print(self.robot.operational_velocity)
+        self.robot.stopl()
+
+    def test_robot_speedj_stopj(self):
+        target_speed_joints = [np.pi/4, 0, 0, 0, 0, 0]
+        self.robot.speedj(target_speed_joints, np.pi, time=1)
+        time.sleep(2)
+        self.robot.stopj()

From 0dde64f93947b694a42fe9635efbd06a1861f916 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Mon, 5 Feb 2024 14:38:06 +0100
Subject: [PATCH 09/32] Added additional unit specification in documentation.

---
 source/pyromocc/pyromocc/robot.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/source/pyromocc/pyromocc/robot.py b/source/pyromocc/pyromocc/robot.py
index 267fa15..60deef1 100644
--- a/source/pyromocc/pyromocc/robot.py
+++ b/source/pyromocc/pyromocc/robot.py
@@ -122,9 +122,9 @@ def translate(self, vec, acceleration, velocity, wait=False):
         vec: np.ndarray, sequence (list, tuple)
             Translate with the relative vector (x, y, z)
         acceleration: float
-            Acceleration to velocity value
+            Acceleration to velocity value (mm/s^2)
         velocity: float
-            Velocity of motion
+            Velocity of motion (mm/s)
         wait: bool
             Wait for the motion to finish before returning
         """
@@ -182,7 +182,7 @@ def speedl(self, velocity, acceleration, time=0.5):
         Parameters
         ----------
         velocity: np.ndarray, Sequence
-            Linear velocity (mm/s)
+            Linear velocity (x, y, z, rx, ry, rz) [mm/s, rad/s]
         acceleration: float
             Linear acceleration (mm/s^2)
         time: float

From 8626e566cad6aa473903d470922bced89c70161f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Mon, 5 Feb 2024 14:38:40 +0100
Subject: [PATCH 10/32] Fixed issue with rotation values being scaled.

---
 source/romocc/manipulators/ur/UrMessageEncoder.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/source/romocc/manipulators/ur/UrMessageEncoder.cpp b/source/romocc/manipulators/ur/UrMessageEncoder.cpp
index 00a8d3e..07e772e 100644
--- a/source/romocc/manipulators/ur/UrMessageEncoder.cpp
+++ b/source/romocc/manipulators/ur/UrMessageEncoder.cpp
@@ -20,7 +20,7 @@ std::string UrMessageEncoder::moveCommand(MotionType typeOfMovement, Eigen::RowV
     }
     else if(typeOfMovement==MotionType::speedl){
         acc = acc/1000; // mm -> m
-        targetConfiguration = targetConfiguration/1000; // mm -> m
+        targetConfiguration.head(3) /= 1000; // mm -> m
         return speedl(targetConfiguration,acc,t);
     }
     else if(typeOfMovement == MotionType::speedj)

From 5e079a7bd9a43be6b225e9a798463053df849555 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Mon, 5 Feb 2024 19:56:39 +0100
Subject: [PATCH 11/32] Added operational and joint velocity limiters on Python
 side.

---
 source/pyromocc/pyromocc/robot.py | 73 +++++++++++++++++++++++++++++--
 1 file changed, 70 insertions(+), 3 deletions(-)

diff --git a/source/pyromocc/pyromocc/robot.py b/source/pyromocc/pyromocc/robot.py
index 60deef1..ffe0e73 100644
--- a/source/pyromocc/pyromocc/robot.py
+++ b/source/pyromocc/pyromocc/robot.py
@@ -1,10 +1,10 @@
 import time
 import warnings
+import numpy as np
 
-from .pyromocc import *
-from typing import Union
+from typing import Sequence, Union
 
-import numpy as np
+from .pyromocc import *
 
 
 class Robot(RobotBase):
@@ -69,6 +69,12 @@ def __init__(self, ip: str, port: int = 30003, manipulator: str = None, sw_versi
         self.manipulator = Manipulator(manipulator_type, self.sw_version)
         self.configure(self.manipulator, self.ip, self.port)
 
+        self._operational_acceleration_limit = 2500  # 2500 mm/s^2
+        self._operational_velocity_limit = [250, 250, 250, np.pi, np.pi, np.pi]  # 250 mm/s, 1 rad/s
+
+        self._joint_acceleration_limit = 4*np.pi  # 4*pi rad/s^2
+        self._joint_velocity_limit = [np.pi, np.pi, np.pi, np.pi, np.pi, np.pi]  # 1 rad/s
+
     def connect(self):
         """ Triggers robot connection process. Checks and confirms connection validity within a 5-second loop.
         Raises a warning if the robot is not connected after 5 seconds. Returns connection status.
@@ -152,6 +158,7 @@ def movep(self, pose, acceleration, velocity, time=0, blend_radius=0, wait=False
         wait: bool
             Wait for the motion to finish before returning.
         """
+        self._verify_limits(acceleration, velocity, is_joint=False)
         self._movep(pose, acceleration, velocity, time, blend_radius, wait)
 
     def movej(self, joint_config, acceleration, velocity, time=0, blend_radius=0, wait=False):
@@ -173,6 +180,7 @@ def movej(self, joint_config, acceleration, velocity, time=0, blend_radius=0, wa
         wait: bool
             Wait for the motion to finish before returning.
         """
+        self._verify_limits(acceleration, velocity, is_joint=True)
         self._movej(joint_config, acceleration, velocity, time, blend_radius, wait)
 
     def speedl(self, velocity, acceleration, time=0.5):
@@ -188,6 +196,7 @@ def speedl(self, velocity, acceleration, time=0.5):
         time: float
             Time to keep motion regardless of the velocity is reached.
         """
+        self._verify_limits(acceleration, velocity, is_joint=False)
         self._speedl(velocity, acceleration, time)
 
     def speedj(self, velocity, acceleration, time=0.5):
@@ -203,6 +212,7 @@ def speedj(self, velocity, acceleration, time=0.5):
         time: float
             Time to keep motion regardless of the velocity is reached.
         """
+        self._verify_limits(acceleration, velocity, is_joint=True)
         self._speedj(velocity, acceleration, time)
 
     def stopl(self, acceleration=500):
@@ -353,6 +363,63 @@ def send_program(self, program: Union[str, bytes]):
             program = program.encode()
         self._send_program(program)
 
+    @property
+    def operational_acceleration_limit(self):
+        return self._operational_acceleration_limit
+
+    @operational_acceleration_limit.setter
+    def operational_acceleration_limit(self, value):
+        self._operational_acceleration_limit = value
+
+    @property
+    def operational_velocity_limit(self):
+        return self._operational_velocity_limit
+
+    @operational_velocity_limit.setter
+    def operational_velocity_limit(self, value: Sequence[float]):
+        if isinstance(value, float):
+            raise ValueError("Operational velocity limit must be a list of 2 or 6 values.")
+        elif isinstance(value, (list, tuple)):
+            if len(value) == 6:
+                self._operational_velocity_limit = value
+            elif len(value) == 2:
+                self._operational_velocity_limit = [value[0]]*3 + [value[1]]*3
+            else:
+                raise ValueError("Operational velocity limit must be a list of 6 values.")
+
+    @property
+    def joint_acceleration_limit(self):
+        return self._joint_acceleration_limit
+
+    @joint_acceleration_limit.setter
+    def joint_acceleration_limit(self, value: float):
+        self._joint_acceleration_limit = value
+
+    @property
+    def joint_velocity_limit(self):
+        return self._joint_velocity_limit
+
+    @joint_velocity_limit.setter
+    def joint_velocity_limit(self, value: Union[float, list]):
+        if isinstance(value, float):
+            self._joint_velocity_limit = [value]*6
+        elif isinstance(value, list):
+            if len(value) != 6:
+                raise ValueError("Joint velocity limit must be a list of 6 values.")
+            self._joint_velocity_limit = value
+
+    def _verify_limits(self, acceleration, velocity, is_joint=True):
+        if is_joint:
+            if np.any(np.abs(acceleration) > self.joint_acceleration_limit):
+                raise ValueError("Joint acceleration exceeds the limit.")
+            if np.any(np.abs(velocity) > self.joint_velocity_limit):
+                raise ValueError("Joint velocity exceeds the limit.")
+        else:
+            if np.any(np.abs(acceleration) > self.operational_acceleration_limit):
+                raise ValueError("Operational acceleration exceeds the limit.")
+            if np.any(np.abs(velocity) > self.operational_velocity_limit):
+                raise ValueError("Operational velocity exceeds the limit.")
+
     def _has_valid_state(self):
         """ Checks if robot is in a valid state by evaluating the joint configuration."""
         joint_config = self.joint_config

From 5bf70f5b5b7304f7ce3f19ea1ba5a831e5660a72 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Mon, 5 Feb 2024 19:58:03 +0100
Subject: [PATCH 12/32] Added tests for speedl rotation and limit exceeding
 motion calls.

---
 source/pyromocc/tests/pyromocc/test_robot.py | 30 ++++++++++++++++++++
 1 file changed, 30 insertions(+)

diff --git a/source/pyromocc/tests/pyromocc/test_robot.py b/source/pyromocc/tests/pyromocc/test_robot.py
index 70c1387..c66270a 100644
--- a/source/pyromocc/tests/pyromocc/test_robot.py
+++ b/source/pyromocc/tests/pyromocc/test_robot.py
@@ -5,6 +5,8 @@
 
 from pyromocc import Robot
 
+np.set_printoptions(precision=2, suppress=True)
+
 
 class TestRobot(TestCase):
     def setUp(self) -> None:
@@ -33,6 +35,10 @@ def test_robot_speedl(self):
         target_speed = [10, 0, 0, 0, 0, 0]
         self.robot.speedl(target_speed, 100, 3)
 
+    def test_robot_speedl_rx(self):
+        target_speed = [0, 0, 0, -np.pi/4, 0, 0]
+        self.robot.speedl(target_speed, 100, 3)
+
     def test_robot_speedj(self):
         target_speed_joints = [-np.pi/2, 0, 0, 0, 0, 0]
         self.robot.speedj(target_speed_joints, np.pi, time=1)
@@ -50,3 +56,27 @@ def test_robot_speedj_stopj(self):
         self.robot.speedj(target_speed_joints, np.pi, time=1)
         time.sleep(2)
         self.robot.stopj()
+
+    def test_robot_movep_exceed_limit(self):
+        target_pose_aa = [1000, 0, 0, 0, 0, 0]
+        self.robot.operational_config_limit = (1000, 1000, 1000, 1000, 1000, 1000)
+        with self.assertRaises(ValueError):
+            self.robot.movep(target_pose_aa, 100, 100, wait=True)
+
+    def test_robot_speedj_exceed_limit(self):
+        target_speed_joints = [np.pi/2, 0, 0, 0, 0, 0]
+        self.robot.joint_velocity_limit = np.pi/4
+        with self.assertRaises(ValueError):
+            self.robot.speedj(target_speed_joints, np.pi, time=1)
+
+    def test_robot_speedl_exceed_limit(self):
+        target_speed = [0, 0, 150, 2*np.pi, 0, 0]
+        self.robot.operational_velocity_limit = (100, np.pi)
+        with self.assertRaises(ValueError):
+            self.robot.speedl(target_speed, 500, 5)
+
+    def test_loop_stopj(self):
+        for i in range(125):
+            self.robot.stopj()
+            time.sleep(1/125.0)
+

From 29dc21f47e81932cb9afb752cf1409bb0053d228 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Tue, 13 Feb 2024 11:15:17 +0100
Subject: [PATCH 13/32] Added servoj and servoc to UR Message encoder. Extended
 move command for support.

---
 .../manipulators/ur/UrMessageEncoder.cpp      | 29 +++++++++++++++++++
 .../romocc/manipulators/ur/UrMessageEncoder.h |  6 +++-
 2 files changed, 34 insertions(+), 1 deletion(-)

diff --git a/source/romocc/manipulators/ur/UrMessageEncoder.cpp b/source/romocc/manipulators/ur/UrMessageEncoder.cpp
index 07e772e..c6dea08 100644
--- a/source/romocc/manipulators/ur/UrMessageEncoder.cpp
+++ b/source/romocc/manipulators/ur/UrMessageEncoder.cpp
@@ -25,8 +25,23 @@ std::string UrMessageEncoder::moveCommand(MotionType typeOfMovement, Eigen::RowV
     }
     else if(typeOfMovement == MotionType::speedj)
         return speedj(targetConfiguration,acc,t);
+    else if(typeOfMovement==MotionType::servoc)
+        return servoc(targetConfiguration, acc, vel, rad);
     else if(typeOfMovement == MotionType::stopj)
         return stopj(acc);
+    else if(typeOfMovement == MotionType::stopl)
+        return stopl(acc);
+    else
+        return std::string("Invalid motion type");
+}
+
+std::string UrMessageEncoder::moveCommand(MotionType typeOfMovement, Eigen::RowVectorXd targetConfiguration, double acc,
+                                          double vel, double t, double lookahead_time, double gain)
+{
+    if(typeOfMovement==MotionType::servoj)
+        return servoj(targetConfiguration,acc,vel,t,lookahead_time,gain);
+    else
+        return std::string("Invalid motion type");
 }
 
 std::string UrMessageEncoder::moveCommand(MotionType typeOfMovement, Eigen::Affine3d pose, double acc, double vel, double t, double radius)
@@ -49,6 +64,8 @@ std::string UrMessageEncoder::stopCommand(MotionType typeOfStop, double acc)
         return stopj(acc);
     else if(typeOfStop==MotionType::stopl)
         return stopl(acc);
+    else
+        return std::string("Invalid stop type");
 }
 
 std::string UrMessageEncoder::shutdownCommand()
@@ -85,6 +102,18 @@ std::string UrMessageEncoder::speedl(Eigen::RowVectorXd ov, double a, double t)
     return format("speedl([%f,%f,%f,%f,%f,%f],a=%f,t_min=%f)", ov(0), ov(1), ov(2), ov(3), ov(4), ov(5), a, t);
 }
 
+std::string UrMessageEncoder::servoj(Eigen::RowVectorXd jp, double a, double v, double t, double lookahead_time, double gain)
+{
+    if(mNewSWVersion)
+        return format("servoj([%f,%f,%f,%f,%f,%f],a=%f,v=%f,t=%f,lookahead_time=%f,gain=%f)", jp(0), jp(1), jp(2), jp(3), jp(4), jp(5), a, v, t, lookahead_time, gain);
+    return format("servoj([%f,%f,%f,%f,%f,%f],a=%f,v=%f,t=%f)", jp(0), jp(1), jp(2), jp(3), jp(4), jp(5), a, v, t);
+}
+
+std::string UrMessageEncoder::servoc(Eigen::RowVectorXd oc, double a, double v, double r)
+{
+    return format("servoc([%f,%f,%f,%f,%f,%f],a=%f,v=%f,r=%f)", oc(0), oc(1), oc(2), oc(3), oc(4), oc(5), a, v, r);
+}
+
 std::string UrMessageEncoder::stopl(double a)
 {
     return format("stopl(%f)", a);
diff --git a/source/romocc/manipulators/ur/UrMessageEncoder.h b/source/romocc/manipulators/ur/UrMessageEncoder.h
index 104548a..c2af404 100644
--- a/source/romocc/manipulators/ur/UrMessageEncoder.h
+++ b/source/romocc/manipulators/ur/UrMessageEncoder.h
@@ -22,13 +22,14 @@ class ROMOCC_EXPORT UrMessageEncoder : public MessageEncoder
         virtual std::string moveCommand(MotionType type, Eigen::RowVectorXd targetConfiguration, double acc, double vel, double t, double rad);
         virtual std::string moveCommand(MotionType type, Eigen::Affine3d targetPose, double acc, double vel, double t, double rad);
         virtual std::string moveCommand(MotionType type, double targetConfig[6], double acc, double vel, double t, double rad);
+        virtual std::string moveCommand(MotionType type, Eigen::RowVectorXd targetConfiguration, double acc, double vel, double t, double lookahead_time, double gain);
 
         virtual std::string stopCommand(MotionType typeOfStop, double acc);
         virtual std::string shutdownCommand();
 
         void setSoftwareVersion(std::string version);
 
-private:
+    private:
         std::string movej(Eigen::RowVectorXd jointConfig, double a, double v, double t, double r);
         std::string movep(Eigen::RowVectorXd operationalConfig, double a, double v, double t, double r);
         std::string movep(double operationalConfig[6], double a, double v, double t, double r);
@@ -36,6 +37,9 @@ class ROMOCC_EXPORT UrMessageEncoder : public MessageEncoder
         std::string speedj(Eigen::RowVectorXd jointVelocity, double a, double t);
         std::string speedl(Eigen::RowVectorXd operationalVelocity, double a, double t);
 
+        std::string servoj(Eigen::RowVectorXd jointPosition, double a, double v, double t, double lookahead_time, double gain);
+        std::string servoc(Eigen::RowVectorXd operationalConfig, double a, double v, double r);
+
         std::string stopj(double a);
         std::string stopl(double a);
 

From 433e7433ef660fed911bb3e534bb8ef16df059c8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Tue, 13 Feb 2024 11:15:55 +0100
Subject: [PATCH 14/32] Added servoj and servoc to valid MotionType.

---
 source/romocc/core/ForwardDeclarations.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/source/romocc/core/ForwardDeclarations.h b/source/romocc/core/ForwardDeclarations.h
index 0e7f938..228d036 100644
--- a/source/romocc/core/ForwardDeclarations.h
+++ b/source/romocc/core/ForwardDeclarations.h
@@ -46,7 +46,7 @@ struct ROMOCC_EXPORT Manipulator{
 };
 
 // Robot motion
-typedef enum {movej,movep,speedj,speedl,stopj,stopl} MotionType;
+typedef enum {movej,movep,speedj,speedl,servoj,servoc,stopj,stopl} MotionType;
 typedef std::vector<struct RobotMotion> MotionQueue;
 
 // Eigen robot typedefs

From 865f87dc235baa3520f84ec6eee60ea7d5cc2140 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Tue, 13 Feb 2024 11:16:33 +0100
Subject: [PATCH 15/32] Extended move function to support lookahead_time and
 gain parameters.

---
 source/romocc/communication/CommunicationInterface.h | 11 +++++++++++
 source/romocc/communication/MessageEncoder.h         |  1 +
 2 files changed, 12 insertions(+)

diff --git a/source/romocc/communication/CommunicationInterface.h b/source/romocc/communication/CommunicationInterface.h
index 5153d52..17d740e 100644
--- a/source/romocc/communication/CommunicationInterface.h
+++ b/source/romocc/communication/CommunicationInterface.h
@@ -35,6 +35,10 @@ class ROMOCC_EXPORT CommunicationInterface : public Object
 
         template <class TargetConfiguration>
         void move(MotionType typeOfMotion, TargetConfiguration target, double acc, double vel, double t, double rad);
+
+        template <class TargetConfiguration>
+        void move(MotionType typeOfMotion, TargetConfiguration target, double acc, double vel, double t, double lookahead_time, double gain);
+
         void stopMove(MotionType typeOfStop, double acc);
 
 private:
@@ -58,6 +62,13 @@ void CommunicationInterface::move(MotionType typeOfMotion, TargetConfiguration t
     sendMessage(mEncoder->moveCommand(typeOfMotion, target, acc, vel, t, rad));
 };
 
+template <class TargetConfiguration>
+void CommunicationInterface::move(MotionType typeOfMotion, TargetConfiguration target, double acc, double vel, double t, double lookahead_time, double gain)
+{
+    sendMessage(mEncoder->moveCommand(typeOfMotion, target, acc, vel, t, lookahead_time, gain));
+};
+
+
 }
 
 #endif //ROMOCC_COMMUNICATIONINTERFACE_H
diff --git a/source/romocc/communication/MessageEncoder.h b/source/romocc/communication/MessageEncoder.h
index 6904263..75ee834 100644
--- a/source/romocc/communication/MessageEncoder.h
+++ b/source/romocc/communication/MessageEncoder.h
@@ -13,6 +13,7 @@ class ROMOCC_EXPORT MessageEncoder : public Object
 {
     public:
         virtual std::string moveCommand(MotionType type, Eigen::RowVectorXd jointConfig, double acc, double vel, double t, double rad) = 0;
+        virtual std::string moveCommand(MotionType type, Eigen::RowVectorXd jointConfig, double acc, double vel, double t, double lookahead_time, double gain) = 0;
         virtual std::string moveCommand(MotionType type, Eigen::Affine3d pose, double acc, double vel, double t, double rad) = 0;
         virtual std::string moveCommand(MotionType type, double target[], double acc, double vel, double t, double rad) = 0;
         virtual std::string stopCommand(MotionType type, double acc) = 0;

From 1d24b2bb528d4978e302e630126964e30e5e8459 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Tue, 13 Feb 2024 11:17:29 +0100
Subject: [PATCH 16/32] Added functionality for wait, sleep and retrieving
 current time.

---
 source/romocc/Robot.cpp | 45 +++++++++++++++++++++++++++++++++++++++++
 source/romocc/Robot.h   |  4 ++++
 2 files changed, 49 insertions(+)

diff --git a/source/romocc/Robot.cpp b/source/romocc/Robot.cpp
index ab9a2ce..6b84c6e 100644
--- a/source/romocc/Robot.cpp
+++ b/source/romocc/Robot.cpp
@@ -105,12 +105,57 @@ void Robot::sendProgram(std::string program)
     mCommunicationInterface->sendMessage(program);
 }
 
+std::chrono::steady_clock::time_point Robot::currentTime()
+{
+    return std::chrono::steady_clock::now();
+}
+
 void Robot::addUpdateSubscription(std::function<void()> updateSignal)
 {
     mActiveSubscription = true;
     mThread = std::make_unique<std::thread>(std::bind(&Robot::startSubscription, this, updateSignal));
 }
 
+void Robot::wait(const std::chrono::steady_clock::time_point &t_cycle_start, double dt)
+{
+    auto t_app_stop = std::chrono::steady_clock::now();
+    auto t_app_duration = std::chrono::duration<double>(t_app_stop - t_cycle_start);
+    if (t_app_duration.count() < dt)
+    {
+        sleep(dt - t_app_duration.count());
+    }
+}
+
+void Robot::sleep(double seconds)
+{
+    static double estimate = 5e-3;
+    static double mean = 5e-3;
+    static double m2 = 0;
+    static int64_t count = 1;
+
+    while (seconds > estimate)
+    {
+        auto start = std::chrono::high_resolution_clock::now();
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        auto end = std::chrono::high_resolution_clock::now();
+
+        double observed = std::chrono::duration<double>(end - start).count();
+        seconds -= observed;
+
+        ++count;
+        double delta = observed - mean;
+        mean += delta / static_cast<double>(count);
+        m2 += delta * (observed - mean);
+        double stddev = sqrt(m2 / (static_cast<double>(count - 1)));
+        estimate = mean + stddev;
+    }
+
+    // Lock
+    auto start = std::chrono::high_resolution_clock::now();
+    while (std::chrono::duration<double>((std::chrono::high_resolution_clock::now() - start)).count() < seconds)
+        ;
+}
+
 void Robot::startSubscription(std::function<void()> updateSignal)
 {
     void *subscriber = zmq_socket(ZMQUtils::getContext(), ZMQ_SUB);
diff --git a/source/romocc/Robot.h b/source/romocc/Robot.h
index 5d3e297..8982dca 100644
--- a/source/romocc/Robot.h
+++ b/source/romocc/Robot.h
@@ -40,6 +40,10 @@ class ROMOCC_EXPORT Robot : public Object
 
         RobotCoordinateSystem::pointer getCoordinateSystem(){ return mCoordinateSystem;};
 
+        std::chrono::steady_clock::time_point currentTime();
+        void wait(const std::chrono::steady_clock::time_point &t_cycle_start, double dt = 1/125.0);
+        static void sleep(double seconds);
+
         Robot();
         ~Robot();
 

From ce97841a77cd33dec17933d55d90ce59b603f83b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Tue, 13 Feb 2024 11:18:08 +0100
Subject: [PATCH 17/32] Extended move command to support lookahead_time and
 gain for servoj.

---
 source/romocc/Robot.h | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/source/romocc/Robot.h b/source/romocc/Robot.h
index 8982dca..f17c92e 100644
--- a/source/romocc/Robot.h
+++ b/source/romocc/Robot.h
@@ -31,7 +31,11 @@ class ROMOCC_EXPORT Robot : public Object
         void addUpdateSubscription(std::function<void()> updateFunction);
 
         template <class Target>
-        void move(MotionType type, Target target, double acc, double vel, double t=0, double rad=0, bool wait=false);
+        void move(MotionType type, Target target, double acc, double vel, double t=0, double rad=0,  bool wait=false);
+
+        template <class Target>
+        void move(MotionType type, Target target, double acc, double vel, double t, double lookahead_time, double gain);
+
         void stopMove(MotionType type, double acc);
         void sendProgram(std::string program);
 
@@ -104,6 +108,11 @@ void Robot::move<double*>(MotionType type, double* target, double acc, double ve
     move(type, targetVector, acc, vel, t, rad, wait);
 }
 
+template <class Target>
+void Robot::move(MotionType type, Target target, double acc, double vel, double t, double lookahead_time, double gain)
+{
+    mCommunicationInterface->move(type, target, acc, vel, t, lookahead_time, gain);
+}
 
 } // namespace romocc
 

From cd2b1c5c2ba647ea367f2a867675e80d8add295b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Tue, 13 Feb 2024 11:19:46 +0100
Subject: [PATCH 18/32] Added tests for servoj, servoc, time functionality,
 wait and sleep. Also added a simple control loop for servoj.

---
 tests/RobotTests.cpp | 47 +++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 44 insertions(+), 3 deletions(-)

diff --git a/tests/RobotTests.cpp b/tests/RobotTests.cpp
index c6d6cba..ff99d96 100644
--- a/tests/RobotTests.cpp
+++ b/tests/RobotTests.cpp
@@ -51,10 +51,10 @@ TEST_CASE("Initialize robot and test kinematics", "[romocc][Robot]") {
 
 TEST_CASE("Initialize robot and listen for new states", "[romocc][Robot]") {
     Robot::pointer robot = Robot::New();
-    robot->configure(Manipulator(ManipulatorType::UR10), "192.168.153.131", 30003);
+    robot->configure(Manipulator(ManipulatorType::UR5), "localhost", 30003);
     robot->connect();
 
-    for(int i = 0; i<100; i++)
+    for(int i = 0; i<1000; i++)
     {
         std::cout << robot->getCurrentState()->getJointConfig().transpose() << std::endl;
     }
@@ -138,7 +138,7 @@ TEST_CASE("Add update subscription", "[romocc][Robot]"){
     };
 
     robot->addUpdateSubscription(print_message);
-    }
+}
 
 TEST_CASE("Transform to all joints", "[romocc][Robot]"){
     auto robot = Robot::New();
@@ -153,5 +153,46 @@ TEST_CASE("Transform to all joints", "[romocc][Robot]"){
     Transform3d m_bM6 = robot->getCurrentState()->getTransformToJoint(6);
 
     std::cout << m_bM2.matrix() << std::endl;
+}
+
+TEST_CASE("Move servoj", "[romocc][Robot]"){
+    Robot::pointer robot = Robot::New();
+    robot->configure(Manipulator(ManipulatorType::UR5), "localhost", 30003);
+    robot->connect();
+
+    std::cout << robot->getCurrentState()->getOperationalConfig() << std::endl;
+    Vector6d target_q = {-1.6007, -1.7271, -2.2030, -0.8080, 1.5951, -0.0310};
+    robot->move(MotionType::servoj, target_q, 500., 500., 1, 0.1, 300.);
+}
+
+TEST_CASE("Move servoc", "[romocc][Robot]"){
+    Robot::pointer robot = Robot::New();
+    robot->configure(Manipulator(ManipulatorType::UR5), "localhost", 30003);
+    robot->connect();
+
+    Vector6d target_oc = {-87.783, 574.295, 516.137, 3.035, -0.813, -0.};
+    robot->move(MotionType::servoc, target_oc, 500., 500., 10);
+}
+
+TEST_CASE("Move servoj with wait", "[romocc][Robot]"){
+    Robot::pointer robot = Robot::New();
+    robot->configure(Manipulator(ManipulatorType::UR5), "localhost", 30003);
+    robot->connect();
+
+    std::cout << robot->getCurrentState()->getOperationalConfig() << std::endl;
+    Vector6d target_q = {-1.6007, -1.7271, -2.2030, -0.8080, 1.5951, -0.0310};
+
+    robot->move(MotionType::movej, target_q, 500, 500, 1, 0, true);
+
+    // 125 Hz control loop for 2 seconds
+    for (unsigned int i=0; i<250; i++)
+    {
+        auto t_start = robot->currentTime();
+        robot->move(MotionType::servoj, target_q, 0., 0., 1/125.0, 0.1, 300.);
+        target_q[0] += 0.001;
+        target_q[1] += 0.001;
+        robot->wait(t_start);
     }
 }
+
+}   // namespace romocc
\ No newline at end of file

From d9e6b0453f43bdec00c495419eeadee32acdf10e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Tue, 13 Feb 2024 11:21:00 +0100
Subject: [PATCH 19/32] Added _servoj and _servoc to Python object. Added wait
 and current_time functions.

---
 source/pyromocc/source/pyromocc.cpp | 24 ++++++++++++++++++++----
 1 file changed, 20 insertions(+), 4 deletions(-)

diff --git a/source/pyromocc/source/pyromocc.cpp b/source/pyromocc/source/pyromocc.cpp
index c0426d5..13fea2c 100644
--- a/source/pyromocc/source/pyromocc.cpp
+++ b/source/pyromocc/source/pyromocc.cpp
@@ -1,12 +1,9 @@
-//
-// Created by androst on 31.03.20.
-//
-
 #include <iostream>
 
 #include <pybind11/pybind11.h>
 #include <pybind11/eigen.h>
 #include <pybind11/stl.h>
+#include <pybind11/chrono.h>
 
 #include "romocc/Robot.h"
 #include "romocc/utilities/MathUtils.h"
@@ -47,16 +44,33 @@ PYBIND11_MODULE(pyromocc, m) {
     robot.def("_speedj", [](Robot& self, Eigen::Ref<const Eigen::RowVectorXd> target, double acc=M_PI_4, double time=0.5){
         self.move(romocc::MotionType::speedj, target, acc, 0, time);
     });
+
     robot.def("_speedl", [](Robot& self, Eigen::Ref<const Eigen::RowVectorXd> target, double acc=500, double time=0.5){
         self.move(romocc::MotionType::speedl, target, acc, 0, time);
     });
+
+    robot.def("_servoj", [](Robot& self, Eigen::Ref<const Eigen::RowVectorXd> target, double acc=500, double vel=500, double time=1/125., double lookahead_time=0.1, double gain=300){
+        self.move(romocc::MotionType::servoj, target, acc, vel, time, lookahead_time, gain);
+    });
+
+    robot.def("_servoc", [](Robot& self, Eigen::Ref<const Eigen::RowVectorXd> target, double acc=500, double vel=500, double rad=10){
+        self.move(romocc::MotionType::servoc, target, acc, vel, rad);
+    });
+
     robot.def("_stopj", [](Robot& self, double acc=M_PI_2){
         self.stopMove(romocc::MotionType::stopj, acc);
     });
+
     robot.def("_stopl", [](Robot& self, double acc=500){
         self.stopMove(romocc::MotionType::stopl, acc);
     });
 
+    robot.def("wait", [](Robot& self, const std::chrono::steady_clock::time_point &t_cycle_start, double dt=1/125.){
+        self.wait(t_cycle_start, dt);
+    });
+
+    robot.def("current_time", &Robot::currentTime);
+
     robot.def("_send_program", &Robot::sendProgram);
 
     pybind11::class_<RobotState, std::shared_ptr<RobotState>> robotState(m, "RobotState");
@@ -106,6 +120,8 @@ PYBIND11_MODULE(pyromocc, m) {
         .value("movep", MotionType::movep)
         .value("speedj", MotionType::speedj)
         .value("speedl", MotionType::speedl)
+        .value("servoj", MotionType::servoj)
+        .value("servoc", MotionType::servoc)
         .value("stopj", MotionType::stopj)
         .value("stopl", MotionType::stopl);
 

From 7520a16509f50628d0a417880cbbf90be07104c2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Tue, 13 Feb 2024 11:21:26 +0100
Subject: [PATCH 20/32] Added servoj to python interface.

---
 source/pyromocc/pyromocc/robot.py | 21 +++++++++++++++++++--
 1 file changed, 19 insertions(+), 2 deletions(-)

diff --git a/source/pyromocc/pyromocc/robot.py b/source/pyromocc/pyromocc/robot.py
index ffe0e73..89fd4c3 100644
--- a/source/pyromocc/pyromocc/robot.py
+++ b/source/pyromocc/pyromocc/robot.py
@@ -215,6 +215,13 @@ def speedj(self, velocity, acceleration, time=0.5):
         self._verify_limits(acceleration, velocity, is_joint=True)
         self._speedj(velocity, acceleration, time)
 
+    def servoj(self, joint_config, acceleration, velocity, time, lookahead_time, gain):
+        self._verify_limits(acceleration, velocity, is_joint=True)
+        self._servoj(joint_config, acceleration, velocity, time, lookahead_time, gain)
+
+    def servoc(self, pose, acceleration, velocity, radius):
+        raise NotImplemented
+
     def stopl(self, acceleration=500):
         """
         Stops the robot with linear motion and given acceleration.
@@ -417,8 +424,18 @@ def _verify_limits(self, acceleration, velocity, is_joint=True):
         else:
             if np.any(np.abs(acceleration) > self.operational_acceleration_limit):
                 raise ValueError("Operational acceleration exceeds the limit.")
-            if np.any(np.abs(velocity) > self.operational_velocity_limit):
-                raise ValueError("Operational velocity exceeds the limit.")
+
+            if isinstance(velocity, (list, tuple)):
+                if len(velocity) == 6:
+                    if np.any(np.abs(velocity) > self.operational_velocity_limit):
+                        raise ValueError("Operational velocity exceeds the limit.")
+                elif len(velocity) == 2:
+                    if (np.any(np.abs(velocity[0]) > self.operational_velocity_limit[:3]) or
+                            np.any(np.abs(velocity[1]) > self.operational_velocity_limit[3:])):
+                        raise ValueError("Operational velocity exceeds the limit.")
+            else:
+                if np.any(np.abs(velocity) > self.operational_velocity_limit[:3]):
+                    raise ValueError("Operational velocity exceeds the limit")
 
     def _has_valid_state(self):
         """ Checks if robot is in a valid state by evaluating the joint configuration."""

From 8a5b5818e4c5721f78250a1b905cfb0fb71b36bc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Tue, 13 Feb 2024 11:21:59 +0100
Subject: [PATCH 21/32] Added tests for servoj, current_time and wait. Also
 added simple control loop for servoj.

---
 source/pyromocc/tests/pyromocc/test_robot.py | 21 ++++++++++++++++++++
 1 file changed, 21 insertions(+)

diff --git a/source/pyromocc/tests/pyromocc/test_robot.py b/source/pyromocc/tests/pyromocc/test_robot.py
index c66270a..0be64a4 100644
--- a/source/pyromocc/tests/pyromocc/test_robot.py
+++ b/source/pyromocc/tests/pyromocc/test_robot.py
@@ -75,6 +75,27 @@ def test_robot_speedl_exceed_limit(self):
         with self.assertRaises(ValueError):
             self.robot.speedl(target_speed, 500, 5)
 
+    def test_robot_servoj(self):
+        target_q = [-np.pi/2, -np.pi/2, -np.pi/2, -np.pi/2, np.pi/2, 0]
+        self.robot.servoj(target_q, 500, 250, 0.5, 0.1, 300.)
+
+    def test_robot_current_time(self):
+        print(self.robot.current_time())
+
+    def test_robot_wait(self):
+        self.robot.wait(0.5, 1/125.)
+
+    def test_robot_servoj_control_loop(self):
+        target_q = [-1.6007, -1.7271, -2.2030, -0.8080, 1.5951, -0.0310]
+        self.robot.movej(target_q, 0.5, 0.5, 0, 0, True)
+
+        for _ in range(250):
+            current_time = self.robot.current_time()
+            self.robot.servoj(target_q, 0.0, 0.0, 1/125.0, 0.1, 300.)
+            target_q[0] += 0.001
+            target_q[1] += 0.001
+            self.robot.wait(current_time, 1/125.0)
+
     def test_loop_stopj(self):
         for i in range(125):
             self.robot.stopj()

From a642ab02eb9ef5c5b15f7906bb3053df76580400 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Tue, 13 Feb 2024 17:49:21 +0100
Subject: [PATCH 22/32] Scale operational velocity to millimetres.

---
 source/romocc/robotics/RobotState.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/source/romocc/robotics/RobotState.cpp b/source/romocc/robotics/RobotState.cpp
index 0e793ca..2eb6bcf 100644
--- a/source/romocc/robotics/RobotState.cpp
+++ b/source/romocc/robotics/RobotState.cpp
@@ -59,7 +59,7 @@ void RobotState::setState(romocc::ConfigState configState) {
         mOperationalVelocity = configState.operationalVelocity;
         m_bMee = TransformUtils::Affine::toAffine3DFromVector6D(mOperationalConfiguration);
     } else{
-        mOperationalVelocity = getJacobian() * mJointVelocity;
+        mOperationalVelocity = TransformUtils::scaleTranslation(getJacobian() * mJointVelocity, 1000);
         m_bMee = transform_to_joint(configState.jointConfiguration);
         mOperationalConfiguration = TransformUtils::Affine::toVector6D(m_bMee);
     }

From 4203934aba9a7db788cbdcae3497b06d06146a7b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Tue, 13 Feb 2024 17:50:04 +0100
Subject: [PATCH 23/32] Fixed bug with operational_velocity property retrieving
 config instead of velocity.

---
 source/pyromocc/pyromocc/robot.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/source/pyromocc/pyromocc/robot.py b/source/pyromocc/pyromocc/robot.py
index 89fd4c3..350927d 100644
--- a/source/pyromocc/pyromocc/robot.py
+++ b/source/pyromocc/pyromocc/robot.py
@@ -258,7 +258,7 @@ def operational_config(self):
 
     @property
     def operational_velocity(self):
-        return self.get_state().get_operational_config()
+        return self.get_state().get_operational_velocity()
 
     @property
     def operational_force(self):

From 1685ffbde3b00f7db9307bdbf7f006ee9b144c2a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Wed, 14 Feb 2024 11:35:08 +0100
Subject: [PATCH 24/32] Added servol command.

---
 source/romocc/core/ForwardDeclarations.h           |  2 +-
 source/romocc/manipulators/ur/UrMessageEncoder.cpp | 11 ++++++++++-
 source/romocc/manipulators/ur/UrMessageEncoder.h   |  1 +
 tests/RobotTests.cpp                               |  9 +++++++++
 4 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/source/romocc/core/ForwardDeclarations.h b/source/romocc/core/ForwardDeclarations.h
index 228d036..c2112fb 100644
--- a/source/romocc/core/ForwardDeclarations.h
+++ b/source/romocc/core/ForwardDeclarations.h
@@ -46,7 +46,7 @@ struct ROMOCC_EXPORT Manipulator{
 };
 
 // Robot motion
-typedef enum {movej,movep,speedj,speedl,servoj,servoc,stopj,stopl} MotionType;
+typedef enum {movej,movep,speedj,speedl,servoj,servol,servoc,stopj,stopl} MotionType;
 typedef std::vector<struct RobotMotion> MotionQueue;
 
 // Eigen robot typedefs
diff --git a/source/romocc/manipulators/ur/UrMessageEncoder.cpp b/source/romocc/manipulators/ur/UrMessageEncoder.cpp
index c6dea08..609b6e2 100644
--- a/source/romocc/manipulators/ur/UrMessageEncoder.cpp
+++ b/source/romocc/manipulators/ur/UrMessageEncoder.cpp
@@ -6,7 +6,7 @@ namespace romocc
 {
 
 std::string UrMessageEncoder::moveCommand(MotionType typeOfMovement, Eigen::RowVectorXd targetConfiguration, double acc,
-                                      double vel, double t, double rad)
+                                          double vel, double t, double rad)
 {
     if(typeOfMovement==MotionType::movej)
         return movej(targetConfiguration,acc,vel,t,rad);
@@ -27,6 +27,10 @@ std::string UrMessageEncoder::moveCommand(MotionType typeOfMovement, Eigen::RowV
         return speedj(targetConfiguration,acc,t);
     else if(typeOfMovement==MotionType::servoc)
         return servoc(targetConfiguration, acc, vel, rad);
+    else if(typeOfMovement==MotionType::servol){
+        targetConfiguration.head(3) /= 1000; // mm -> m
+        return servol(targetConfiguration, t);
+    }
     else if(typeOfMovement == MotionType::stopj)
         return stopj(acc);
     else if(typeOfMovement == MotionType::stopl)
@@ -109,6 +113,11 @@ std::string UrMessageEncoder::servoj(Eigen::RowVectorXd jp, double a, double v,
     return format("servoj([%f,%f,%f,%f,%f,%f],a=%f,v=%f,t=%f)", jp(0), jp(1), jp(2), jp(3), jp(4), jp(5), a, v, t);
 }
 
+std::string UrMessageEncoder::servol(Eigen::RowVectorXd oc, double t)
+{
+    return format("servol(p[%f,%f,%f,%f,%f,%f],%f)", oc(0), oc(1), oc(2), oc(3), oc(4), oc(5), t);
+}
+
 std::string UrMessageEncoder::servoc(Eigen::RowVectorXd oc, double a, double v, double r)
 {
     return format("servoc([%f,%f,%f,%f,%f,%f],a=%f,v=%f,r=%f)", oc(0), oc(1), oc(2), oc(3), oc(4), oc(5), a, v, r);
diff --git a/source/romocc/manipulators/ur/UrMessageEncoder.h b/source/romocc/manipulators/ur/UrMessageEncoder.h
index c2af404..6d44920 100644
--- a/source/romocc/manipulators/ur/UrMessageEncoder.h
+++ b/source/romocc/manipulators/ur/UrMessageEncoder.h
@@ -38,6 +38,7 @@ class ROMOCC_EXPORT UrMessageEncoder : public MessageEncoder
         std::string speedl(Eigen::RowVectorXd operationalVelocity, double a, double t);
 
         std::string servoj(Eigen::RowVectorXd jointPosition, double a, double v, double t, double lookahead_time, double gain);
+        std::string servol(Eigen::RowVectorXd operationalConfig, double t);
         std::string servoc(Eigen::RowVectorXd operationalConfig, double a, double v, double r);
 
         std::string stopj(double a);
diff --git a/tests/RobotTests.cpp b/tests/RobotTests.cpp
index ff99d96..8272235 100644
--- a/tests/RobotTests.cpp
+++ b/tests/RobotTests.cpp
@@ -165,6 +165,15 @@ TEST_CASE("Move servoj", "[romocc][Robot]"){
     robot->move(MotionType::servoj, target_q, 500., 500., 1, 0.1, 300.);
 }
 
+TEST_CASE("Move servol", "[romocc][Robot]"){
+    Robot::pointer robot = Robot::New();
+    robot->configure(Manipulator(ManipulatorType::UR5e), "localhost", 30003);
+    robot->connect();
+
+    Vector6d target_oc = {-120.11, -431.76, 100, 0.00, -3.00, 0.00};
+    robot->move(MotionType::servol, target_oc, 0., 0., 5, 0.0, false);
+}
+
 TEST_CASE("Move servoc", "[romocc][Robot]"){
     Robot::pointer robot = Robot::New();
     robot->configure(Manipulator(ManipulatorType::UR5), "localhost", 30003);

From d6a2560d0ebd281b8ea664a7e6912ee29a8d3a6b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Wed, 14 Feb 2024 11:35:59 +0100
Subject: [PATCH 25/32] Added timestamp property.

---
 source/pyromocc/pyromocc/robot.py   | 4 ++++
 source/pyromocc/source/pyromocc.cpp | 1 +
 2 files changed, 5 insertions(+)

diff --git a/source/pyromocc/pyromocc/robot.py b/source/pyromocc/pyromocc/robot.py
index 350927d..c94d99b 100644
--- a/source/pyromocc/pyromocc/robot.py
+++ b/source/pyromocc/pyromocc/robot.py
@@ -264,6 +264,10 @@ def operational_velocity(self):
     def operational_force(self):
         return self.get_state().get_operational_force()
 
+    @property
+    def timestamp(self):
+        return self.get_state().get_timestamp()
+
     @property
     def pose(self):
         pose = np.copy(self.get_state().get_pose())
diff --git a/source/pyromocc/source/pyromocc.cpp b/source/pyromocc/source/pyromocc.cpp
index 13fea2c..a22ff55 100644
--- a/source/pyromocc/source/pyromocc.cpp
+++ b/source/pyromocc/source/pyromocc.cpp
@@ -79,6 +79,7 @@ PYBIND11_MODULE(pyromocc, m) {
     robotState.def("get_operational_config", &RobotState::getOperationalConfig);
     robotState.def("get_operational_velocity", &RobotState::getOperationalVelocity);
     robotState.def("get_operational_force", &RobotState::getOperationalForce);
+    robotState.def("get_timestamp", &RobotState::getTimestamp);
     robotState.def("joint_to_pose", [](RobotState& self, Eigen::Ref<const Eigen::RowVectorXd> joint_config){
         return self.jointConfigToOperationalConfig(joint_config).matrix();
     });

From 15e22ae0158ebe9f2ca02d0910d338725d1b8a44 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Wed, 14 Feb 2024 11:39:08 +0100
Subject: [PATCH 26/32] Added simple test for timestamps.

---
 source/pyromocc/tests/pyromocc/test_robot.py | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/source/pyromocc/tests/pyromocc/test_robot.py b/source/pyromocc/tests/pyromocc/test_robot.py
index 0be64a4..3b290b9 100644
--- a/source/pyromocc/tests/pyromocc/test_robot.py
+++ b/source/pyromocc/tests/pyromocc/test_robot.py
@@ -19,6 +19,16 @@ def test_robot_connect(self):
     def test_operational_config(self):
         assert self.robot.operational_config is not None
 
+    def test_robot_get_timestamp_update_loop(self):
+        timestamps = [self.robot.timestamp]
+        for _ in range(100000):
+            current_timestamp = self.robot.timestamp
+            if current_timestamp != timestamps[-1]:
+                timestamps.append(current_timestamp)
+        sample_time = np.average(np.diff(timestamps))
+        sample_rate = 1/sample_time
+        print(len(timestamps), sample_time, sample_rate)
+
     def test_robot_movej(self):
         target_q = [-np.pi/2, -np.pi/2, -np.pi/2, -np.pi/2, np.pi/2, 0]
         self.robot.movej(target_q, np.pi/4, np.pi/4, wait=True)

From d2b8a38a690de4a1765264ef2a4467169cf4a0d8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Wed, 14 Feb 2024 11:49:45 +0100
Subject: [PATCH 27/32] Added servol to python interface.

---
 source/pyromocc/pyromocc/robot.py   | 4 ++++
 source/pyromocc/source/pyromocc.cpp | 5 +++++
 2 files changed, 9 insertions(+)

diff --git a/source/pyromocc/pyromocc/robot.py b/source/pyromocc/pyromocc/robot.py
index c94d99b..b217a62 100644
--- a/source/pyromocc/pyromocc/robot.py
+++ b/source/pyromocc/pyromocc/robot.py
@@ -219,6 +219,10 @@ def servoj(self, joint_config, acceleration, velocity, time, lookahead_time, gai
         self._verify_limits(acceleration, velocity, is_joint=True)
         self._servoj(joint_config, acceleration, velocity, time, lookahead_time, gain)
 
+    def servol(self, pose, time):
+        # TODO: Check limits
+        self._servol(pose, time)
+
     def servoc(self, pose, acceleration, velocity, radius):
         raise NotImplemented
 
diff --git a/source/pyromocc/source/pyromocc.cpp b/source/pyromocc/source/pyromocc.cpp
index a22ff55..c7dbb43 100644
--- a/source/pyromocc/source/pyromocc.cpp
+++ b/source/pyromocc/source/pyromocc.cpp
@@ -53,6 +53,10 @@ PYBIND11_MODULE(pyromocc, m) {
         self.move(romocc::MotionType::servoj, target, acc, vel, time, lookahead_time, gain);
     });
 
+    robot.def("_servol", [](Robot& self, Eigen::Ref<const Eigen::RowVectorXd> target, double time=1/125.){
+        self.move(romocc::MotionType::servol, target, 0, 0, time);
+    });
+
     robot.def("_servoc", [](Robot& self, Eigen::Ref<const Eigen::RowVectorXd> target, double acc=500, double vel=500, double rad=10){
         self.move(romocc::MotionType::servoc, target, acc, vel, rad);
     });
@@ -122,6 +126,7 @@ PYBIND11_MODULE(pyromocc, m) {
         .value("speedj", MotionType::speedj)
         .value("speedl", MotionType::speedl)
         .value("servoj", MotionType::servoj)
+        .value("servol", MotionType::servol)
         .value("servoc", MotionType::servoc)
         .value("stopj", MotionType::stopj)
         .value("stopl", MotionType::stopl);

From 7b9a5f309679fd9d8c13cd6626456c9cc6be0e45 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Wed, 14 Feb 2024 11:50:03 +0100
Subject: [PATCH 28/32] Added simple servol test.

---
 source/pyromocc/tests/pyromocc/test_robot.py | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/source/pyromocc/tests/pyromocc/test_robot.py b/source/pyromocc/tests/pyromocc/test_robot.py
index 3b290b9..a855e09 100644
--- a/source/pyromocc/tests/pyromocc/test_robot.py
+++ b/source/pyromocc/tests/pyromocc/test_robot.py
@@ -89,6 +89,10 @@ def test_robot_servoj(self):
         target_q = [-np.pi/2, -np.pi/2, -np.pi/2, -np.pi/2, np.pi/2, 0]
         self.robot.servoj(target_q, 500, 250, 0.5, 0.1, 300.)
 
+    def test_robot_servol(self):
+        target_oc = [-120.11, -431.76, 200, 0.00, -3.00, 0.00]
+        self.robot.servol(target_oc, 5)
+
     def test_robot_current_time(self):
         print(self.robot.current_time())
 

From 4522557c5d8d304492db5c374e0cfa94a4d602bf Mon Sep 17 00:00:00 2001
From: Martin Albertsen Brandt <martinalbertsenbrandt@gmail.com>
Date: Wed, 14 Feb 2024 15:32:04 +0100
Subject: [PATCH 29/32] Quick fix of speedl verify limits bug

---
 source/pyromocc/pyromocc/robot.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/source/pyromocc/pyromocc/robot.py b/source/pyromocc/pyromocc/robot.py
index b217a62..69711a5 100644
--- a/source/pyromocc/pyromocc/robot.py
+++ b/source/pyromocc/pyromocc/robot.py
@@ -433,7 +433,10 @@ def _verify_limits(self, acceleration, velocity, is_joint=True):
             if np.any(np.abs(acceleration) > self.operational_acceleration_limit):
                 raise ValueError("Operational acceleration exceeds the limit.")
 
-            if isinstance(velocity, (list, tuple)):
+            if isinstance(velocity, float):
+                if np.any(np.abs(velocity) > self.operational_velocity_limit[:3]):
+                    raise ValueError("Operational velocity exceeds the limit.")
+            else:
                 if len(velocity) == 6:
                     if np.any(np.abs(velocity) > self.operational_velocity_limit):
                         raise ValueError("Operational velocity exceeds the limit.")
@@ -441,9 +444,6 @@ def _verify_limits(self, acceleration, velocity, is_joint=True):
                     if (np.any(np.abs(velocity[0]) > self.operational_velocity_limit[:3]) or
                             np.any(np.abs(velocity[1]) > self.operational_velocity_limit[3:])):
                         raise ValueError("Operational velocity exceeds the limit.")
-            else:
-                if np.any(np.abs(velocity) > self.operational_velocity_limit[:3]):
-                    raise ValueError("Operational velocity exceeds the limit")
 
     def _has_valid_state(self):
         """ Checks if robot is in a valid state by evaluating the joint configuration."""

From e86134e4d78401dc483819761e6f8b5a8c8477b6 Mon Sep 17 00:00:00 2001
From: ROMO MOCAP LAB <romo@lab.none>
Date: Wed, 21 Feb 2024 11:27:21 +0100
Subject: [PATCH 30/32] Quick fix of robot connect loop running forever

---
 source/pyromocc/pyromocc/robot.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/source/pyromocc/pyromocc/robot.py b/source/pyromocc/pyromocc/robot.py
index 69711a5..c1ffb78 100644
--- a/source/pyromocc/pyromocc/robot.py
+++ b/source/pyromocc/pyromocc/robot.py
@@ -83,7 +83,7 @@ def connect(self):
         self.is_connected = self._connect()
 
         t0 = time.time()
-        while not self._has_valid_state() or time.time()-t0 > 5.0:
+        while not self._has_valid_state() and time.time()-t0 < 5.0:
             time.sleep(0.1)
             self.is_connected = self._has_valid_state()
 

From edd11809de21c274831fecab0ead1ede4fb07b77 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Wed, 28 Feb 2024 14:57:04 +0100
Subject: [PATCH 31/32] Updated README with additional windows instructions.

---
 README.md | 20 ++++++--------------
 1 file changed, 6 insertions(+), 14 deletions(-)

diff --git a/README.md b/README.md
index 414cb27..5d89a0b 100644
--- a/README.md
+++ b/README.md
@@ -51,12 +51,6 @@ mkdir build
 cd build
 ```
 
-#### Windows ####
-```bash
-cmake .. -G "Visual Studio 16 2019" -A x64 # Change generator string to reflect your VS version
-cmake --build . --config Release -j 4
-```
-
 #### Ubuntu ####
 
 ```bash
@@ -64,16 +58,14 @@ cmake ..
 make -j 4
 ```
 
-### Setup and build (Windows) ###
-
-These instructions assume you have Visual Studio and CMake GUI available on your computer. 
+#### Windows ####
 ```bash
-git clone https://github.com/SINTEFMedtek/libromocc.git
-cd libromocc
-mkdir build
-cd build
-cmake-gui.exe ..
+cmake .. -G "Visual Studio 16 2019" -A x64 # Change generator string to reflect your VS version
+cmake --build . --config Release -j 4
 ```
+
+##### Visual studio #####
+
 Update the paths to the source code and the build directory as needed:
 - Source: ```<path_to>/libromocc/source```
 - Build: ```<path_to>/libromocc/build```

From 036bc8d422b5278b802fb819b307590645a2804e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20=C3=98stvik?= <a.ostvik@gmail.com>
Date: Wed, 28 Feb 2024 14:57:32 +0100
Subject: [PATCH 32/32] Added int as possible instance for velocity input.

---
 source/pyromocc/pyromocc/robot.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/source/pyromocc/pyromocc/robot.py b/source/pyromocc/pyromocc/robot.py
index c1ffb78..6dbf0fe 100644
--- a/source/pyromocc/pyromocc/robot.py
+++ b/source/pyromocc/pyromocc/robot.py
@@ -433,7 +433,7 @@ def _verify_limits(self, acceleration, velocity, is_joint=True):
             if np.any(np.abs(acceleration) > self.operational_acceleration_limit):
                 raise ValueError("Operational acceleration exceeds the limit.")
 
-            if isinstance(velocity, float):
+            if isinstance(velocity, (float, int)):
                 if np.any(np.abs(velocity) > self.operational_velocity_limit[:3]):
                     raise ValueError("Operational velocity exceeds the limit.")
             else: