From 52b7c3e7eb5b84188dca1d2ea8935faaa634e155 Mon Sep 17 00:00:00 2001
From: Baris Yazici <baris.yazici@franka.de>
Date: Thu, 18 Jan 2024 15:44:21 +0100
Subject: [PATCH 1/2] bump version compatible with system image 5.5.0

remove the temporary workarounds fixed in the system image
---
 CHANGELOG.md       |  7 +++++++
 CMakeLists.txt     |  2 +-
 common             |  2 +-
 src/robot.cpp      | 13 ++++++-------
 src/robot_impl.cpp | 22 +++-------------------
 5 files changed, 18 insertions(+), 28 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 1a570f3f..6d7445c2 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,5 +1,12 @@
 # CHANGELOG
 
+## 0.13.3 - 2024-01-18
+
+Requires Franka Research 3 system version >= 5.5.0
+
+ * Bump libfranka-common version compatible with 5.5.0
+ * Delete the temporary-workaround max-path-pose deviation. Fixed in the system image.
+
 ## 0.13.2 - 2023-12-04
 
  * Hotfix: (temporary-workaround) for max-path-pose-deviation in ExternalMode for active control.
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 01b19840..55b58d25 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.4)
 
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
 
-set(libfranka_VERSION 0.13.2)
+set(libfranka_VERSION 0.13.3)
 
 project(libfranka
   VERSION ${libfranka_VERSION}
diff --git a/common b/common
index 5adeec65..917979c7 160000
--- a/common
+++ b/common
@@ -1 +1 @@
-Subproject commit 5adeec6566d6badf5a7ed99326ee212103328bf1
+Subproject commit 917979c73709832ea2e75040f555e6e3e14465a8
diff --git a/src/robot.cpp b/src/robot.cpp
index cd5558b1..345024c2 100644
--- a/src/robot.cpp
+++ b/src/robot.cpp
@@ -55,13 +55,12 @@ void Robot::control(std::function<Torques(const RobotState&, franka::Duration)>
                     double cutoff_frequency) {
   std::unique_lock<std::mutex> control_lock(control_mutex_, std::try_to_lock);
   assertOwningLock(control_lock);
-  // temporary workaround for the max_path_pose_deviation issue
-  limit_rate = true;
-  ControlLoop<JointPositions> loop(*impl_, std::move(control_callback),
-                                   [](const RobotState& robot_state, Duration) -> JointPositions {
-                                     return JointPositions(robot_state.q);
-                                   },
-                                   limit_rate, cutoff_frequency);
+
+  ControlLoop<JointVelocities> loop(*impl_, std::move(control_callback),
+                                    [](const RobotState&, Duration) -> JointVelocities {
+                                      return {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+                                    },
+                                    limit_rate, cutoff_frequency);
   loop();
 }
 
diff --git a/src/robot_impl.cpp b/src/robot_impl.cpp
index c4b264c7..785fcda6 100644
--- a/src/robot_impl.cpp
+++ b/src/robot_impl.cpp
@@ -8,9 +8,6 @@
 
 #include "load_calculations.h"
 
-#include <franka/lowpass_filter.h>
-#include <franka/rate_limiting.h>
-
 namespace franka {
 
 namespace {
@@ -100,25 +97,12 @@ RobotState Robot::Impl::readOnce() {
 void Robot::Impl::writeOnce(const Torques& control_input) {
   research_interface::robot::ControllerCommand control_command =
       createControllerCommand(control_input);
-  research_interface::robot::MotionGeneratorCommand command{};
-  double cutoff_frequency{100.0};
-
-  // Temporary workaround for the max-path-pose-deviation bug[BFFTRAC-1639]
-  JointVelocities motion(current_state_.dq);
-  command.dq_c = motion.dq;
-  for (size_t i = 0; i < 7; i++) {
-    command.dq_c[i] =
-        lowpassFilter(kDeltaT, command.dq_c[i], current_state_.dq_d[i], cutoff_frequency);
-  }
-  command.dq_c =
-      limitRate(computeUpperLimitsJointVelocity(current_state_.q_d),
-                computeLowerLimitsJointVelocity(current_state_.q_d), kMaxJointAcceleration,
-                kMaxJointJerk, command.dq_c, current_state_.dq_d, current_state_.ddq_d);
-  checkFinite(command.dq_c);
+  research_interface::robot::MotionGeneratorCommand motion_command{};
+  motion_command.dq_c = {0, 0, 0, 0, 0, 0, 0};
 
   network_->tcpThrowIfConnectionClosed();
 
-  sendRobotCommand(&command, &control_command);
+  sendRobotCommand(&motion_command, &control_command);
 }
 
 template <typename MotionGeneratorType>

From b60e9fdcb9ac8697b36f585e7f856a72a99893a0 Mon Sep 17 00:00:00 2001
From: Baris Yazici <baris.yazici@franka.de>
Date: Thu, 25 Jan 2024 13:15:10 +0100
Subject: [PATCH 2/2] bump robot version in common

---
 common | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/common b/common
index 917979c7..dd768c88 160000
--- a/common
+++ b/common
@@ -1 +1 @@
-Subproject commit 917979c73709832ea2e75040f555e6e3e14465a8
+Subproject commit dd768c882855801b64b37ab82a05a4db11fa8166