docs

Author: evan-palmer

ik_solvers/README.md

@@ -0,0 +1,21 @@
+# Inverse Kinematics Solvers
+
+This package implements inverse kinematics solvers for an underwater vehicle
+manipulator system. The solvers are designed to be used in conjunction with
+the whole_body_controllers/IKController.
+
+## Closed-Loop Task Priority Inverse Kinematics Solver
+
+A singularity-robust inverse kinematics solver proposed by Moe, et al. [^1].
+The solver implements joint limit constraints (using the joint limits defined
+in the robot description) and end-effector pose tracking. The Jacobian
+pseudoinverse is calculated using the damped least squares method.
+
+[^1]: S. Moe, G. Antonelli, A. R. Teel, K. Y. Pettersen, and J. Schrimpf,
+"Set-Based Tasks within the Singularity-Robust Multiple Task-Priority Inverse
+Kinematics Framework: General Formulation, Stability Analysis, and Experimental
+Results", in *Frontiers in Robotics and AI*, 2016.
+
+### Plugin Library
+
+ik_solvers/task_priority_solver

ik_solvers/include/ik_solvers/task_priority_solver.hpp

@@ -47,7 +47,7 @@ class Constraint
   : primal_(primal),
     constraint_(constraint),
     priority_(priority),
-    gain_(gain) {};
+    gain_(gain){};
 
   /// Destructor.
   virtual ~Constraint() = default;

ik_solvers/src/pseudoinverse.cpp

@@ -0,0 +1,28 @@
+#include "pseudoinverse.hpp"
+
+#include <cmath>
+
+namespace ik_solvers::pinv
+{
+
+auto least_squares(const Eigen::MatrixXd & jac) -> Eigen::MatrixXd
+{
+  // we don't need to do much here - this is mostly a convenience function to make it easier to try out alternative
+  // pseudoinverse methods in the solvers
+  return jac.completeOrthogonalDecomposition().pseudoInverse();
+}
+
+auto damped_least_squares(const Eigen::MatrixXd & jac, const Eigen::VectorXd & damping) -> Eigen::MatrixXd
+{
+  if (damping.size() != jac.rows()) {
+    throw std::invalid_argument("Damping vector must have the same size as the number of rows in the Jacobian.");
+  }
+  return jac.transpose() * ((jac * jac.transpose()) + damping.asDiagonal().toDenseMatrix()).inverse();
+}
+
+auto damped_least_squares(const Eigen::MatrixXd & jac, double damping) -> Eigen::MatrixXd
+{
+  return damped_least_squares(jac, damping * Eigen::MatrixXd::Identity(jac.rows(), jac.rows()));
+}
+
+}  // namespace ik_solvers::pinv

ik_solvers/src/pseudoinverse.hpp

@@ -0,0 +1,39 @@
+// Copyright 2025, Evan Palmer
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#include <Eigen/Dense>
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace ik_solvers::pinv
+{
+
+/// Calculate the pseudoinverse of a Jacobian matrix using the Moore-Penrose method.
+auto least_squares(const Eigen::MatrixXd & jac) -> Eigen::MatrixXd;
+
+/// Calculate the pseudoinverse of a Jacobian matrix using the damped least squares method.
+///
+/// This version allows for more granular selection of the damping values to tune the solution velocities.
+auto damped_least_squares(const Eigen::MatrixXd & jac, const Eigen::VectorXd & damping) -> Eigen::MatrixXd;
+
+/// Calculate the pseudoinverse of a Jacobian matrix using the damped least squares method.
+auto damped_least_squares(const Eigen::MatrixXd & jac, double damping) -> Eigen::MatrixXd;
+
+}  // namespace ik_solvers::pinv

ik_solvers/src/task_priority_solver.cpp

@@ -28,6 +28,7 @@
 #include "pinocchio/algorithm/frames.hpp"
 #include "pinocchio/algorithm/jacobian.hpp"
 #include "pinocchio/algorithm/kinematics.hpp"
