feat (robot model): Damped least squared pseudoinverse (#143)

Damped pseudoinverse slows down robot near singularities

.gitignore

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

CHANGELOG.md

@@ -28,6 +28,7 @@ dependencies. This has been fixed in #134.
 - feat(build): add configuration files for VS Code devcontainer (#137, #138)
 - feat(python): auto-generate stubs for python modules in Dockerfile (#135, #139)
 - fix: build all python modules (#134)
+- feat(robot model): Damped least squared pseudoinverse (#143)
 
 ## 7.1.1
 

VERSION

@@ -1 +1 @@
-7.2.0
+7.2.1

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.2.1 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.2.1
 
 # 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.2.1)
 
 # 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.2.1"
 __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.2.1)
 
 # Build options
 option(BUILD_TESTING "Build all tests." OFF)

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

source/robot_model/test/fixtures/generateRobotModelKinematicsTestConfigurations.m

@@ -28,6 +28,7 @@ function generateRobotModelKinematicsTestConfigurations(urdf, nConfigurations, s
 
 % initialise the random number generator
 rng(seed)
+disp(rng)
 
 % load the robot model
 robot = importrobot(urdf);
@@ -43,33 +44,48 @@ function generateRobotModelKinematicsTestConfigurations(urdf, nConfigurations, s
 fprintf('std::vector<state_representation::CartesianPose> test_fk_ee_expects;\n');
 fprintf('std::vector<state_representation::CartesianPose> test_fk_link4_expects;\n');
 fprintf('std::vector<state_representation::CartesianTwist> test_velocity_fk_expects;\n');
+fprintf('std::vector<Eigen::MatrixXd> test_damped_jacobian_ee_expects;\n');
 fprintf('Eigen::Matrix<double, 6, 1> twist;\n');
+fprintf('Eigen::Matrix<double, 7, 1> joint_vel_damped;\n');
+fprintf('std::vector<double> test_dls_lambdas;\n');
+fprintf('std::vector<state_representation::CartesianTwist> test_ee_velocities;\n');
+fprintf('std::vector<state_representation::JointVelocities> test_velocity_damped_ik_expects;\n');
 
 % generate the configurations
 for conf = 1:nConfigurations
     % configurations
     q = robot.randomConfiguration;
     v = rand(size(q))*2 - 1;
+    v_ee = rand(6,1)*2 - 1;
+    dls_lambda = 10^(-rand());
 
     % transforms
     ee_T = robot.getTransform(q, 'panda_link8');
     link4_T = robot.getTransform(q, 'panda_link4');
 
     % jacobian
     ee_jac = robot.geometricJacobian(q, 'panda_link8');
+    ee_jac_r(1:3,:) = ee_jac(4:6,:);
+    ee_jac_r(4:6,:) = ee_jac(1:3,:);
+    ee_jac_damped = pinv(pseudoInverseMat(ee_jac_r, dls_lambda));
 
     % expected results
     ee_pose = [tform2trvec(ee_T), tform2quat(ee_T)]; % (1x7)
     link4_pose = [tform2trvec(link4_T), tform2quat(link4_T)]; % (1x7)
     ee_twist = ee_jac * v'; % (1x6)
+    joint_vel_damped = ee_jac_damped\v_ee; % (1x7)
 
     % code generation (definitions)
     fprintf('\n// Random test configuration %i:\n', conf);
     fprintf(['state_representation::JointState config%i', ...
         '(franka->get_robot_name(), franka->get_joint_frames());\n'], ...
         conf);
-    fprintf('config%i.set_positions(%s);\n', conf, num2stdvec(q));
-    fprintf('config%i.set_velocities(%s);\n', conf, num2stdvec(v));
+    fprintf('config%i.set_positions(std::vector<double>%s);\n', conf, num2stdvec(q));
+    fprintf('config%i.set_velocities(std::vector<double>%s);\n', conf, num2stdvec(v));
+    fprintf('state_representation::CartesianTwist test_ee_velocity%i("franka");\n', conf);
+    fprintf('test_ee_velocity%i.set_data(std::vector<double>%s);\n', conf, num2stdvec(v_ee));
+    fprintf('test_ee_velocities.push_back(test_ee_velocity%i);\n', conf);
+    fprintf('test_dls_lambdas.push_back(%f);\n', dls_lambda);
     fprintf('test_configs.push_back(config%i);\n', conf);
 
     fprintf('\n// Expected results for configuration %i:\n', conf);
@@ -85,6 +101,16 @@ function generateRobotModelKinematicsTestConfigurations(urdf, nConfigurations, s
     fprintf('\t%f, %f, %f, %f, %f, %f, %f; \n', ee_jac(3, :));
     fprintf('test_jacobian_ee_expects.emplace_back(jac%i);\n', conf);
 
+    % populate damped Jacobian matrix
+    fprintf('Eigen::MatrixXd damped_jac%i(6, 7);\n', conf);
+    fprintf('damped_jac%i << %f, %f, %f, %f, %f, %f, %f, \n', conf, ee_jac_damped(4, :));
+    fprintf('\t%f, %f, %f, %f, %f, %f, %f, \n', ee_jac_damped(5, :));
+    fprintf('\t%f, %f, %f, %f, %f, %f, %f, \n', ee_jac_damped(6, :));
+    fprintf('\t%f, %f, %f, %f, %f, %f, %f, \n', ee_jac_damped(1, :));
+    fprintf('\t%f, %f, %f, %f, %f, %f, %f, \n', ee_jac_damped(2, :));
+    fprintf('\t%f, %f, %f, %f, %f, %f, %f; \n', ee_jac_damped(3, :));
+    fprintf('test_damped_jacobian_ee_expects.emplace_back(damped_jac%i);\n', conf);
+
     % populate forward kinematics results
     fprintf(['test_fk_ee_expects.emplace_back(state_representation::CartesianPose' ...
         '("ee", Eigen::Vector3d(%f, %f, %f), Eigen::Quaterniond(%f, %f, %f, %f), ', ...
@@ -98,4 +124,9 @@ function generateRobotModelKinematicsTestConfigurations(urdf, nConfigurations, s
     fprintf('twist << %f, %f, %f, %f, %f, %f;\n', ee_twist([4:6, 1:3]));
     fprintf(['test_velocity_fk_expects.emplace_back', ...
         '(state_representation::CartesianTwist("ee", twist, franka->get_base_frame()));\n']);
+
+    % populate damped inverse velocity results
+    fprintf('joint_vel_damped << %f, %f, %f, %f, %f, %f, %f;\n', joint_vel_damped);
+    fprintf(['test_velocity_damped_ik_expects.emplace_back', ...
+        '(state_representation::JointVelocities("franka", joint_vel_damped));\n']);
 end

source/robot_model/test/fixtures/pseudoInverseMat.m

@@ -0,0 +1,19 @@
+% This code was taken from the Marco Hutter's Robot Dynamics class 
+% in ETH Zurich
+
+function [ pinvA ] = pseudoInverseMat(A, lambda)
+% Input: Any m-by-n matrix, and a damping factor.
+% Output: An n-by-m pseudo-inverse of the input according to the Moore-Penrose formula
+
+% Get the number of rows (m) and columns (n) of A
+[m,n] = size(A);
+
+% Compute the pseudo inverse for both left and right cases
+if (m>n)
+  % Compute the left pseudoinverse.
+  pinvA = (A'*A + lambda*lambda*eye(n,n))\A';
+elseif (m<=n)
+  % Compute the right pseudoinverse.
+  pinvA = A'/(A*A' + lambda*lambda*eye(m,m));
+end
+end