Skip to content
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

Multi-dof gains #224

Merged
merged 1 commit into from
Nov 20, 2024
Merged

Multi-dof gains #224

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
10 changes: 7 additions & 3 deletions lbr_description/ros2_control/lbr_controllers.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,15 +97,17 @@
ros__parameters:
robot_name: lbr
admittance:
mass: 0.01
damping: 1.0
stiffness: 0.0
mass: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
damping: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
stiffness: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
inv_jac_ctrl:
chain_root: lbr_link_0
chain_tip: lbr_link_ee
damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian
max_linear_velocity: 2.0 # maximum linear velocity
max_angular_velocity: 2.0 # maximum linear acceleration
joint_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # joint gains
cartesian_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # cartesian gains

/**/twist_controller:
ros__parameters:
Expand All @@ -117,4 +119,6 @@
damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian
max_linear_velocity: 0.1 # maximum linear velocity
max_angular_velocity: 0.1 # maximum linear acceleration
joint_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # joint gains
cartesian_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # cartesian gains
timeout: 0.2 # stop controller if no command is received within this time [s]
42 changes: 28 additions & 14 deletions lbr_fri_ros2/include/lbr_fri_ros2/control.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,12 +17,14 @@

namespace lbr_fri_ros2 {
struct InvJacCtrlParameters {
std::string chain_root;
std::string chain_tip;
bool twist_in_tip_frame;
double damping;
double max_linear_velocity;
double max_angular_velocity;
std::string chain_root = "lbr_link_0";
std::string chain_tip = "lbr_link_ee";
bool twist_in_tip_frame = true;
double damping = 0.2;
double max_linear_velocity = 0.1;
double max_angular_velocity = 0.1;
jnt_array_t joint_gains = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
cart_array_t cartesian_gains = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
};

class InvJacCtrlImpl {
Expand All @@ -44,41 +46,51 @@ class InvJacCtrlImpl {

protected:
void compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq);
void set_all_zero_();

InvJacCtrlParameters parameters_;

jnt_array_t q_;
std::unique_ptr<Kinematics> kinematics_ptr_;
Eigen::Matrix<double, N_JNTS, CARTESIAN_DOF> jacobian_inv_;
Eigen::Matrix<double, CARTESIAN_DOF, 1> twist_target_;
Eigen::Matrix<double, N_JNTS, 1> q_gains_;
Eigen::Matrix<double, CARTESIAN_DOF, 1> x_gains_;
};

struct AdmittanceParameters {
AdmittanceParameters() = delete;
AdmittanceParameters(const double &m = 1.0, const double &b = 0.1, const double &k = 0.0)
AdmittanceParameters(const_cart_array_t_ref m = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0},
const_cart_array_t_ref b = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
const_cart_array_t_ref k = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0})
: m(m), b(b), k(k) {
if (m <= 0.0) {
if (std::any_of(m.cbegin(), m.cend(), [](const double &m_i) { return m_i <= 0.0; })) {
throw std::runtime_error("Mass must be positive and greater zero.");
}
if (b < 0.0) {
if (std::any_of(b.cbegin(), b.cend(), [](const double &b_i) { return b_i < 0.0; })) {
throw std::runtime_error("Damping must be positive.");
}
if (k < 0.0) {
if (std::any_of(k.cbegin(), k.cend(), [](const double &k_i) { return k_i < 0.0; })) {
throw std::runtime_error("Stiffness must be positive.");
}
}

double m = 1.0;
double b = 0.1;
double k = 0.0;
cart_array_t m = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
cart_array_t b = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1};
cart_array_t k = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
};

class AdmittanceImpl {
protected:
static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::AdmittanceImpl";

public:
AdmittanceImpl(const AdmittanceParameters &parameters) : parameters_(parameters) {}
AdmittanceImpl() = delete;
AdmittanceImpl(const AdmittanceParameters &parameters) : parameters_(parameters) {
m_ = Eigen::Map<Eigen::Matrix<double, CARTESIAN_DOF, 1>>(parameters_.m.data());
b_ = Eigen::Map<Eigen::Matrix<double, CARTESIAN_DOF, 1>>(parameters_.b.data());
k_ = Eigen::Map<Eigen::Matrix<double, CARTESIAN_DOF, 1>>(parameters_.k.data());
}

void compute(const Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> &f_ext,
const Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> &x,
Expand All @@ -89,6 +101,8 @@ class AdmittanceImpl {

protected:
AdmittanceParameters parameters_;

Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> m_, b_, k_;
};
} // namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__CONTROL_HPP_
69 changes: 51 additions & 18 deletions lbr_fri_ros2/src/control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,10 @@ InvJacCtrlImpl::InvJacCtrlImpl(const std::string &robot_description,
: parameters_(parameters) {
kinematics_ptr_ = std::make_unique<Kinematics>(robot_description, parameters_.chain_root,
parameters_.chain_tip);
set_all_zero_();
q_gains_ = Eigen::Map<Eigen::Matrix<double, N_JNTS, 1>>(parameters_.joint_gains.data());
x_gains_ =
Eigen::Map<Eigen::Matrix<double, CARTESIAN_DOF, 1>>(parameters_.cartesian_gains.data());
}

void InvJacCtrlImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target,
Expand Down Expand Up @@ -56,6 +60,43 @@ void InvJacCtrlImpl::log_info() const {
parameters_.max_linear_velocity);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Max angular velocity: %.3f",
parameters_.max_angular_velocity);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME),
"* Joint gains: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f]",
parameters_.joint_gains[0], parameters_.joint_gains[1], parameters_.joint_gains[2],
parameters_.joint_gains[3], parameters_.joint_gains[4], parameters_.joint_gains[5],
parameters_.joint_gains[6]);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME),
"* Cartesian gains: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]",
parameters_.cartesian_gains[0], parameters_.cartesian_gains[1],
parameters_.cartesian_gains[2], parameters_.cartesian_gains[3],
parameters_.cartesian_gains[4], parameters_.cartesian_gains[5]);
}

