Merge pull request frankaemika#158 in SWDEV/libfranka from bugfix/SRR…

…-1511-fix-rotations to develop

* commit '1fe3e5d4dd47a7247ce797e87b97e6a8fb8b0903':
  add test for normalizing rotations in cartesian lowpass filter
  normalize rotations

CHANGELOG.md

@@ -4,6 +4,7 @@
 
 Requires Panda system version >= 4.2.1
 
+ * Use orthonormalized rotations in `cartesianLowpassFilter`, `limitRate` and `cartesian_impedance_control` example
  * support building libfranka from outside the library, so the debian package name can be set externally.
  * check if Github remote is always in sync
  * bug fixes in `rate_limitng_tests.cpp`

examples/cartesian_impedance_control.cpp

@@ -55,7 +55,7 @@ int main(int argc, char** argv) {
     // equilibrium point is the initial position
     Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
     Eigen::Vector3d position_d(initial_transform.translation());
-    Eigen::Quaterniond orientation_d(initial_transform.linear());
+    Eigen::Quaterniond orientation_d(initial_transform.rotation());
 
     // set collision behavior
     robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
@@ -79,7 +79,7 @@ int main(int argc, char** argv) {
       Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
       Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
       Eigen::Vector3d position(transform.translation());
-      Eigen::Quaterniond orientation(transform.linear());
+      Eigen::Quaterniond orientation(transform.rotation());
 
       // compute error to desired equilibrium pose
       // position error
@@ -95,7 +95,7 @@ int main(int argc, char** argv) {
       Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
       error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
       // Transform to base frame
-      error.tail(3) << -transform.linear() * error.tail(3);
+      error.tail(3) << -transform.rotation() * error.tail(3);
 
       // compute control
       Eigen::VectorXd tau_task(7), tau_d(7);

src/lowpass_filter.cpp

@@ -46,8 +46,8 @@ std::array<double, 16> cartesianLowpassFilter(double sample_time,
   }
   Eigen::Affine3d transform(Eigen::Matrix4d::Map(y.data()));
   Eigen::Affine3d transform_last(Eigen::Matrix4d::Map(y_last.data()));
-  Eigen::Quaterniond orientation(transform.linear());
-  Eigen::Quaterniond orientation_last(transform_last.linear());
+  Eigen::Quaterniond orientation(transform.rotation());
+  Eigen::Quaterniond orientation_last(transform_last.rotation());
 
   double gain = sample_time / (sample_time + (1.0 / (2.0 * M_PI * cutoff_frequency)));
   transform.translation() =

src/rate_limiting.cpp

@@ -214,8 +214,8 @@ std::array<double, 16> limitRate(
   dx.head(3) << (commanded_pose.translation() - last_commanded_pose.translation()) / kDeltaT;
 
   // Compute rotational velocity
-  Eigen::AngleAxisd rot_difference(commanded_pose.linear() *
-                                   last_commanded_pose.linear().transpose());
+  Eigen::AngleAxisd rot_difference(commanded_pose.rotation() *
+                                   last_commanded_pose.rotation().transpose());
   dx.tail(3) << rot_difference.axis() * rot_difference.angle() / kDeltaT;
 
   // Limit the rate of the twist
@@ -231,7 +231,7 @@ std::array<double, 16> limitRate(
 
   // Integrate limited twist
   limited_commanded_pose.translation() << last_commanded_pose.translation() + dx.head(3) * kDeltaT;
-  limited_commanded_pose.linear() << last_commanded_pose.linear();
+  limited_commanded_pose.linear() << last_commanded_pose.rotation();
   if (dx.tail(3).norm() > kNormEps) {
     Eigen::Matrix3d omega_skew;
     Eigen::Vector3d w_norm(dx.tail(3) / dx.tail(3).norm());
@@ -240,7 +240,7 @@ std::array<double, 16> limitRate(
     // NOLINTNEXTLINE(readability-identifier-naming)
     Eigen::Matrix3d R = Eigen::Matrix3d::Identity() + sin(theta) * omega_skew +
                         (1.0 - cos(theta)) * (omega_skew * omega_skew);
-    limited_commanded_pose.linear() << R * last_commanded_pose.linear();
+    limited_commanded_pose.linear() << R * last_commanded_pose.rotation();
   }
 
   std::array<double, 16> limited_values{};

test/helpers.cpp

@@ -6,6 +6,7 @@
 #include <sstream>
 
 #include <gtest/gtest.h>
+#include <Eigen/Dense>
 
 #include "load_calculations.h"
 
@@ -654,6 +655,23 @@ void testGripperStatesAreEqual(const research_interface::gripper::GripperState&
   EXPECT_EQ(expected.temperature, actual.temperature);
 }
 
+std::array<double, 6> differentiateOneSample(std::array<double, 16> value,
+                                             std::array<double, 16> last_value,
+                                             double delta_t) {
+  Eigen::Affine3d pose(Eigen::Matrix4d::Map(value.data()));
+  Eigen::Affine3d last_pose(Eigen::Matrix4d::Map(last_value.data()));
+  Eigen::Matrix<double, 6, 1> dx;
+
+  dx.head(3) << (pose.translation() - last_pose.translation()) / delta_t;
+  auto delta_rotation = (pose.linear() - last_pose.linear()) / delta_t;
+  Eigen::Matrix3d rotational_twist = delta_rotation * last_pose.linear().transpose();
+  dx.tail(3) << rotational_twist(2, 1), rotational_twist(0, 2), rotational_twist(1, 0);
+
+  std::array<double, 6> twist{};
+  Eigen::Map<Eigen::Matrix<double, 6, 1>>(&twist[0], 6, 1) = dx;
+  return twist;
+}
+
 namespace research_interface {
 namespace robot {
 

test/helpers.h

@@ -57,6 +57,9 @@ void testRobotCommandsAreEqual(const research_interface::robot::RobotCommand& ex
                                const franka::RobotCommand actual);
 std::array<double, 16> identityMatrix();
 franka::RobotState generateValidRobotState();
+std::array<double, 6> differentiateOneSample(std::array<double, 16> value,
+                                             std::array<double, 16> last_value,
+                                             double delta_t);
 
 namespace research_interface {
 namespace robot {

test/lowpass_filter_tests.cpp

@@ -6,6 +6,8 @@
 
 #include <franka/lowpass_filter.h>
 
+#include "helpers.h"
+
 using namespace franka;
 
 const double kNoLimit{std::numeric_limits<double>::max()};
@@ -21,3 +23,46 @@ TEST(LowpassFilter, DoesFilter) {
   EXPECT_NEAR(lowpassFilter(0.001, 1.0, 0.0, 500.0), 0.7585, 1e-4);
   EXPECT_NEAR(lowpassFilter(0.001, 1.0, 0.0, 900.0), 0.8497, 1e-4);
 }
+
+TEST(CartesianLowpassFilter, CanFixNonOrthonormalRotation) {
+  // These three poses are all only barely orthonormal, such that the cartesianLowpassFilter will
+  // generate a jerky movement when it does not orthonormalize these first before applying the
+  // filter.
+  Eigen::Affine3d pose1;
+  Eigen::Affine3d pose2;
+  Eigen::Affine3d pose3;
+  pose1.translation() << 1, 1, 1;
+  pose2.translation() << 1, 1, 1;
+  pose3.translation() << 1, 1, 1;
+  // clang-format off
+  pose1.linear() << 0.00462567, 0.999974, 0.00335239,
+                    0.0145489, -0.00341934, 0.999888,
+                    0.999874, -0.00457638, -0.0145646;
+  pose2.linear() << 0.00463526, 0.999984, 0.00335239,
+                    0.014549, -0.00341951, 0.999888,
+                    0.999883, -0.00458597, -0.0145646;
+  pose3.linear() << 0.00465436, 0.999984, 0.00335239,
+                    0.0145489, -0.00341979, 0.999888,
+                    0.999883, -0.00460507, -0.0145646;
+  // clang-format on
+  std::array<double, 16> pose_array1{};
+  std::array<double, 16> pose_array2{};
+  std::array<double, 16> pose_array3{};
+  Eigen::Map<Eigen::Matrix4d>(&pose_array1[0], 4, 4) = pose1.matrix();
+  Eigen::Map<Eigen::Matrix4d>(&pose_array2[0], 4, 4) = pose2.matrix();
+  Eigen::Map<Eigen::Matrix4d>(&pose_array3[0], 4, 4) = pose3.matrix();
+
+  auto output_array1 = cartesianLowpassFilter(0.001, pose_array2, pose_array1, 100.);
+  auto output_array2 = cartesianLowpassFilter(0.001, pose_array3, pose_array2, 100.);
+  auto velocity1 = differentiateOneSample(output_array1, pose_array1, 0.001);
+  auto velocity2 = differentiateOneSample(output_array2, pose_array2, 0.001);
+
+  std::array<double, 6> jerk;
+  for (int i = 0; i < 6; i++) {
+    double acceleration1 = velocity1[i] / 0.001;
+    double acceleration2 = (velocity2[i] - velocity1[i]) / 0.001;
+    jerk[i] = (acceleration2 - acceleration1) / 0.001;
+  }
+  double total_jerk = std::sqrt(std::pow(jerk[3], 2) + std::pow(jerk[4], 2) + std::pow(jerk[5], 2));
+  EXPECT_LT(total_jerk, 1000);
+}

test/rate_limiting_tests.cpp

@@ -6,6 +6,8 @@
 
 #include <franka/rate_limiting.h>
 
+#include "helpers.h"
+
 using namespace franka;
 
 const double kNoLimit{std::numeric_limits<double>::max()};
@@ -42,31 +44,14 @@ std::array<double, 16> integrateOneSample(std::array<double, 16> last_pose,
   Eigen::Map<Eigen::Matrix<double, 6, 1>> dx(twist.data());
   Eigen::Matrix3d omega_skew;
   omega_skew << 0, -dx[5], dx[4], dx[5], 0, -dx[3], -dx[4], dx[3], 0;
-  pose.linear() << pose.linear() + omega_skew * pose.linear() * delta_t;
+  pose.linear() << pose.rotation() + omega_skew * pose.rotation() * delta_t;
   pose.translation() << pose.translation() + dx.head(3) * delta_t;
 
   std::array<double, 16> pose_after_integration{};
   Eigen::Map<Eigen::Matrix4d>(&pose_after_integration[0], 4, 4) = pose.matrix();
   return pose_after_integration;
 }
 
-std::array<double, 6> differentiateOneSample(std::array<double, 16> value,
-                                             std::array<double, 16> last_value,
-                                             double delta_t) {
-  Eigen::Affine3d pose(Eigen::Matrix4d::Map(value.data()));
-  Eigen::Affine3d last_pose(Eigen::Matrix4d::Map(last_value.data()));
-  Eigen::Matrix<double, 6, 1> dx;
-
-  dx.head(3) << (pose.translation() - last_pose.translation()) / delta_t;
-  auto delta_rotation = (pose.linear() - last_pose.linear()) / delta_t;
-  Eigen::Matrix3d rotational_twist = delta_rotation * last_pose.linear().transpose();
-  dx.tail(3) << rotational_twist(2, 1), rotational_twist(0, 2), rotational_twist(1, 0);
-
-  std::array<double, 6> twist{};
-  Eigen::Map<Eigen::Matrix<double, 6, 1>>(&twist[0], 6, 1) = dx;
-  return twist;
-}
-
 bool violatesLimits(double desired_value, double max_value) {
   return std::abs(desired_value) > max_value;
 }