+#include "pseudoinverse.hpp"
 #include "tf2_eigen/tf2_eigen.hpp"
 
 namespace ik_solvers
@@ -173,7 +174,7 @@ namespace
 auto compute_nullspace(const Eigen::MatrixXd & augmented_jacobian) -> Eigen::MatrixXd
 {
   const Eigen::MatrixXd eye = Eigen::MatrixXd::Identity(augmented_jacobian.cols(), augmented_jacobian.cols());
-  return eye - (augmented_jacobian.completeOrthogonalDecomposition().pseudoInverse() * augmented_jacobian);
+  return eye - (pinv::least_squares(augmented_jacobian) * augmented_jacobian);
 }
 
 /// Construct the augmented Jacobian matrix from a list of Jacobian matrices.
@@ -198,42 +199,37 @@ auto construct_augmented_jacobian(const std::vector<Eigen::MatrixXd> & jacobians
   return augmented_jacobian;
 }
 
-/// Closed-loop task priority IK using the damped pseudoinverse.
+/// Closed-loop task priority IK.
 auto tpik(const hierarchy::ConstraintSet & tasks, size_t nv, double damping)
   -> std::expected<Eigen::VectorXd, SolverError>
 {
   if (tasks.empty()) {
     return std::unexpected(SolverError::NO_SOLUTION);
   }
 
-  Eigen::VectorXd vel = Eigen::VectorXd::Zero(nv);
   std::vector<Eigen::MatrixXd> jacobians;
   jacobians.reserve(tasks.size());
+
+  Eigen::VectorXd vel = Eigen::VectorXd::Zero(nv);
   Eigen::MatrixXd nullspace = Eigen::MatrixXd::Identity(nv, nv);
 
   for (const auto & task : tasks) {
-    // damped pseudoinverse of the Jacobian
-    const Eigen::MatrixXd J = task->jacobian();
-    const auto eye = Eigen::MatrixXd::Identity(J.rows(), J.rows());
-    const Eigen::MatrixXd J_inv = J.transpose() * (J * J.transpose() + damping * eye).inverse();
-
-    // closed-loop task priority IK
+    const Eigen::MatrixXd J_inv = pinv::damped_least_squares(task->jacobian(), damping);
     vel += nullspace * J_inv * (task->gain() * task->error());
 
-    jacobians.push_back(J);
+    jacobians.push_back(task->jacobian());
     nullspace = compute_nullspace(construct_augmented_jacobian(jacobians));
   }
 
   return vel;
 }
 