void AdmittanceImpl::compute(const Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> &f_ext,
const Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> &x,
const Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> &dx,
Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> &ddx) {
if ((m_.array() < 0.).any()) {
throw std::runtime_error("Mass must be positive and greater zero.");
}
ddx = (f_ext - b_.asDiagonal() * dx - k_.asDiagonal() * x).array() / m_.array();
}

void AdmittanceImpl::log_info() const {
{
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:");
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Mass: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]",
parameters_.m[0], parameters_.m[1], parameters_.m[2], parameters_.m[3],
parameters_.m[4], parameters_.m[5]);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME),
"* Damping: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", parameters_.b[0],
parameters_.b[1], parameters_.b[2], parameters_.b[3], parameters_.b[4],
parameters_.b[5]);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME),
"* Stiffness: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", parameters_.k[0],
parameters_.k[1], parameters_.k[2], parameters_.k[3], parameters_.k[4],
parameters_.k[5]);
}
}

void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) {
Expand All @@ -76,30 +117,22 @@ void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq)
Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.bottomRows(3);
}

twist_target_ = x_gains_.asDiagonal() * twist_target_;

// compute jacobian
auto jacobian = kinematics_ptr_->compute_jacobian(q);
jacobian_inv_ = pinv(jacobian.data, parameters_.damping);

// compute target joint veloctiy and map it to dq
Eigen::Map<Eigen::Matrix<double, N_JNTS, 1>>(dq.data()) = jacobian_inv_ * twist_target_;
Eigen::Map<Eigen::Matrix<double, N_JNTS, 1>>(dq.data()) =
q_gains_.asDiagonal() * jacobian_inv_ * twist_target_;
}

void AdmittanceImpl::compute(const Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> &f_ext,
const Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> &x,
const Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> &dx,
Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> &ddx) {
if (parameters_.m <= 0.0) {
throw std::runtime_error("Mass must be positive and greater zero.");
}
ddx = (f_ext - parameters_.b * dx - parameters_.k * x) / parameters_.m;
}

