Skip to content

Commit

Permalink
added multi-dof gains (#163)
Browse files Browse the repository at this point in the history
  • Loading branch information
@mhubii
mhubii committed Nov 20, 2024
1 parent eff5a63 commit 7c00ac9
Show file tree
Hide file tree
Showing 5 changed files with 206 additions and 45 deletions.
10 changes: 7 additions & 3 deletions lbr_description/ros2_control/lbr_controllers.yaml
View file
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
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
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
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
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

0 comments on commit 7c00ac9

Please sign in to comment.