diff --git a/CHANGELOG.md b/CHANGELOG.md index f7e69d1b..1a570f3f 100644 --- a/CHANGELOG.md +++ b/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. diff --git a/CMakeLists.txt b/CMakeLists.txt index f06fdc4e..06b18081 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.1) +set(libfranka_VERSION 0.13.2) project(libfranka VERSION ${libfranka_VERSION} diff --git a/src/robot_impl.cpp b/src/robot_impl.cpp index e1eb7fcc..edfa0dff 100644 --- a/src/robot_impl.cpp +++ b/src/robot_impl.cpp @@ -8,6 +8,9 @@ #include "load_calculations.h" +#include +#include + namespace franka { namespace { @@ -90,20 +93,32 @@ RobotState Robot::Impl::readOnce() { research_interface::robot::RobotState robot_state; while (network_->udpReceive(&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 diff --git a/src/robot_impl.h b/src/robot_impl.h index bd71ffad..f583ba97 100644 --- a/src/robot_impl.h +++ b/src/robot_impl.h @@ -107,6 +107,8 @@ class Robot::Impl : public RobotControl { bool controllerRunning() const noexcept; private: + RobotState current_state_; + template void writeOnce(const MotionGeneratorType& motion_generator_input); diff --git a/test/robot_impl_tests.cpp b/test/robot_impl_tests.cpp index c872eb90..dc2e6ad2 100644 --- a/test/robot_impl_tests.cpp +++ b/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();