void AdmittanceImpl::log_info() const {
{
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:");
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Mass: %.3f", parameters_.m);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Damping: %.3f", parameters_.b);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Stiffness: %.3f", parameters_.k);
}
void InvJacCtrlImpl::set_all_zero_() {
std::fill(q_.begin(), q_.end(), 0.0);
jacobian_inv_.setZero();
twist_target_.setZero();
q_gains_.setZero();
x_gains_.setZero();
}
} // namespace lbr_fri_ros2
94 changes: 85 additions & 9 deletions lbr_ros2_control/src/controllers/admittance_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,14 +39,21 @@ AdmittanceController::state_interface_configuration() const {
controller_interface::CallbackReturn AdmittanceController::on_init() {
try {
this->get_node()->declare_parameter("robot_name", "lbr");
this->get_node()->declare_parameter("admittance.mass", 1.0);
this->get_node()->declare_parameter("admittance.damping", 0.1);
this->get_node()->declare_parameter("admittance.stiffness", 0.0);
this->get_node()->declare_parameter("admittance.mass",
std::vector<double>(lbr_fri_ros2::CARTESIAN_DOF, 1.0));
this->get_node()->declare_parameter("admittance.damping",
std::vector<double>(lbr_fri_ros2::CARTESIAN_DOF, 0.0));
this->get_node()->declare_parameter("admittance.stiffness",
std::vector<double>(lbr_fri_ros2::CARTESIAN_DOF, 0.0));
this->get_node()->declare_parameter("inv_jac_ctrl.chain_root", "lbr_link_0");
this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee");
this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2);
this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1);
this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1);
this->get_node()->declare_parameter("inv_jac_ctrl.joint_gains",
std::vector<double>(lbr_fri_ros2::N_JNTS, 0.0));
this->get_node()->declare_parameter("inv_jac_ctrl.cartesian_gains",
std::vector<double>(lbr_fri_ros2::CARTESIAN_DOF, 0.0));
configure_joint_names_();
configure_admittance_impl_();
configure_inv_jac_ctrl_impl_();
Expand Down Expand Up @@ -201,14 +208,82 @@ void AdmittanceController::configure_joint_names_() {
}

void AdmittanceController::configure_admittance_impl_() {
admittance_impl_ptr_ =
std::make_unique<lbr_fri_ros2::AdmittanceImpl>(lbr_fri_ros2::AdmittanceParameters{
this->get_node()->get_parameter("admittance.mass").as_double(),
this->get_node()->get_parameter("admittance.damping").as_double(),
this->get_node()->get_parameter("admittance.stiffness").as_double()});
if (this->get_node()->get_parameter("admittance.mass").as_double_array().size() !=
lbr_fri_ros2::CARTESIAN_DOF) {
RCLCPP_ERROR(this->get_node()->get_logger(),
"Number of mass values (%ld) does not match the number of cartesian degrees of "
"freedom (%d).",
this->get_node()->get_parameter("admittance.mass").as_double_array().size(),
lbr_fri_ros2::CARTESIAN_DOF);
throw std::runtime_error("Failed to configure admittance parameters.");
}
if (this->get_node()->get_parameter("admittance.damping").as_double_array().size() !=
lbr_fri_ros2::CARTESIAN_DOF) {
RCLCPP_ERROR(
this->get_node()->get_logger(),
"Number of damping values (%ld) does not match the number of cartesian degrees of freedom "
"(%d).",
this->get_node()->get_parameter("admittance.damping").as_double_array().size(),
lbr_fri_ros2::CARTESIAN_DOF);
throw std::runtime_error("Failed to configure admittance parameters.");
}
if (this->get_node()->get_parameter("admittance.stiffness").as_double_array().size() !=
lbr_fri_ros2::CARTESIAN_DOF) {
RCLCPP_ERROR(this->get_node()->get_logger(),
"Number of stiffness values (%ld) does not match the number of cartesian degrees "
"of freedom "
"(%d).",
this->get_node()->get_parameter("admittance.stiffness").as_double_array().size(),
lbr_fri_ros2::CARTESIAN_DOF);
throw std::runtime_error("Failed to configure admittance parameters.");
}
lbr_fri_ros2::cart_array_t mass_array;
for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
mass_array[i] = this->get_node()->get_parameter("admittance.mass").as_double_array()[i];
}
lbr_fri_ros2::cart_array_t damping_array;
for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
damping_array[i] = this->get_node()->get_parameter("admittance.damping").as_double_array()[i];
}
lbr_fri_ros2::cart_array_t stiffness_array;
for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
stiffness_array[i] =
this->get_node()->get_parameter("admittance.stiffness").as_double_array()[i];
}
admittance_impl_ptr_ = std::make_unique<lbr_fri_ros2::AdmittanceImpl>(
lbr_fri_ros2::AdmittanceParameters{mass_array, damping_array, stiffness_array});
}

