Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
07c83d40eThis file is pinned to the selected release. Download the original to open it in your design tool.
#include <geometry_msgs/msg/pose.hpp>
#include <kalman_interfaces/action/pose_ik_navigate_to_pose.hpp>
#include <kalman_interfaces/msg/arm_values.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <Eigen/Dense>
#include <kdl/chain.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/jacobian.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/tree.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <std_msgs/msg/string.hpp>
#include <urdf/model.h>
#include <algorithm>
#include <cmath>
#include <functional>
#include <memory>
#include <mutex>
#include <sstream>
#include <string>
#include <vector>
namespace kalman_arm2 {
class PoseIK : public rclcpp::Node {
public:
using Action = kalman_interfaces::action::PoseIKNavigateToPose;
using GoalHandle = rclcpp_action::ServerGoalHandle<Action>;
static constexpr const char *ARM_LINK_END = "arm_link_end";
static constexpr const char *ARM_LINK_GRIPPER = "arm_link_gripper";
rclcpp::Subscription<kalman_interfaces::msg::ArmValues>::SharedPtr
joint_pos_sub;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr
robot_description_sub;
rclcpp::Publisher<kalman_interfaces::msg::ArmValues>::SharedPtr joint_vel_pub;
rclcpp_action::Server<Action>::SharedPtr action_server;
rclcpp::TimerBase::SharedPtr compute_timer;
std::shared_ptr<tf2_ros::TransformListener> tf_listener;
std::shared_ptr<tf2_ros::Buffer> tf_buffer;
std::string base_link;
float max_joint_vel;
double update_rate;
double default_timeout_seconds;
double base_damping;
double max_damping;
double singularity_sigma_threshold;
double joint_centering_gain;
bool enable_singularity_logging;
double singularity_log_period_ms;
double linear_kp;
double angular_kp;
double max_linear_speed;
double min_linear_speed;
double min_speed_tolerance_scale;
double max_angular_speed;
double fine_approach_distance;
double fine_approach_angular_scale;
double fine_approach_posture_scale;
double position_tolerance;
double orientation_tolerance_rad;
KDL::Chain arm_link_end_chain;
KDL::Chain arm_link_gripper_chain;
std::unique_ptr<KDL::ChainJntToJacSolver> arm_link_end_jacobian_solver;
std::unique_ptr<KDL::ChainJntToJacSolver>
arm_link_gripper_jacobian_solver;
KDL::Chain *active_chain = nullptr;
KDL::ChainJntToJacSolver *active_jacobian_solver = nullptr;
KDL::JntArray current_joint_positions;
Eigen::VectorXd joint_preferred_positions;
Eigen::VectorXd joint_motion_weights;
Eigen::VectorXd singularity_avoidance_gains;
Eigen::VectorXd singularity_avoidance_thresholds;
Eigen::VectorXd singularity_preferred_positions;
bool joints_initialized = false;
bool kinematics_ready = false;
std::mutex state_mutex;
std::shared_ptr<GoalHandle> active_goal;
bool goal_reserved = false;
rclcpp::Time deadline;
PoseIK(const rclcpp::NodeOptions &options) : Node("pose_ik", options) {
declare_parameter<std::string>("base_link", "base_link");
declare_parameter<float>("max_joint_vel", 0.5);
declare_parameter<double>("update_rate", 10.0);
declare_parameter<double>("default_timeout_seconds", 10.0);
declare_parameter<double>("base_damping", 0.03);
declare_parameter<double>("max_damping", 0.35);
declare_parameter<double>("singularity_sigma_threshold", 0.12);
declare_parameter<double>("joint_centering_gain", 0.35);
declare_parameter<bool>("enable_singularity_logging", true);
declare_parameter<double>("singularity_log_period_ms", 1000.0);
declare_parameter<std::vector<double>>(
"joint_motion_weights", std::vector<double>{}
);
declare_parameter<std::vector<double>>(
"singularity_avoidance_gains", std::vector<double>{}
);
declare_parameter<std::vector<double>>(
"singularity_avoidance_thresholds", std::vector<double>{}
);
declare_parameter<std::vector<double>>(
"singularity_preferred_positions", std::vector<double>{}
);
declare_parameter<double>("linear_kp", 0.8);
declare_parameter<double>("angular_kp", 2.0);
declare_parameter<double>("max_linear_speed", 0.12);
declare_parameter<double>("min_linear_speed", 0.015);
declare_parameter<double>("min_speed_tolerance_scale", 1.5);
declare_parameter<double>("max_angular_speed", 0.6);
declare_parameter<double>("fine_approach_distance", 0.08);
declare_parameter<double>("fine_approach_angular_scale", 0.2);
declare_parameter<double>("fine_approach_posture_scale", 0.0);
declare_parameter<double>("position_tolerance", 0.01);
declare_parameter<double>("orientation_tolerance_rad", 0.15);
get_parameter("base_link", base_link);
get_parameter("max_joint_vel", max_joint_vel);
get_parameter("update_rate", update_rate);
get_parameter("default_timeout_seconds", default_timeout_seconds);
if (!std::isfinite(default_timeout_seconds) ||
default_timeout_seconds <= 0.0) {
RCLCPP_WARN(
get_logger(),
"default_timeout_seconds must be positive; using 10.0 seconds"
);
default_timeout_seconds = 10.0;
}
get_parameter("base_damping", base_damping);
get_parameter("max_damping", max_damping);
get_parameter(
"singularity_sigma_threshold", singularity_sigma_threshold
);
get_parameter("joint_centering_gain", joint_centering_gain);
get_parameter("enable_singularity_logging", enable_singularity_logging);
get_parameter("singularity_log_period_ms", singularity_log_period_ms);
get_parameter("linear_kp", linear_kp);
get_parameter("angular_kp", angular_kp);
get_parameter("max_linear_speed", max_linear_speed);
get_parameter("min_linear_speed", min_linear_speed);
get_parameter("min_speed_tolerance_scale", min_speed_tolerance_scale);
get_parameter("max_angular_speed", max_angular_speed);
get_parameter("fine_approach_distance", fine_approach_distance);
get_parameter(
"fine_approach_angular_scale", fine_approach_angular_scale
);
get_parameter(
"fine_approach_posture_scale", fine_approach_posture_scale
);
get_parameter("position_tolerance", position_tolerance);
get_parameter("orientation_tolerance_rad", orientation_tolerance_rad);
tf_buffer = std::make_shared<tf2_ros::Buffer>(get_clock());
tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);
joint_vel_pub = create_publisher<kalman_interfaces::msg::ArmValues>(
"target_vel", 10
);
joint_pos_sub = create_subscription<kalman_interfaces::msg::ArmValues>(
"current_pos",
10,
std::bind(&PoseIK::on_joint_positions, this, std::placeholders::_1)
);
using namespace std::placeholders;
action_server = rclcpp_action::create_server<Action>(
this,
"pose_ik_navigate_to_pose",
std::bind(&PoseIK::handle_goal, this, _1, _2),
std::bind(&PoseIK::handle_cancel, this, _1),
std::bind(&PoseIK::handle_accepted, this, _1)
);
auto timer_period = std::chrono::duration<double>(1.0 / update_rate);
compute_timer = create_wall_timer(
timer_period, std::bind(&PoseIK::compute_joint_velocities, this)
);
rclcpp::QoS robot_description_qos(1);
robot_description_qos.transient_local();
robot_description_qos.reliable();
robot_description_sub = create_subscription<std_msgs::msg::String>(
"/robot_description",
robot_description_qos,
std::bind(
&PoseIK::on_robot_description, this, std::placeholders::_1
)
);
}
Eigen::VectorXd vector_from_parameter(
const std::string &name, size_t size, const Eigen::VectorXd &fallback
) {
auto values = get_parameter(name).as_double_array();
if (values.empty()) {
return fallback;
}
if (values.size() != size) {
RCLCPP_WARN(
get_logger(),
"Parameter '%s' has %zu elements, expected %zu. Using "
"defaults.",
name.c_str(),
values.size(),
size
);
return fallback;
}
Eigen::VectorXd result(size);
for (size_t i = 0; i < size; ++i) {
result(i) = values[i];
}
return result;
}
std::string format_vector(const Eigen::VectorXd &values) const {
std::ostringstream stream;
stream << "[";
for (Eigen::Index i = 0; i < values.size(); ++i) {
if (i != 0) {
stream << ", ";
}
stream << values(i);
}
stream << "]";
return stream.str();
}
void on_robot_description(const std_msgs::msg::String::SharedPtr msg) {
{
std::lock_guard<std::mutex> lock(state_mutex);
if (kinematics_ready) {
return;
}
}
urdf::Model model;
if (!model.initString(msg->data)) {
RCLCPP_ERROR(
get_logger(), "Failed to parse URDF from /robot_description."
);
return;
}
KDL::Tree kdl_tree;
if (!kdl_parser::treeFromUrdfModel(model, kdl_tree)) {
RCLCPP_ERROR(get_logger(), "Failed to create KDL tree from URDF.");
return;
}
KDL::Chain end_chain;
KDL::Chain gripper_chain;
if (!kdl_tree.getChain(base_link, ARM_LINK_END, end_chain)) {
RCLCPP_ERROR(
get_logger(),
"Failed to extract KDL chain from '%s' to '%s'.",
base_link.c_str(),
ARM_LINK_END
);
return;
}
if (!kdl_tree.getChain(base_link, ARM_LINK_GRIPPER, gripper_chain)) {
RCLCPP_ERROR(
get_logger(),
"Failed to extract KDL chain from '%s' to '%s'.",
base_link.c_str(),
ARM_LINK_GRIPPER
);
return;
}
const size_t nj = end_chain.getNrOfJoints();
if (nj == 0 || gripper_chain.getNrOfJoints() != nj) {
RCLCPP_ERROR(
get_logger(),
"PoseIK chains must have the same non-zero movable joint count "
"('%s': %u, '%s': %u).",
ARM_LINK_END,
end_chain.getNrOfJoints(),
ARM_LINK_GRIPPER,
gripper_chain.getNrOfJoints()
);
return;
}
Eigen::VectorXd preferred_positions = Eigen::VectorXd::Zero(nj);
Eigen::VectorXd motion_weights = Eigen::VectorXd::Ones(nj);
Eigen::VectorXd avoidance_gains = Eigen::VectorXd::Zero(nj);
Eigen::VectorXd avoidance_thresholds = Eigen::VectorXd::Zero(nj);
Eigen::VectorXd singularity_positions = Eigen::VectorXd::Zero(nj);
size_t joint_index = 0;
for (const auto &segment : end_chain.segments) {
const auto &joint = segment.getJoint();
if (joint.getType() == KDL::Joint::None) {
continue;
}
if (auto urdf_joint = model.getJoint(joint.getName())) {
if (urdf_joint->limits) {
preferred_positions(joint_index) =
0.5 *
(urdf_joint->limits->lower + urdf_joint->limits->upper);
}
}
++joint_index;
}
if (nj > 3) {
motion_weights(3) = 10.0;
}
if (nj > 4) {
motion_weights(4) = 0.5;
}
if (nj > 5) {
avoidance_gains(5) = 1.2;
avoidance_thresholds(5) = 0.35;
singularity_positions(5) = 1.4;
}
motion_weights = vector_from_parameter(
"joint_motion_weights", nj, motion_weights
);
avoidance_gains = vector_from_parameter(
"singularity_avoidance_gains", nj, avoidance_gains
);
avoidance_thresholds = vector_from_parameter(
"singularity_avoidance_thresholds", nj, avoidance_thresholds
);
singularity_positions = vector_from_parameter(
"singularity_preferred_positions", nj, singularity_positions
);
std::lock_guard<std::mutex> lock(state_mutex);
if (kinematics_ready) {
return;
}
arm_link_end_chain = end_chain;
arm_link_gripper_chain = gripper_chain;
arm_link_end_jacobian_solver =
std::make_unique<KDL::ChainJntToJacSolver>(arm_link_end_chain);
arm_link_gripper_jacobian_solver =
std::make_unique<KDL::ChainJntToJacSolver>(arm_link_gripper_chain);
joint_preferred_positions = std::move(preferred_positions);
joint_motion_weights = std::move(motion_weights);
singularity_avoidance_gains = std::move(avoidance_gains);
singularity_avoidance_thresholds = std::move(avoidance_thresholds);
singularity_preferred_positions = std::move(singularity_positions);
current_joint_positions.resize(nj);
kinematics_ready = true;
RCLCPP_INFO(
get_logger(),
"PoseIK initialized both tip chains with %zu joints. "
"motion_weights=%s avoidance_gains=%s",
nj,
format_vector(joint_motion_weights).c_str(),
format_vector(singularity_avoidance_gains).c_str()
);
}
void
on_joint_positions(const kalman_interfaces::msg::ArmValues::SharedPtr msg) {
std::lock_guard<std::mutex> lock(state_mutex);
if (!kinematics_ready) {
return;
}
if (msg->joints.size() < current_joint_positions.rows()) {
return;
}
for (size_t i = 0; i < current_joint_positions.rows(); ++i) {
current_joint_positions(i) = msg->joints[i];
}
joints_initialized = true;
}
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID &uuid,
const std::shared_ptr<const Action::Goal> goal
) {
(void)uuid;
std::lock_guard<std::mutex> lock(state_mutex);
if (!kinematics_ready) {
RCLCPP_WARN(get_logger(), "Rejecting PoseIK goal: kinematics not ready");
return rclcpp_action::GoalResponse::REJECT;
}
if (goal_reserved || active_goal) {
RCLCPP_WARN(get_logger(), "Rejecting PoseIK goal: server is busy");
return rclcpp_action::GoalResponse::REJECT;
}
if (goal->end_effector_link != ARM_LINK_END &&
goal->end_effector_link != ARM_LINK_GRIPPER) {
RCLCPP_WARN(
get_logger(),
"Rejecting PoseIK goal: unsupported end-effector link '%s'",
goal->end_effector_link.c_str()
);
return rclcpp_action::GoalResponse::REJECT;
}
if (goal->base_frame.empty()) {
RCLCPP_WARN(get_logger(), "Rejecting PoseIK goal: base frame is empty");
return rclcpp_action::GoalResponse::REJECT;
}
goal_reserved = true;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse
handle_cancel(const std::shared_ptr<GoalHandle> goal_handle) {
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void handle_accepted(const std::shared_ptr<GoalHandle> goal_handle) {
std::lock_guard<std::mutex> lock(state_mutex);
const auto &goal = *goal_handle->get_goal();
if (goal.end_effector_link == ARM_LINK_END) {
active_chain = &arm_link_end_chain;
active_jacobian_solver = arm_link_end_jacobian_solver.get();
} else {
active_chain = &arm_link_gripper_chain;
active_jacobian_solver = arm_link_gripper_jacobian_solver.get();
}
const double timeout =
std::isfinite(goal.timeout_seconds) && goal.timeout_seconds > 0.0
? goal.timeout_seconds
: default_timeout_seconds;
deadline = now() + rclcpp::Duration::from_seconds(timeout);
active_goal = goal_handle;
goal_reserved = false;
}
Eigen::VectorXd current_joint_positions_eigen() const {
Eigen::VectorXd q(current_joint_positions.rows());
for (size_t i = 0; i < current_joint_positions.rows(); ++i) {
q(i) = current_joint_positions(i);
}
return q;
}
double compute_adaptive_damping(double min_sigma) const {
if (min_sigma >= singularity_sigma_threshold) {
return base_damping;
}
if (singularity_sigma_threshold <= 0.0) {
return max_damping;
}
const double ratio = 1.0 - (min_sigma / singularity_sigma_threshold);
return base_damping + (max_damping - base_damping) * ratio * ratio;
}
double compute_min_singular_value(const Eigen::MatrixXd &jacobian) const {
Eigen::JacobiSVD<Eigen::MatrixXd> svd(
jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV
);
if (svd.singularValues().size() == 0) {
return 0.0;
}
return svd.singularValues().minCoeff();
}
Eigen::VectorXd compute_posture_bias(const Eigen::VectorXd &q) const {
Eigen::VectorXd bias =
joint_centering_gain * (joint_preferred_positions - q);
for (Eigen::Index i = 0; i < q.size(); ++i) {
const double gain = singularity_avoidance_gains(i);
const double threshold = singularity_avoidance_thresholds(i);
if (gain <= 0.0 || threshold <= 0.0) {
continue;
}
const double joint_angle = q(i);
if (std::abs(joint_angle) < threshold) {
const double preferred = singularity_preferred_positions(i);
const double direction = joint_angle >= 0.0 ? 1.0 : -1.0;
const double signed_preferred =
preferred == 0.0 ? 0.0 : direction * std::abs(preferred);
const double ratio = 1.0 - (std::abs(joint_angle) / threshold);
bias(i) += gain * ratio * (signed_preferred - joint_angle);
}
}
return bias;
}
bool solve_joint_velocities(
const geometry_msgs::msg::Twist &base_twist,
double position_error,
bool position_only,
Eigen::VectorXd &joint_velocities,
double &min_sigma_out,
double &damping_out,
Eigen::Matrix<double, 6, 1> &achieved_twist_out
) {
if (!active_chain || !active_jacobian_solver) {
return false;
}
KDL::Jacobian kdl_jacobian(active_chain->getNrOfJoints());
const int jacobian_result = active_jacobian_solver->JntToJac(
current_joint_positions, kdl_jacobian
);
if (jacobian_result < 0) {
return false;
}
const Eigen::MatrixXd jacobian = kdl_jacobian.data;
Eigen::MatrixXd weight_inv =
joint_motion_weights.cwiseInverse().asDiagonal();
Eigen::MatrixXd active_jacobian;
Eigen::VectorXd twist_vector;
if (position_only) {
active_jacobian = jacobian.topRows(3);
twist_vector.resize(3);
twist_vector << base_twist.linear.x, base_twist.linear.y,
base_twist.linear.z;
} else {
active_jacobian = jacobian;
twist_vector.resize(6);
twist_vector << base_twist.linear.x, base_twist.linear.y,
base_twist.linear.z, base_twist.angular.x, base_twist.angular.y,
base_twist.angular.z;
}
const double min_sigma = compute_min_singular_value(active_jacobian);
const double damping = compute_adaptive_damping(min_sigma);
const Eigen::MatrixXd task_metric =
active_jacobian * weight_inv * active_jacobian.transpose() +
(damping * damping) *
Eigen::MatrixXd::Identity(
active_jacobian.rows(), active_jacobian.rows()
);
const Eigen::MatrixXd weighted_pseudoinverse =
weight_inv * active_jacobian.transpose() *
task_metric.ldlt().solve(
Eigen::MatrixXd::Identity(
active_jacobian.rows(), active_jacobian.rows()
)
);
const Eigen::VectorXd task_velocity =
weighted_pseudoinverse * twist_vector;
const Eigen::VectorXd q = current_joint_positions_eigen();
Eigen::VectorXd posture_bias = compute_posture_bias(q);
if (position_error < fine_approach_distance) {
posture_bias *= fine_approach_posture_scale;
}
const Eigen::MatrixXd nullspace_projector =
Eigen::MatrixXd::Identity(q.size(), q.size()) -
weighted_pseudoinverse * active_jacobian;
joint_velocities = task_velocity + nullspace_projector * posture_bias;
min_sigma_out = min_sigma;
damping_out = damping;
achieved_twist_out = jacobian * joint_velocities;
if (enable_singularity_logging) {
RCLCPP_INFO_THROTTLE(
get_logger(),
*get_clock(),
static_cast<int64_t>(singularity_log_period_ms),
"PoseIK diagnostics: sigma_min=%.4f damping=%.4f mode=%s "
"qdot=%s",
min_sigma,
damping,
position_only ? "position_only" : "full_pose",
format_vector(joint_velocities).c_str()
);
}
return true;
}
geometry_msgs::msg::Pose transform_pose_to_base_frame(
const geometry_msgs::msg::Pose &pose, const std::string &source_frame
) {
if (source_frame == base_link) {
return pose;
}
const auto transform = tf_buffer->lookupTransform(
base_link,
source_frame,
rclcpp::Time(0, 0, get_clock()->get_clock_type()),
rclcpp::Duration::from_seconds(0.1)
);
geometry_msgs::msg::Pose base_pose;
tf2::doTransform(pose, base_pose, transform);
return base_pose;
}
geometry_msgs::msg::Pose
current_ee_pose(const std::string &end_effector_link) {
auto transform = tf_buffer->lookupTransform(
base_link,
end_effector_link,
rclcpp::Time(0, 0, get_clock()->get_clock_type()),
rclcpp::Duration::from_seconds(0.1)
);
geometry_msgs::msg::Pose pose;
pose.position.x = transform.transform.translation.x;
pose.position.y = transform.transform.translation.y;
pose.position.z = transform.transform.translation.z;
pose.orientation = transform.transform.rotation;
return pose;
}
double quaternion_angle_to_target(
const geometry_msgs::msg::Quaternion ¤t_msg,
const geometry_msgs::msg::Quaternion &target_msg
) const {
tf2::Quaternion q_current, q_target;
tf2::fromMsg(current_msg, q_current);
tf2::fromMsg(target_msg, q_target);
q_current.normalize();
q_target.normalize();
tf2::Quaternion q_error = q_current.inverse() * q_target;
q_error.normalize();
if (q_error.getW() < 0.0) {
q_error = tf2::Quaternion(
-q_error.getX(),
-q_error.getY(),
-q_error.getZ(),
-q_error.getW()
);
}
const double w =
std::clamp(static_cast<double>(q_error.getW()), -1.0, 1.0);
return 2.0 * std::acos(w);
}
geometry_msgs::msg::Twist pose_error_to_twist(
const geometry_msgs::msg::Pose ¤t,
const geometry_msgs::msg::Pose &target,
double position_error
) const {
geometry_msgs::msg::Twist twist;
tf2::Vector3 linear_error(
target.position.x - current.position.x,
target.position.y - current.position.y,
target.position.z - current.position.z
);
tf2::Vector3 linear_cmd = linear_error * linear_kp;
const double linear_speed = linear_cmd.length();
const double min_speed_disable_distance =
position_tolerance * min_speed_tolerance_scale;
if (linear_speed > max_linear_speed) {
linear_cmd *= max_linear_speed / linear_speed;
} else if (linear_speed > 1e-9 &&
position_error > min_speed_disable_distance &&
linear_speed < min_linear_speed) {
linear_cmd *= min_linear_speed / linear_speed;
}
twist.linear.x = linear_cmd.x();
twist.linear.y = linear_cmd.y();
twist.linear.z = linear_cmd.z();
tf2::Quaternion q_current, q_target;
tf2::fromMsg(current.orientation, q_current);
tf2::fromMsg(target.orientation, q_target);
q_current.normalize();
q_target.normalize();
// KDL Jacobian angular rows are expressed in base_link, so use the
// spatial rotation error rather than the body-frame rotation error.
tf2::Quaternion q_error = q_target * q_current.inverse();
q_error.normalize();
if (q_error.getW() < 0.0) {
q_error = tf2::Quaternion(
-q_error.getX(),
-q_error.getY(),
-q_error.getZ(),
-q_error.getW()
);
}
const double w =
std::clamp(static_cast<double>(q_error.getW()), -1.0, 1.0);
const double angle = 2.0 * std::acos(w);
const double s = std::sqrt(std::max(1e-16, 1.0 - w * w));
tf2::Vector3 axis(1.0, 0.0, 0.0);
if (s > 1e-8) {
axis = tf2::Vector3(
q_error.getX() / s, q_error.getY() / s, q_error.getZ() / s
);
}
tf2::Vector3 angular_cmd = axis * angle * angular_kp;
const double angular_speed = angular_cmd.length();
if (angular_speed > max_angular_speed) {
angular_cmd *= max_angular_speed / angular_speed;
}
twist.angular.x = angular_cmd.x();
twist.angular.y = angular_cmd.y();
twist.angular.z = angular_cmd.z();
if (position_error < fine_approach_distance) {
twist.angular.x *= fine_approach_angular_scale;
twist.angular.y *= fine_approach_angular_scale;
twist.angular.z *= fine_approach_angular_scale;
}
return twist;
}
void publish_zero_velocity() {
auto vel_msg = kalman_interfaces::msg::ArmValues();
vel_msg.header.stamp = now();
vel_msg.joints.fill(0.0);
vel_msg.jaw = 0.0;
joint_vel_pub->publish(vel_msg);
}
void clear_active_goal() {
active_goal.reset();
active_chain = nullptr;
active_jacobian_solver = nullptr;
goal_reserved = false;
}
void compute_joint_velocities() {
std::lock_guard<std::mutex> lock(state_mutex);
if (!active_goal) {
return;
}
if (active_goal->is_canceling()) {
publish_zero_velocity();
auto result = std::make_shared<Action::Result>();
result->result = false;
result->message = "Pose IK navigation canceled";
active_goal->canceled(result);
clear_active_goal();
return;
}
if (!active_goal->is_active()) {
publish_zero_velocity();
clear_active_goal();
return;
}
if (now() >= deadline) {
publish_zero_velocity();
auto result = std::make_shared<Action::Result>();
result->result = false;
result->message = "Pose IK navigation timed out";
active_goal->abort(result);
clear_active_goal();
return;
}
if (!kinematics_ready || !joints_initialized) {
return;
}
const auto goal = active_goal->get_goal();
try {
const geometry_msgs::msg::Pose target_pose =
transform_pose_to_base_frame(goal->pose, goal->base_frame);
const geometry_msgs::msg::Pose current_pose =
current_ee_pose(goal->end_effector_link);
const double dx = target_pose.position.x - current_pose.position.x;
const double dy = target_pose.position.y - current_pose.position.y;
const double dz = target_pose.position.z - current_pose.position.z;
const double position_error =
std::sqrt(dx * dx + dy * dy + dz * dz);
const double orientation_error = quaternion_angle_to_target(
current_pose.orientation, target_pose.orientation
);
auto feedback = std::make_shared<Action::Feedback>();
feedback->position_error = position_error;
feedback->orientation_error = orientation_error;
active_goal->publish_feedback(feedback);
if (position_error <= position_tolerance &&
orientation_error <= orientation_tolerance_rad) {
publish_zero_velocity();
auto result = std::make_shared<Action::Result>();
result->result = true;
result->message = "Target pose reached";
active_goal->succeed(result);
clear_active_goal();
return;
}
const geometry_msgs::msg::Twist base_twist =
pose_error_to_twist(current_pose, target_pose, position_error);
const bool position_only =
orientation_error <= orientation_tolerance_rad &&
position_error < fine_approach_distance;
Eigen::VectorXd joint_velocities;
double min_sigma = 0.0;
double damping = 0.0;
Eigen::Matrix<double, 6, 1> achieved_twist =
Eigen::Matrix<double, 6, 1>::Zero();
if (!solve_joint_velocities(
base_twist,
position_error,
position_only,
joint_velocities,
min_sigma,
damping,
achieved_twist
)) {
return;
}
const double max_computed_vel =
joint_velocities.cwiseAbs().maxCoeff();
if (max_computed_vel > max_joint_vel) {
joint_velocities *= max_joint_vel / max_computed_vel;
}
auto vel_msg = kalman_interfaces::msg::ArmValues();
vel_msg.header.stamp = now();
for (Eigen::Index i = 0; i < joint_velocities.size(); ++i) {
vel_msg.joints[i] = joint_velocities(i);
}
joint_vel_pub->publish(vel_msg);
const double achieved_linear_speed = std::sqrt(
achieved_twist(0) * achieved_twist(0) +
achieved_twist(1) * achieved_twist(1) +
achieved_twist(2) * achieved_twist(2)
);
const double commanded_linear_speed = std::sqrt(
base_twist.linear.x * base_twist.linear.x +
base_twist.linear.y * base_twist.linear.y +
base_twist.linear.z * base_twist.linear.z
);
const char *gate_state =
position_error > position_tolerance
? (orientation_error > orientation_tolerance_rad
? "position+orientation"
: "position")
: "orientation";
#if 0
RCLCPP_INFO_THROTTLE(
get_logger(),
*get_clock(),
1000,
"PoseIK target=(%.3f %.3f %.3f) current=(%.3f %.3f %.3f) "
"pos_err=(%.3f %.3f %.3f) pos_norm=%.4f ang_err=%.4f gate=%s "
"mode=%s cmd_v=(%.3f %.3f %.3f) |cmd_v|=%.4f "
"cmd_w=(%.3f %.3f %.3f) sigma_min=%.4f damping=%.4f "
"q=(%.3f %.3f %.3f %.3f %.3f %.3f) "
"qdot=(%.5f %.5f %.5f %.5f %.5f %.5f) "
"achieved_v=(%.3f %.3f %.3f) |achieved_v|=%.4f",
target_pose.position.x,
target_pose.position.y,
target_pose.position.z,
current_pose.position.x,
current_pose.position.y,
current_pose.position.z,
dx,
dy,
dz,
position_error,
orientation_error,
gate_state,
position_only ? "position_only" : "full_pose",
base_twist.linear.x,
base_twist.linear.y,
base_twist.linear.z,
commanded_linear_speed,
base_twist.angular.x,
base_twist.angular.y,
base_twist.angular.z,
min_sigma,
damping,
current_joint_positions(0),
current_joint_positions(1),
current_joint_positions(2),
current_joint_positions(3),
current_joint_positions(4),
current_joint_positions(5),
joint_velocities(0),
joint_velocities(1),
joint_velocities(2),
joint_velocities(3),
joint_velocities(4),
joint_velocities(5),
achieved_twist(0),
achieved_twist(1),
achieved_twist(2),
achieved_linear_speed
);
#endif
} catch (const tf2::TransformException &ex) {
RCLCPP_WARN_THROTTLE(
get_logger(),
*get_clock(),
1000,
"PoseIK TF problem for '%s' in '%s': %s",
goal->end_effector_link.c_str(),
goal->base_frame.c_str(),
ex.what()
);
}
}
};
} // namespace kalman_arm2
RCLCPP_COMPONENTS_REGISTER_NODE(kalman_arm2::PoseIK)