normalize rotations

SysCV · Jul 15, 2022 · b82f0aa · b82f0aa
1 parent feef8ae
commit b82f0aa
Show file tree

Hide file tree

Showing 5 changed files with 11 additions and 10 deletions.
diff --git a/CHANGELOG.md b/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`

diff --git a/examples/cartesian_impedance_control.cpp b/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);

diff --git a/src/lowpass_filter.cpp b/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() =

diff --git a/src/rate_limiting.cpp b/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{};

diff --git a/test/rate_limiting_tests.cpp b/test/rate_limiting_tests.cpp
@@ -42,7 +42,7 @@ 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{};