release: version 7.3.0 (#149)

.gitignore

@@ -41,6 +41,7 @@ source/.vs*
 
 # build
 source/build
+source/tmp
 
 # clang-format
 .clang-format

.vscode/c_cpp_properties.json

@@ -3,6 +3,9 @@
         {
             "name": "AMD",
             "includePath": [
+                "/usr/include/eigen3",
+                "/usr/include/osqp",
+                "/usr/include/OsqpEigen",
                 "/src/source/**/include"
             ],
             "compilerPath": "/usr/bin/gcc",

CHANGELOG.md

@@ -1,6 +1,7 @@
 # CHANGELOG
 
 Release Versions:
+- [7.3.0](#730)
 - [7.2.0](#720)
 - [7.1.1](#711)
 - [7.1.0](#710)
@@ -9,6 +10,27 @@ Release Versions:
 - [6.3.0](#630)
 - [6.2.0](#620)
 
+## 7.3.0
+
+Version 7.3.0 contains new improvements and a fix to the control libraries.
+
+### Features
+
+Setting controller gain parameters is now even easier than before as vectors and arrays of size 1 are also allowed and
+interpreted the same way as a double. Additionally, the robot model now has an improved inverse velocity calculation
+that uses a damped least squared pseudoinverse if desired.
+
+### Fix
+
+An error in the scalar multiplication operator of a Cartesian state that generated an incorrect orientation has now been
+fixed.
+
+### Full changelog
+
+- feat(robot model): Damped least squared pseudoinverse (#143)
+- feat(controllers): improve parameter validation of impedance controller (#148)
+- fix(state_representation): remove error in orientation scaling (#147)
+
 ## 7.2.0
 
 Version 7.2.0 contains improvements for the Python bindings of control libraries.

VERSION

@@ -1 +1 @@
-7.2.0
+7.3.0

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.0 CONFIG REQUIRED)
+find_package(control_libraries 7.3.0 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.0
+PROJECT_NUMBER = 7.3.0
 
 # 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.0)
+project(clproto VERSION 7.3.0)
 
 # 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.0"
+__version__ = "7.3.0"
 __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model']
 __include_dirs__ = ['include']
 

python/source/robot_model/bind_model.cpp

@@ -75,10 +75,10 @@ void model(py::module_& m) {
   c.def("forward_velocity", py::overload_cast<const JointState&, const std::string&>(&Model::forward_velocity),
         "Compute the forward velocity kinematics, i.e. the twist of the end-effector from the joint velocities", "joint_state"_a, "frame"_a = std::string(""));
 
-  c.def("inverse_velocity", py::overload_cast<const std::vector<CartesianTwist>&, const JointPositions&, const std::vector<std::string>&>(&Model::inverse_velocity),
-        "Compute the inverse velocity kinematics, i.e. joint velocities from the velocities of the frames in parameter the Jacobian", "cartesian_twists"_a, "joint_positions"_a, "frames"_a);
-  c.def("inverse_velocity", py::overload_cast<const CartesianTwist&, const JointPositions&, const std::string&>(&Model::inverse_velocity),
-        "Compute the inverse velocity kinematics, i.e. joint velocities from the twist of the end-effector using the Jacobian", "cartesian_twist"_a, "joint_positions"_a, "frame"_a = std::string(""));
+  c.def("inverse_velocity", py::overload_cast<const std::vector<CartesianTwist>&, const JointPositions&, const std::vector<std::string>&, const double>(&Model::inverse_velocity),
+        "Compute the inverse velocity kinematics, i.e. joint velocities from the velocities of the frames in parameter the Jacobian", "cartesian_twists"_a, "joint_positions"_a, "frames"_a, "dls_lambda"_a = 0.0);
+  c.def("inverse_velocity", py::overload_cast<const CartesianTwist&, const JointPositions&, const std::string&, const double>(&Model::inverse_velocity),
+        "Compute the inverse velocity kinematics, i.e. joint velocities from the twist of the end-effector using the Jacobian", "cartesian_twist"_a, "joint_positions"_a, "frame"_a = std::string(""), "dls_lambda"_a = 0.0);
   c.def("inverse_velocity", py::overload_cast<const std::vector<CartesianTwist>&, const JointPositions&, const QPInverseVelocityParameters&, const std::vector<std::string>&>(&Model::inverse_velocity),
         "Compute the inverse velocity kinematics, i.e. joint velocities from the velocities of the frames in parameter using the QP optimization method", "cartesian_twists"_a, "joint_positions"_a, "parameters"_a, "frames"_a);
   c.def("inverse_velocity", py::overload_cast<const CartesianTwist&, const JointPositions&, const QPInverseVelocityParameters&, const std::string&>(&Model::inverse_velocity),

source/CMakeLists.txt

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

source/controllers/include/controllers/impedance/Impedance.hpp

@@ -82,8 +82,7 @@ Impedance<S>::Impedance(unsigned int dimensions) :
     state_representation::make_shared_parameter<Eigen::MatrixXd>(
         "inertia", Eigen::MatrixXd::Identity(dimensions, dimensions))), feed_forward_force_(
             state_representation::make_shared_parameter<bool>("feed_forward_force", false)), force_limit_(
-    state_representation::make_shared_parameter<Eigen::VectorXd>(
-        "force_limit", Eigen::VectorXd::Zero(dimensions))), dimensions_(dimensions) {
+    state_representation::make_shared_parameter<Eigen::VectorXd>("force_limit")), dimensions_(dimensions) {
   this->parameters_.insert(std::make_pair("stiffness", stiffness_));
   this->parameters_.insert(std::make_pair("damping", damping_));
   this->parameters_.insert(std::make_pair("inertia", inertia_));
@@ -101,11 +100,8 @@ Impedance<S>::Impedance(
 
 template<class S>
 void Impedance<S>::clamp_force(Eigen::VectorXd& force) {
-  auto limit = this->force_limit_->get_value();
-  for (std::size_t index = 0; index < this->dimensions_; ++index) {
-    if (limit(index) > 0.0 && abs(force(index)) > limit(index)) {
-      force(index) = force(index) > 0.0 ? limit(index) : -limit(index);
-    }
+  if (*this->force_limit_) {
+    force = force.cwiseMax(-this->force_limit_->get_value()).cwiseMin(this->force_limit_->get_value());
   }
 }
 
@@ -122,6 +118,10 @@ void Impedance<S>::validate_and_set_parameter(
   } else if (parameter->get_name() == "feed_forward_force") {
     this->feed_forward_force_->set_value(parameter->get_parameter_value<bool>());
   } else if (parameter->get_name() == "force_limit") {
+    if (parameter->get_parameter_type() == state_representation::ParameterType::MATRIX) {
+      throw state_representation::exceptions::InvalidParameterException(
+          "Parameter " + parameter->get_name() + " has incorrect type");
+    }
     auto limit_matrix = this->gain_matrix_from_parameter(parameter);
     this->force_limit_->set_value(limit_matrix.diagonal());
   } else {
@@ -141,22 +141,27 @@ Eigen::MatrixXd Impedance<S>::gain_matrix_from_parameter(
     matrix = gain->get_value() * Eigen::MatrixXd::Identity(this->dimensions_, this->dimensions_);
   } else if (parameter->get_parameter_type() == state_representation::ParameterType::DOUBLE_ARRAY) {
     auto gain = std::static_pointer_cast<state_representation::Parameter<std::vector<double>>>(parameter);
-    if (gain->get_value().size() != this->dimensions_) {
+    if (gain->get_value().size() == 1) {
+      matrix = gain->get_value().at(0) * Eigen::MatrixXd::Identity(this->dimensions_, this->dimensions_);
+    } else if (gain->get_value().size() == this->dimensions_) {
+      Eigen::VectorXd diagonal = Eigen::VectorXd::Map(gain->get_value().data(), this->dimensions_);
+      matrix = diagonal.asDiagonal();
+    } else {
       throw state_representation::exceptions::IncompatibleSizeException(
           "The provided diagonal coefficients do not match the dimensionality of the controller ("
               + std::to_string(this->dimensions_) + ")");
     }
-    Eigen::VectorXd diagonal = Eigen::VectorXd::Map(gain->get_value().data(), this->dimensions_);
-    matrix = diagonal.asDiagonal();
   } else if (parameter->get_parameter_type() == state_representation::ParameterType::VECTOR) {
     auto gain = std::static_pointer_cast<state_representation::Parameter<Eigen::VectorXd>>(parameter);
-    if (gain->get_value().size() != this->dimensions_) {
+    if (gain->get_value().size() == 1) {
+      matrix = gain->get_value()(0) * Eigen::MatrixXd::Identity(this->dimensions_, this->dimensions_);
+    } else if (gain->get_value().size() == this->dimensions_) {
+      matrix = gain->get_value().asDiagonal();
+    } else {
       throw state_representation::exceptions::IncompatibleSizeException(
           "The provided diagonal coefficients do not match the dimensionality of the controller ("
               + std::to_string(this->dimensions_) + ")");
     }
-    Eigen::VectorXd diagonal = gain->get_value();
-    matrix = diagonal.asDiagonal();
   } else if (parameter->get_parameter_type() == state_representation::ParameterType::MATRIX) {
     auto gain = std::static_pointer_cast<state_representation::Parameter<Eigen::MatrixXd>>(parameter);
     if (gain->get_value().rows() != this->dimensions_ || gain->get_value().cols() != this->dimensions_) {
@@ -169,6 +174,10 @@ Eigen::MatrixXd Impedance<S>::gain_matrix_from_parameter(
     throw state_representation::exceptions::InvalidParameterException(
         "Parameter " + parameter->get_name() + " has incorrect type");
   }
+  if ((matrix.array() < 0).any()) {
+    throw state_representation::exceptions::InvalidParameterException(
+        "Parameter " + parameter->get_name() + " cannot have negative elements");
+  }
   return matrix;
 }
 

source/controllers/src/impedance/Impedance.cpp

@@ -37,7 +37,7 @@ CartesianState Impedance<CartesianState>::compute_command(
   Eigen::VectorXd wrench(6);
   wrench << position_control, orientation_control;
   // if the 'feed_forward_force' parameter is set to true, also add the wrench error to the command
-  if (this->get_parameter_value<bool>("feed_forward_force")) {
+  if (this->feed_forward_force_->get_value()) {
     wrench += state_error.get_wrench();
   }
   clamp_force(wrench);
@@ -59,10 +59,11 @@ JointState Impedance<JointState>::compute_command(
       + this->inertia_->get_value() * command_state.get_accelerations();
 
   // if the 'feed_forward_force' parameter is set to true, also add the torque error to the command
-  if (this->get_parameter_value<bool>("feed_forward_force")) {
+  if (this->feed_forward_force_->get_value()) {
     torque_control += state_error.get_torques();
   }
   clamp_force(torque_control);
+
   command.set_torques(torque_control);
   return command;
 }

source/robot_model/include/robot_model/Model.hpp

@@ -405,11 +405,13 @@ class Model {
    * @param cartesian_twists vector of twist
    * @param joint_positions current joint positions, used to compute the Jacobian matrix
    * @param frames names of the frames at which to compute the twists
+   * @param dls_lambda damped least square term
    * @return the joint velocities of the robot
    */
   state_representation::JointVelocities inverse_velocity(const std::vector<state_representation::CartesianTwist>& cartesian_twists,
                                                          const state_representation::JointPositions& joint_positions,
-                                                         const std::vector<std::string>& frames);
+                                                         const std::vector<std::string>& frames,
+                                                         const double dls_lambda = 0.0);
 
   /**
    * @brief Compute the inverse velocity kinematics, i.e. joint velocities from the twist of the end-effector using the
@@ -419,11 +421,13 @@ class Model {
    * @param frame name of the frame at which to compute the twist
    * @param parameters parameters of the inverse velocity kinematics algorithm (default is default values of the
    * QPInverseVelocityParameters structure)
+   * @param dls_lambda damped least square term
    * @return the joint velocities of the robot
    */
   state_representation::JointVelocities inverse_velocity(const state_representation::CartesianTwist& cartesian_twist,
                                                          const state_representation::JointPositions& joint_positions,
-                                                         const std::string& frame = "");
+                                                         const std::string& frame = "",
+                                                         const double dls_lambda = 0.0);
 
   /**
    * @brief Compute the inverse velocity kinematics, i.e. joint velocities from the velocities of the frames in parameter

source/robot_model/src/Model.cpp

@@ -393,7 +393,8 @@ void Model::check_inverse_velocity_arguments(const std::vector<state_representat
 state_representation::JointVelocities
 Model::inverse_velocity(const std::vector<state_representation::CartesianTwist>& cartesian_twists,
                         const state_representation::JointPositions& joint_positions,
-                        const std::vector<std::string>& frames) {
+                        const std::vector<std::string>& frames,
+                        const double dls_lambda) {
   // sanity check
   this->check_inverse_velocity_arguments(cartesian_twists, joint_positions, frames);
 
@@ -411,19 +412,37 @@ Model::inverse_velocity(const std::vector<state_representation::CartesianTwist>&
   dX.tail(6) = cartesian_twists.back().data();
   jacobian.bottomRows(6) = this->compute_jacobian(joint_positions, frames.back()).data();
 
-  // solve a linear system
-  return state_representation::JointVelocities(joint_positions.get_name(),
+  if (dls_lambda == 0.0){
+    return state_representation::JointVelocities(joint_positions.get_name(),
                                                joint_positions.get_names(),
                                                jacobian.colPivHouseholderQr().solve(dX));
+  }
+
+  // add damped least square term
+  if (jacobian.rows() > jacobian.cols()){
+    Eigen::MatrixXd j_prime = jacobian.transpose() * jacobian + 
+                dls_lambda * dls_lambda * Eigen::MatrixXd::Identity(jacobian.cols(), jacobian.cols());
+    return state_representation::JointVelocities(joint_positions.get_name(),
+                                               joint_positions.get_names(),
+                                               j_prime.colPivHouseholderQr().solve(jacobian.transpose() * dX));
+  } else {
+    Eigen::MatrixXd j_prime = jacobian * jacobian.transpose() + 
+                dls_lambda * dls_lambda * Eigen::MatrixXd::Identity(jacobian.rows(), jacobian.rows());
+    return state_representation::JointVelocities(joint_positions.get_name(),
+                                               joint_positions.get_names(),
+                                               jacobian.transpose() * j_prime.colPivHouseholderQr().solve(dX));
+  }
 }
 
 state_representation::JointVelocities Model::inverse_velocity(const state_representation::CartesianTwist& cartesian_twist,
                                                               const state_representation::JointPositions& joint_positions,
-                                                              const std::string& frame) {
+                                                              const std::string& frame,
+                                                              const double dls_lambda) {
   std::string actual_frame = frame.empty() ? this->robot_model_.frames.back().name : frame;
   return this->inverse_velocity(std::vector<state_representation::CartesianTwist>({cartesian_twist}),
                                 joint_positions,
-                                std::vector<std::string>({actual_frame}));
+                                std::vector<std::string>({actual_frame}),
+                                dls_lambda);
 }
 
 state_representation::JointVelocities