diff --git a/.ci/Dockerfile.focal b/.ci/Dockerfile.focal index 5e7ab257..ae449d09 100644 --- a/.ci/Dockerfile.focal +++ b/.ci/Dockerfile.focal @@ -45,16 +45,19 @@ RUN apt-get update \ liburdfdom-headers-dev \ libconsole-bridge-dev \ libtinyxml2-dev \ - lsb-release curl \ + lsb-release \ + curl \ + wget \ && rm -rf /var/lib/apt/lists/* -RUN apt-get install -qqy lsb-release curl RUN mkdir -p /etc/apt/keyrings -RUN curl http://robotpkg.openrobots.org/packages/debian/robotpkg.asc \ - | tee /etc/apt/keyrings/robotpkg.asc -RUN echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg" \ - | tee /etc/apt/sources.list.d/robotpkg.list +# robotpkg was blocking curl request without the user-agent string +RUN curl -A "Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/58.0.3029.110 Safari/537.36" \ + http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | sudo tee /etc/apt/keyrings/robotpkg.asc + +RUN echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg" \ +| sudo tee /etc/apt/sources.list.d/robotpkg.list RUN apt-get update \ && apt-get install -qqy robotpkg-pinocchio diff --git a/CHANGELOG.md b/CHANGELOG.md index b3a88058..2acd1ad3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,8 +1,16 @@ # CHANGELOG +## 0.14.0 - 2024-07-31 + +Requires Franka Research 3 system version >= 5.8.0 + +- Joint/Cartesian Pose velocity and acceleration errors are minimized. +- Joint position and cartesian pose examples can now start from the initial q or O_T_EE +- Joint position and cartesian pose low-pass filters are fixed to use the same command in the first package. + ## 0.13.4 - 2024-03-26 -Requires Franka Research 3 system version >= 5.5.0 +Requires Franka Research 3 system version >= 5.7.0 - Compute dynamic robot parameters with Pinocchio library. diff --git a/CMakeLists.txt b/CMakeLists.txt index 90c96c19..de42cb88 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.4) +set(libfranka_VERSION 0.14.0) project(libfranka VERSION ${libfranka_VERSION} diff --git a/Jenkinsfile b/Jenkinsfile index 2a6931ce..02b9b013 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -180,7 +180,7 @@ pipeline { dir("build-release.${env.DISTRO}") { catchError(buildResult: env.UNSTABLE, stageResult: env.UNSTABLE) { sh 'cpack' - fePublishDebian('*.deb', 'futuretech-common', "deb.distribution=${env.DISTRO};deb.component=main;deb.architecture=amd64") + fePublishDebian('*.deb', 'fci', "deb.distribution=${env.DISTRO};deb.component=main;deb.architecture=amd64") dir('doc') { sh 'tar cfz ../libfranka-docs.tar.gz html' } diff --git a/common b/common index 22b08375..cd38d0ec 160000 --- a/common +++ b/common @@ -1 +1 @@ -Subproject commit 22b083750c45469ac22ed29f97a84ffd755ef8ce +Subproject commit cd38d0ec300b7e6864407d85d1e88e2fba31ccd5 diff --git a/examples/examples_common.cpp b/examples/examples_common.cpp index 6d921bb8..dc8e35ca 100644 --- a/examples/examples_common.cpp +++ b/examples/examples_common.cpp @@ -116,7 +116,7 @@ franka::JointPositions MotionGenerator::operator()(const franka::RobotState& rob time_ += period.toSec(); if (time_ == 0.0) { - q_start_ = Vector7d(robot_state.q_d.data()); + q_start_ = Vector7d(robot_state.q.data()); delta_q_ = q_goal_ - q_start_; calculateSynchronizedValues(); } diff --git a/examples/generate_cartesian_pose_motion.cpp b/examples/generate_cartesian_pose_motion.cpp index e8fbdf9c..0e91206f 100644 --- a/examples/generate_cartesian_pose_motion.cpp +++ b/examples/generate_cartesian_pose_motion.cpp @@ -49,7 +49,7 @@ int main(int argc, char** argv) { time += period.toSec(); if (time == 0.0) { - initial_pose = robot_state.O_T_EE_c; + initial_pose = robot_state.O_T_EE; } constexpr double kRadius = 0.3; diff --git a/examples/generate_elbow_motion.cpp b/examples/generate_elbow_motion.cpp index 0468ab12..6f7be487 100644 --- a/examples/generate_elbow_motion.cpp +++ b/examples/generate_elbow_motion.cpp @@ -51,8 +51,8 @@ int main(int argc, char** argv) { time += period.toSec(); if (time == 0.0) { - initial_pose = robot_state.O_T_EE_c; - initial_elbow = robot_state.elbow_c; + initial_pose = robot_state.O_T_EE; + initial_elbow = robot_state.elbow; } double angle = M_PI / 10.0 * (1.0 - std::cos(M_PI / 5.0 * time)); diff --git a/examples/generate_joint_position_motion.cpp b/examples/generate_joint_position_motion.cpp index e5129ef3..722ba06b 100644 --- a/examples/generate_joint_position_motion.cpp +++ b/examples/generate_joint_position_motion.cpp @@ -49,7 +49,7 @@ int main(int argc, char** argv) { time += period.toSec(); if (time == 0.0) { - initial_position = robot_state.q_d; + initial_position = robot_state.q; } double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time)); diff --git a/src/control_loop.cpp b/src/control_loop.cpp index 853f4157..013187c3 100644 --- a/src/control_loop.cpp +++ b/src/control_loop.cpp @@ -159,10 +159,17 @@ void ControlLoop::convertMotion( const RobotState& robot_state, research_interface::robot::MotionGeneratorCommand* command) { command->q_c = motion.q; + std::array previous_joint_position{}; + if (!initialized_filter_) { + previous_joint_position = command->q_c; + initialized_filter_ = true; + } else { + previous_joint_position = robot_state.q_d; + } if (cutoff_frequency_ < kMaxCutoffFrequency) { for (size_t i = 0; i < 7; i++) { command->q_c[i] = - lowpassFilter(kDeltaT, command->q_c[i], robot_state.q_d[i], cutoff_frequency_); + lowpassFilter(kDeltaT, command->q_c[i], previous_joint_position[i], cutoff_frequency_); } } if (limit_rate_) { @@ -201,11 +208,25 @@ void ControlLoop::convertMotion( const RobotState& robot_state, research_interface::robot::MotionGeneratorCommand* command) { command->O_T_EE_c = motion.O_T_EE; - if (cutoff_frequency_ < kMaxCutoffFrequency) { - command->O_T_EE_c = - cartesianLowpassFilter(kDeltaT, command->O_T_EE_c, robot_state.O_T_EE_c, cutoff_frequency_); + std::array previous_cartesian_pose{}; + std::array previous_elbow_pose{}; + if (!initialized_filter_) { + previous_cartesian_pose = command->O_T_EE_c; + if (motion.hasElbow()) { + previous_elbow_pose = motion.elbow; + } + initialized_filter_ = true; + } else { + previous_cartesian_pose = robot_state.O_T_EE_c; + if (motion.hasElbow()) { + previous_elbow_pose = robot_state.elbow_c; + } } + if (cutoff_frequency_ < kMaxCutoffFrequency) { + command->O_T_EE_c = cartesianLowpassFilter(kDeltaT, command->O_T_EE_c, previous_cartesian_pose, + cutoff_frequency_); + } if (limit_rate_) { command->O_T_EE_c = limitRate( kMaxTranslationalVelocity, kMaxTranslationalAcceleration, kMaxTranslationalJerk, @@ -219,7 +240,7 @@ void ControlLoop::convertMotion( command->elbow_c = motion.elbow; if (cutoff_frequency_ < kMaxCutoffFrequency) { command->elbow_c[0] = - lowpassFilter(kDeltaT, command->elbow_c[0], robot_state.elbow_c[0], cutoff_frequency_); + lowpassFilter(kDeltaT, command->elbow_c[0], previous_elbow_pose[0], cutoff_frequency_); } if (limit_rate_) { command->elbow_c[0] = diff --git a/src/control_loop.h b/src/control_loop.h index b90c9d63..b9946163 100644 --- a/src/control_loop.h +++ b/src/control_loop.h @@ -57,10 +57,10 @@ class ControlLoop { const bool limit_rate_; // NOLINT(readability-identifier-naming) const double cutoff_frequency_; // NOLINT(readability-identifier-naming) uint32_t motion_id_ = 0; - void convertMotion(const T& motion, const RobotState& robot_state, research_interface::robot::MotionGeneratorCommand* command); + bool initialized_filter_{false}; }; } // namespace franka diff --git a/test/control_loop_tests.cpp b/test/control_loop_tests.cpp index bddc615a..1fe5e038 100644 --- a/test/control_loop_tests.cpp +++ b/test/control_loop_tests.cpp @@ -191,7 +191,7 @@ auto ControlLoops>::getField(const JointPosition template <> auto ControlLoops>::getField(const JointPositions& values) { - return Field(&research_interface::robot::MotionGeneratorCommand::q_c, Lt(values.q)); + return Field(&research_interface::robot::MotionGeneratorCommand::q_c, Eq(values.q)); } template <>