void AdmittanceController::configure_inv_jac_ctrl_impl_() {
if (this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size() !=
lbr_fri_ros2::N_JNTS) {
RCLCPP_ERROR(
this->get_node()->get_logger(),
"Number of joint gains (%ld) does not match the number of joints in the robot (%d).",
this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size(),
lbr_fri_ros2::N_JNTS);
throw std::runtime_error("Failed to configure joint gains.");
}
if (this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size() !=
lbr_fri_ros2::CARTESIAN_DOF) {
RCLCPP_ERROR(
this->get_node()->get_logger(),
"Number of cartesian gains (%ld) does not match the number of cartesian degrees of freedom "
"(%d).",
this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size(),
lbr_fri_ros2::CARTESIAN_DOF);
throw std::runtime_error("Failed to configure cartesian gains.");
}
lbr_fri_ros2::jnt_array_t joint_gains_array;
for (unsigned int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
joint_gains_array[i] =
this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array()[i];
}
lbr_fri_ros2::cart_array_t cartesian_gains_array;
for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
cartesian_gains_array[i] =
this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array()[i];
}
inv_jac_ctrl_impl_ptr_ = std::make_unique<lbr_fri_ros2::InvJacCtrlImpl>(
this->get_robot_description(),
lbr_fri_ros2::InvJacCtrlParameters{
Expand All @@ -217,7 +292,8 @@ void AdmittanceController::configure_inv_jac_ctrl_impl_() {
false, // always assume twist in root frame
this->get_node()->get_parameter("inv_jac_ctrl.damping").as_double(),
this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(),
this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double()});
this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double(),
joint_gains_array, cartesian_gains_array});
}

void AdmittanceController::zero_all_values_() {
Expand Down
36 changes: 35 additions & 1 deletion lbr_ros2_control/src/controllers/twist_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,10 @@ controller_interface::CallbackReturn TwistController::on_init() {
this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2);
this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1);
this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1);
this->get_node()->declare_parameter("inv_jac_ctrl.joint_gains",
std::vector<double>(lbr_fri_ros2::N_JNTS, 0.0));
this->get_node()->declare_parameter("inv_jac_ctrl.cartesian_gains",
std::vector<double>(lbr_fri_ros2::CARTESIAN_DOF, 0.0));
this->get_node()->declare_parameter("timeout", 0.2);
configure_joint_names_();
configure_inv_jac_ctrl_impl_();
Expand Down Expand Up @@ -166,6 +170,35 @@ void TwistController::configure_joint_names_() {
}

void TwistController::configure_inv_jac_ctrl_impl_() {
if (this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size() !=
lbr_fri_ros2::N_JNTS) {
RCLCPP_ERROR(
this->get_node()->get_logger(),
"Number of joint gains (%ld) does not match the number of joints in the robot (%d).",
this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size(),
lbr_fri_ros2::N_JNTS);
throw std::runtime_error("Failed to configure joint gains.");
}
if (this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size() !=
lbr_fri_ros2::CARTESIAN_DOF) {
RCLCPP_ERROR(
this->get_node()->get_logger(),
"Number of cartesian gains (%ld) does not match the number of cartesian degrees of freedom "
"(%d).",
this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size(),
lbr_fri_ros2::CARTESIAN_DOF);
throw std::runtime_error("Failed to configure cartesian gains.");
}
lbr_fri_ros2::jnt_array_t joint_gains_array;
for (unsigned int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
joint_gains_array[i] =
this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array()[i];
}
lbr_fri_ros2::cart_array_t cartesian_gains_array;
for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
cartesian_gains_array[i] =
this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array()[i];
}
inv_jac_ctrl_impl_ptr_ = std::make_unique<lbr_fri_ros2::InvJacCtrlImpl>(
this->get_robot_description(),
lbr_fri_ros2::InvJacCtrlParameters{
Expand All @@ -174,7 +207,8 @@ void TwistController::configure_inv_jac_ctrl_impl_() {
this->get_node()->get_parameter("inv_jac_ctrl.twist_in_tip_frame").as_bool(),
this->get_node()->get_parameter("inv_jac_ctrl.damping").as_double(),
this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(),
this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double()});
this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double(),
joint_gains_array, cartesian_gains_array});
}

void TwistController::log_info_() const { inv_jac_ctrl_impl_ptr_->log_info(); }
Expand Down
Loading