Pull request #199: Fix/PRCUN-2276 motion generator q start star

Merge in SWDEV/libfranka from fix/PRCUN-2276-motion-generator-q-start-star to main * commit '77ec32a35012e78e9a5768765b3925ce8eff1aa1': chore: remove unused optional bump version 0.14.0 bump-robot-version to 9 fix: low pass filter behavior use commanded value for the first package
frankaemika · Aug 8, 2024 · fee3901 · fee3901
2 parents 8f52b64 + 77ec32a
commit fee3901
Show file tree

Hide file tree

Showing 12 changed files with 54 additions and 22 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -159,10 +159,17 @@ void ControlLoop<JointPositions>::convertMotion(
     const RobotState& robot_state,
     research_interface::robot::MotionGeneratorCommand* command) {
   command->q_c = motion.q;
+  std::array<double, 7> 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<CartesianPose>::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<double, 16> previous_cartesian_pose{};
+  std::array<double, 2> 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<CartesianPose>::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
@@ -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
@@ -191,7 +191,7 @@ auto ControlLoops<JointPositionMotion<true, true>>::getField(const JointPosition
 
 template <>
 auto ControlLoops<JointPositionMotion<false, true>>::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 <>