Skip to content

Added redundancy resolution with joint damping term through null-space projection #55

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Prier merged 4 commits into from
Sep 1, 2026
Merged

Added redundancy resolution with joint damping term through null-space projection #55

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
082356f
Added redundancy resolution with joint damping term through null-spac…
Prier Aug 27, 2026
00430c6
Added pi constant instead of M_PI in eurofusion_cartesian_motion_cont…
Prier Aug 27, 2026
14e0866
Since MSVC cannot implicitly convert std::filesystem::path to std::st…
Prier Aug 27, 2026
346bc45
Added a function set_Kd_null() for setting the damping used for maint…
Prier Aug 27, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions examples/simulation/eurofusion/cpp/eurofusion_cartesian_motion_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@
using namespace sdu_controllers;
using namespace sdu_controllers::utils;

constexpr double pi = 3.14159265358979323846;

Check warning on line 17 in examples/simulation/eurofusion/cpp/eurofusion_cartesian_motion_controller.cpp

View check run for this annotation

SonarQubeCloud / SonarCloud Code Analysis

Replace constant with "std::numbers::pi". 


See more on https://sonarcloud.io/project/issues?id=SDU-Robotics_sdu_controllers&issues=AaBCyulHVW9Uhc9pjquF&open=AaBCyulHVW9Uhc9pjquF&pullRequest=55

int main()
{
// Setup writing of output trajectory to csv.
Expand Down Expand Up @@ -87,9 +89,9 @@
pos_desired[2] += 2;

Vector3d rpy_zyz_desired = rpy_zyz0;
rpy_zyz_desired[0] += M_PI / 8;
rpy_zyz_desired[0] += pi / 8;
rpy_zyz_desired[1] += 0;
rpy_zyz_desired[2] += M_PI / 2;
rpy_zyz_desired[2] += pi / 2;

x_d << pos_desired, rpy_zyz_desired;
dx_d.setZero();
Expand Down
6 changes: 6 additions & 0 deletions include/sdu_controllers/controllers/operational_space_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,10 +51,16 @@ namespace sdu_controllers::controllers
*/
void set_kappa(double kappa);

/**
* Set damping used for maintaining joint positions on parallel joints.
*/
void set_Kd_null(double Kd_null);

private:
Eigen::VectorXd y_;
Eigen::MatrixXd Kp_, Kd_;
double kappa_;
double Kd_null_;
std::shared_ptr<models::RobotModel> robot_model_;
};

Expand Down
2 changes: 2 additions & 0 deletions include/sdu_controllers/models/parameter_robot_model.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
#include <optional>
#include <vector>
#include <string>
#include <filesystem>

namespace sdu_controllers::models
{
Expand All @@ -24,6 +25,7 @@ namespace sdu_controllers::models
explicit ParameterRobotModel() = default;
explicit ParameterRobotModel(const std::string &yaml_filepath);
explicit ParameterRobotModel(const RobotParameters &params);
explicit ParameterRobotModel(const std::filesystem::path &config_path) : ParameterRobotModel(config_path.string()) {}
virtual ~ParameterRobotModel() = default;

/**
Expand Down
34 changes: 28 additions & 6 deletions src/controllers/operational_space_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,8 @@
Kd_(std::move(Kd)),
robot_model_(std::move(robot_model))
{
kappa_ = 0;
kappa_ = 0.0;
Kd_null_ = 5.0;

Check warning on line 20 in src/controllers/operational_space_controller.cpp

View check run for this annotation

SonarQubeCloud / SonarCloud Code Analysis

Do not assign data members in a constructor. Initialize member "Kd_null_" in an initialization list. 


See more on https://sonarcloud.io/project/issues?id=SDU-Robotics_sdu_controllers&issues=AaBC_ct3IVN40NsQ_m4i&open=AaBC_ct3IVN40NsQ_m4i&pullRequest=55
}

void OperationalSpaceController::step(
Expand Down Expand Up @@ -51,18 +52,34 @@
kappaI.setIdentity(6, 6);
kappaI *= kappa_*kappa_; // todo: should be user configurable

// The following implements damped least-squares, see eq. (3.59)
y_ = J_A.transpose() * (J_A * J_A.transpose() + kappaI).fullPivHouseholderQr().solve(
ddx_d + Kd_ * dx_tilde + Kp_ * x_tilde - Jdot_A * dq
);
// Explicitly calculate the DLS pseudo-inverse, see eq. (3.59)
Eigen::MatrixXd J_pinv = J_A.transpose() *
(J_A * J_A.transpose() + kappaI).fullPivHouseholderQr().solve(Eigen::MatrixXd::Identity(6, 6));

// Calculate the primary task command
Eigen::VectorXd task_cmd = ddx_d + Kd_ * dx_tilde + Kp_ * x_tilde - Jdot_A * dq;

// Project primary task into joint space
Eigen::VectorXd y_primary = J_pinv * task_cmd;

// Calculate the null-space projection matrix: N = I - J_pinv * J_A
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(6, 6);
Eigen::MatrixXd N = I - J_pinv * J_A;

// Secondary task (Joint Damping to stop drift)
Eigen::VectorXd secondary_cmd = -Kd_null_ * dq;

// Combine primary and secondary tasks (see eq. 3.54)
y_ = y_primary + N * secondary_cmd;
}

void OperationalSpaceController::reset()
{
Kp_.setZero();
Kd_.setZero();
y_.setZero();
kappa_ = 0;
kappa_ = 0.0;
Kd_null_ = 5.0;
}

VectorXd OperationalSpaceController::get_output()
Expand All @@ -75,4 +92,9 @@
kappa_ = kappa;
}

void OperationalSpaceController::set_Kd_null(double Kd_null)
{
Kd_null_ = Kd_null;
}

} // namespace sdu_controllers::controllers
Loading