fix(state_representation): remove error in orientation scaling

CHANGELOG.md

@@ -12,6 +12,7 @@ Release Versions:
 ## Upcoming changes (in development)
 
 - feat(controllers): improve parameter validation of impedance controller (#148)
+- fix(state_representation): remove error in orientation scaling (#147)
 
 ## 7.2.0
 

VERSION

@@ -1 +1 @@
-7.2.2
+7.2.3

demos/CMakeLists.txt

@@ -15,7 +15,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-find_package(control_libraries 7.2.2 CONFIG REQUIRED)
+find_package(control_libraries 7.2.3 CONFIG REQUIRED)
 
 set(DEMOS_SCRIPTS
   task_space_control_loop

doxygen/doxygen.conf

@@ -38,7 +38,7 @@ PROJECT_NAME           = "Control Libraries"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER = 7.2.2
+PROJECT_NUMBER = 7.2.3
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

protocol/clproto_cpp/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.15)
 
-project(clproto VERSION 7.2.2)
+project(clproto VERSION 7.2.3)
 
 # Default to C99
 if(NOT CMAKE_C_STANDARD)

python/setup.py

@@ -11,7 +11,7 @@
 # names of the environment variables that define osqp and openrobots include directories
 osqp_path_var = 'OSQP_INCLUDE_DIR'
 
-__version__ = "7.2.2"
+__version__ = "7.2.3"
 __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model']
 __include_dirs__ = ['include']
 

source/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.15)
 
-project(control_libraries VERSION 7.2.2)
+project(control_libraries VERSION 7.2.3)
 
 # Build options
 option(BUILD_TESTING "Build all tests." OFF)

source/state_representation/src/space/cartesian/CartesianState.cpp

@@ -701,9 +701,11 @@ CartesianState& CartesianState::operator*=(double lambda) {
   // operation
   this->set_position(lambda * this->get_position());
   // calculate the scaled rotation as a displacement from identity
-  Eigen::Quaterniond w = math_tools::log(this->get_orientation());
-  // calculate the orientation corresponding to the scaled velocity
-  this->set_orientation(math_tools::exp(w, lambda / 2.));
+  auto q = math_tools::exp(math_tools::log(this->get_orientation()), lambda);
+  if (this->get_orientation().w() * q.w() < 0) {
+    q = Eigen::Quaterniond(-q.coeffs());
+  }
+  this->set_orientation(q);
   // calculate the other vectors normally
   this->set_twist(lambda * this->get_twist());
   this->set_acceleration(lambda * this->get_acceleration());

source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp

@@ -731,7 +731,7 @@ TEST(CartesianStateTest, ScalarMultiplication) {
   CartesianState cs = CartesianState::Random("test");
   CartesianState cscaled = scalar * cs;
   EXPECT_TRUE(cscaled.get_position().isApprox(scalar * cs.get_position()));
-  Eigen::Quaterniond qscaled = math_tools::exp(math_tools::log(cs.get_orientation()), scalar / 2.);
+  auto qscaled = math_tools::exp(math_tools::log(cs.get_orientation()), scalar);
   EXPECT_TRUE(cscaled.get_orientation().coeffs().isApprox(qscaled.coeffs()));
   EXPECT_TRUE(cscaled.get_twist().isApprox(scalar * cs.get_twist()));
   EXPECT_TRUE(cscaled.get_acceleration().isApprox(scalar * cs.get_acceleration()));
@@ -749,7 +749,7 @@ TEST(CartesianStateTest, ScalarDivision) {
   CartesianState cs = CartesianState::Random("test");
   CartesianState cscaled = cs / scalar;
   EXPECT_TRUE(cscaled.get_position().isApprox(cs.get_position() / scalar));
-  Eigen::Quaterniond qscaled = math_tools::exp(math_tools::log(cs.get_orientation()), 1.0 / (2. * scalar));
+  auto qscaled = math_tools::exp(math_tools::log(cs.get_orientation()), 1.0 / scalar);
   EXPECT_TRUE(cscaled.get_orientation().coeffs().isApprox(qscaled.coeffs()));
   EXPECT_TRUE(cscaled.get_twist().isApprox(cs.get_twist() / scalar));
   EXPECT_TRUE(cscaled.get_acceleration().isApprox(cs.get_acceleration() / scalar));
@@ -763,6 +763,30 @@ TEST(CartesianStateTest, ScalarDivision) {
   EXPECT_THROW(empty / scalar, exceptions::EmptyStateException);
 }
 
+TEST(CartesianStateTest, OrientationScaling) {
+  auto cs = CartesianState::Random("A");
+
+  for (int i = 0; i < 5; ++i) {
+    double scale = static_cast<double>(rand()) / RAND_MAX + i;
+
+    auto qscaled = Eigen::Quaterniond::Identity();
+    auto cscaled = scale * cs;
+    for (int j = 0; j < i; ++j) {
+      qscaled = qscaled * cs.get_orientation();
+    }
+    qscaled = qscaled * Eigen::Quaterniond::Identity().slerp(scale - i, cs.get_orientation());
+    EXPECT_LT(cscaled.get_orientation().angularDistance(qscaled), 1e-3);
+
+    qscaled = Eigen::Quaterniond::Identity();
+    cscaled = - scale * cs;
+    for (int j = 0; j < i; ++j) {
+      qscaled = qscaled * cs.get_orientation();
+    }
+    qscaled = qscaled * Eigen::Quaterniond::Identity().slerp(scale - i, cs.get_orientation());
+    EXPECT_LT(cscaled.get_orientation().angularDistance(qscaled.conjugate()), 1e-3);
+  }
+}
+
 TEST(CartesianStateTest, Multiplication) {
   CartesianState world_cs_first = CartesianState::Random("first");
   CartesianState first_cs_second = CartesianState::Random("second", "first");