Merge branch 'hotfix/max-path-pose-for-active-control' into fr3-develop

CHANGELOG.md

@@ -1,5 +1,10 @@
 # CHANGELOG
 
+## 0.13.2 - 2023-12-04
+
+ * Hotfix: (temporary-workaround) for max-path-pose-deviation in ExternalMode for active control.
+
+
 ## 0.13.1 - 2023-11-30
 
  * Hotfix: (temporary-workaround) for max-path-pose-deviation in ExternalMode for callback based control.

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.1)
+set(libfranka_VERSION 0.13.2)
 
 project(libfranka
   VERSION ${libfranka_VERSION}

src/robot_impl.cpp

@@ -8,6 +8,9 @@
 
 #include "load_calculations.h"
 
+#include <franka/lowpass_filter.h>
+#include <franka/rate_limiting.h>
+
 namespace franka {
 
 namespace {
@@ -90,20 +93,32 @@ RobotState Robot::Impl::readOnce() {
   research_interface::robot::RobotState robot_state;
   while (network_->udpReceive<decltype(robot_state)>(&robot_state)) {
   }
-
-  return convertRobotState(receiveRobotState());
+  current_state_ = convertRobotState(receiveRobotState());
+  return current_state_;
 }
 
 void Robot::Impl::writeOnce(const Torques& control_input) {
   research_interface::robot::ControllerCommand control_command =
       createControllerCommand(control_input);
-
-  research_interface::robot::MotionGeneratorCommand motion_command{};
-  motion_command.dq_c = {0, 0, 0, 0, 0, 0, 0};
+  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);
 
   network_->tcpThrowIfConnectionClosed();
 
-  sendRobotCommand(&motion_command, &control_command);
+  sendRobotCommand(&command, &control_command);
 }
 
 template <typename MotionGeneratorType>

src/robot_impl.h

@@ -107,6 +107,8 @@ class Robot::Impl : public RobotControl {
   bool controllerRunning() const noexcept;
 
  private:
+  RobotState current_state_;
+
   template <typename MotionGeneratorType>
   void writeOnce(const MotionGeneratorType& motion_generator_input);
 

test/robot_impl_tests.cpp

@@ -251,7 +251,6 @@ TEST(RobotImpl, CanStartExternalControllerMotion) {
   server
       .onReceiveRobotCommand([=](const RobotCommand& command) {
         EXPECT_EQ(kMessageID, command.message_id);
-        testMotionGeneratorCommandsAreEqual(sent_command.motion, command.motion);
         testControllerCommandsAreEqual(sent_command.control, command.control);
       })
       .spinOnce();