-/// Check if the solution is feasible with respect to the set constraints.
+/// Check if the solution is feasible.
 auto is_feasible(const hierarchy::ConstraintSet & constraints, const Eigen::VectorXd & solution) -> bool
 {
   for (const auto & constraint : constraints) {
     auto set_task = std::dynamic_pointer_cast<hierarchy::SetConstraint>(constraint);
     const double pred = (constraint->jacobian() * solution).value();
-
     if (!((set_task->primal() > set_task->lower_threshold() && pred < 0) ||
           (set_task->primal() < set_task->upper_threshold() && pred > 0))) {
       return false;
@@ -253,39 +249,32 @@ auto search_solutions(
     return std::unexpected(SolverError::NO_SOLUTION);
   }
 
-  Eigen::VectorXd solution;
-
+  // there is only one solution if there are no set tasks
   if (set_tasks.empty()) {
-    const auto out = tpik(hierarchies.front(), nv, damping);
-    if (!out.has_value()) {
-      return std::unexpected(out.error());
-    }
-    solution = out.value();
-  } else {
-    std::vector<Eigen::VectorXd> solutions;
-    solutions.reserve(hierarchies.size());
-
-    for (const auto & tasks : hierarchies) {
-      const auto out = tpik(tasks, nv, damping);
-      if (!out.has_value()) {
-        continue;
-      }
+    return tpik(hierarchies.front(), nv, damping);
+  }
 
-      const Eigen::VectorXd & current_solution = out.value();
-      if (is_feasible(set_tasks, current_solution)) {
-        solutions.push_back(current_solution);
-      }
+  std::vector<Eigen::VectorXd> solutions;
+  solutions.reserve(hierarchies.size());
+
+  for (const auto & tasks : hierarchies) {
+    const auto out = tpik(tasks, nv, damping);
+    if (!out.has_value()) {
+      continue;
     }
 
-    if (solutions.empty()) {
-      return std::unexpected(SolverError::NO_SOLUTION);
+    const Eigen::VectorXd & current_solution = out.value();
+    if (is_feasible(set_tasks, current_solution)) {
+      solutions.push_back(current_solution);
     }
+  }
 
-    // Choose the solution with the smallest norm
-    solution = *std::ranges::min_element(solutions, {}, [](const auto & a) { return a.norm(); });
+  if (solutions.empty()) {
+    return std::unexpected(SolverError::NO_SOLUTION);
   }
 
-  return solution;
+  // Choose the solution with the smallest norm
+  return *std::ranges::min_element(solutions, {}, [](const auto & a) { return a.norm(); });
 }
 
 auto pinocchio_to_eigen(const pinocchio::SE3 & pose) -> Eigen::Affine3d
@@ -354,7 +343,7 @@ auto TaskPriorityIKSolver::solve_ik(const Eigen::Affine3d & target_pose, const E
     const double activation = params_.joint_limit_task.constrained_joints_map[joint_name].activation_threshold;
     const double joint_limit_gain = params_.joint_limit_task.constrained_joints_map[joint_name].gain;
 
-    // insert the joint constraint
+    // insert the joint limit constraint
     hierarchy_.insert(std::make_shared<hierarchy::JointConstraint>(
       model_, primal, ub, lb, tol, activation, joint_name, joint_limit_gain));
   }

velocity_controllers/README.md

@@ -48,7 +48,6 @@ T<sub>ry</sub>, T<sub>rz</sub> in N and Nm, respectively.
 
 - adaptive_integral_terminal_sliding_mode_controller/reference [geometry_msgs::msg::Twist]
 - adaptive_integral_terminal_sliding_mode_controller/system_state [nav_msgs::msg::Odometry]
-- robot_description [std_msgs::msg::String]
 
 ### Publishers
 
@@ -97,7 +96,6 @@ T<sub>ry</sub>, T<sub>rz</sub> in N and Nm, respectively.
 
 - integral_sliding_mode_controller/reference [geometry_msgs::msg::Twist]
 - integral_sliding_mode_controller/system_state [nav_msgs::msg::Odometry]
-- robot_description [std_msgs::msg::String]
 
 ### Publishers
 

whole_body_controllers/README.md

@@ -1,4 +1,41 @@
 # Whole Body Controllers
 
 This package provides a collection of whole-body controllers for underwater
-vehicle manipulator systems.
+vehicle manipulator systems (UVMS).
+
+## Inverse Kinematics Controller
+
+A chainable controller that uses inverse kinematics solvers to compute joint
+states required to achieve a desired end effector pose. This controller
+can be used with UVMS that have a single manipulator.
+
+### Plugin Library
+
+whole_body_controllers/ik_controller
+
+### References
+
+- Target end effector pose $\eta$<sub>EE</sub>
+
+### State Feedback
+
+- Measured system velocity V: V<sub>vehicle</sub>, V<sub>manipulator</sub>
+- Measured system position q: q<sub>vehicle</sub>, q<sub>manipulator</sub>
+
+### Commands
+
+The output of this controller is the solution to the chosen inverse kinematics
+solver.
+
+### Subscribers
+
+- ik_controller/reference [geometry_msgs::msg::Pose]
+- ik_controller/vehicle_state [nav_msgs::msg::Odometry]
+
+> [!NOTE]
+> The IK controller does not currently support sending manipulator states via
+> a topic interface.
+
+### Publishers
+
+- ik_controller/status [auv_control_msgs::msg::IKControllerStateStamped]