Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <kalman_interfaces/msg/arm_values.hpp>
#include <rclcpp/rclcpp.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 <limits>
#include <mutex>
#include <sstream>
#include <vector>
namespace kalman_arm2 {
class PoseIK : public rclcpp::Node {
public:
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub;
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::TimerBase::SharedPtr compute_timer;
std::shared_ptr<tf2_ros::TransformListener> tf_listener;
std::shared_ptr<tf2_ros::Buffer> tf_buffer;
std::string base_link;
std::string end_effector_link;
float max_joint_vel;
double update_rate;
double control_timeout;
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_chain;
std::unique_ptr<KDL::ChainJntToJacSolver> jacobian_solver;
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;
bool kinematics_ready;
bool was_active;
geometry_msgs::msg::PoseStamped::SharedPtr latest_pose_goal;
std::mutex pose_mutex;
rclcpp::Time last_goal_time;
PoseIK(const rclcpp::NodeOptions &options)
: Node("pose_ik", options), joints_initialized(false),
kinematics_ready(false), was_active(false), last_goal_time(now()) {
declare_parameter<std::string>("base_link", "base_link");
declare_parameter<std::string>("end_effector_link", "arm_link_end");
declare_parameter<float>("max_joint_vel", 0.5);
declare_parameter<double>("update_rate", 10.0);
declare_parameter<double>("control_timeout", 0.5);
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("end_effector_link", end_effector_link);
get_parameter("max_joint_vel", max_joint_vel);
get_parameter("update_rate", update_rate);
get_parameter("control_timeout", control_timeout);
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)
);
pose_sub = create_subscription<geometry_msgs::msg::PoseStamped>(
"target_pose",
10,
std::bind(&PoseIK::on_target_pose, this, std::placeholders::_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) {
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;
}
if (!kdl_tree.getChain(base_link, end_effector_link, arm_chain)) {
RCLCPP_ERROR(
get_logger(),
"Failed to extract KDL chain from '%s' to '%s'.",
base_link.c_str(),
end_effector_link.c_str()
);
return;
}
size_t nj = arm_chain.getNrOfJoints();
jacobian_solver = std::make_unique<KDL::ChainJntToJacSolver>(arm_chain);
joint_preferred_positions = Eigen::VectorXd::Zero(nj);
joint_motion_weights = Eigen::VectorXd::Ones(nj);
singularity_avoidance_gains = Eigen::VectorXd::Zero(nj);
singularity_avoidance_thresholds = Eigen::VectorXd::Zero(nj);
singularity_preferred_positions = Eigen::VectorXd::Zero(nj);
size_t joint_index = 0;
for (const auto &segment : arm_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) {
joint_preferred_positions(joint_index) =
0.5 *
(urdf_joint->limits->lower + urdf_joint->limits->upper);
}
}
++joint_index;
}
if (nj > 3) {
joint_motion_weights(3) = 10.0;
}
if (nj > 4) {
joint_motion_weights(4) = 0.5;
}
if (nj > 5) {
singularity_avoidance_gains(5) = 1.2;
singularity_avoidance_thresholds(5) = 0.35;
singularity_preferred_positions(5) = 1.4;
}
joint_motion_weights = vector_from_parameter(
"joint_motion_weights", nj, joint_motion_weights
);
singularity_avoidance_gains = vector_from_parameter(
"singularity_avoidance_gains", nj, singularity_avoidance_gains
);
singularity_avoidance_thresholds = vector_from_parameter(
"singularity_avoidance_thresholds",
nj,
singularity_avoidance_thresholds
);
singularity_preferred_positions = vector_from_parameter(
"singularity_preferred_positions",
nj,
singularity_preferred_positions
);
current_joint_positions.resize(arm_chain.getNrOfJoints());
kinematics_ready = true;
RCLCPP_INFO(
get_logger(),
"PoseIK initialized with %d joints. motion_weights=%s "
"avoidance_gains=%s",
arm_chain.getNrOfJoints(),
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) {
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;
}
void on_target_pose(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
std::lock_guard<std::mutex> lock(pose_mutex);
latest_pose_goal = msg;
last_goal_time = now();
}
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
) {
KDL::Jacobian kdl_jacobian(current_joint_positions.rows());
const int jacobian_result =
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::PoseStamped::SharedPtr &msg
) {
geometry_msgs::msg::Pose base_pose = msg->pose;
if (!msg->header.frame_id.empty() &&
msg->header.frame_id != base_link) {
try {
geometry_msgs::msg::TransformStamped transform =
tf_buffer->lookupTransform(
base_link,
msg->header.frame_id,
msg->header.stamp,
rclcpp::Duration::from_seconds(0.1)
);
tf2::doTransform(msg->pose, base_pose, transform);
} catch (const tf2::TransformException &ex) {
RCLCPP_WARN_THROTTLE(
get_logger(),
*get_clock(),
1000,
"Could not transform pose from '%s' to '%s': %s",
msg->header.frame_id.c_str(),
base_link.c_str(),
ex.what()
);
}
}
return base_pose;
}
geometry_msgs::msg::Pose current_ee_pose() {
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();
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);
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 compute_joint_velocities() {
if (!kinematics_ready || !joints_initialized) {
return;
}
geometry_msgs::msg::PoseStamped::SharedPtr goal_msg;
{
std::lock_guard<std::mutex> lock(pose_mutex);
if (!latest_pose_goal ||
(now() - last_goal_time).seconds() > control_timeout) {
if (was_active) {
publish_zero_velocity();
was_active = false;
}
return;
}
goal_msg = latest_pose_goal;
}
was_active = true;
try {
const geometry_msgs::msg::Pose target_pose =
transform_pose_to_base_frame(goal_msg);
const geometry_msgs::msg::Pose current_pose = current_ee_pose();
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
);
if (position_error <= position_tolerance &&
orientation_error <= orientation_tolerance_rad) {
publish_zero_velocity();
was_active = false;
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: %s",
ex.what()
);
}
}
};
} // namespace kalman_arm2
RCLCPP_COMPONENTS_REGISTER_NODE(kalman_arm2::PoseIK)
d060b5a20This file is pinned to the selected release. Download the original to open it in your design tool.