Inverse kinematics solution with additional custom joint constraints.
Opened this issue · 9 comments
Is your feature request related to a problem? Please describe.
When using setFromIK, I am receiving an inverse kinematics solution that respects joint limits and avoids collision with objects in the robot's workspace, which is good.
However, I cannot limit the solution space to additional custom-made joint constraints (moveit_msgs::msg::JointConstraint), at least I have not found the way to add them yet.
Describe the solution you'd like
I would be happy if I could pass additional parameter (array of custom joint constraints) to setFromIK that would further limit joint solution space, in addition to joint limits.
Describe alternatives you've considered
I considered looking for IK solution and then check whether it satisfies custom joint constraints manually. However, it is time costly.
When looking into setFromIK, I saw that there is a const GroupStateValidityCallbackFn& constraint parameter. Is it possible to use it to check the validity of the sampled state including additional joint constraints?
Additional context
No response
The GroupStateValidityCallbackFn can be used to create custom validation for sampled IK solutions. In my case, I use it to validate solutions against additional joint constraints. For reference, the code is provided below.
The current logic is that the solution is sampled and then validated.
However, I am curious whether it is possible to directly apply constraints to IK solver. In my case, it would be to update the solution space to exclude joint configurations that are beyond user-defined joint constraints.
Code:
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <thread>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/msg/display_trajectory.hpp>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_state/robot_state.h>
int main(int argc, char *argv[])
{
// Initialize ROS and create the Node
rclcpp::init(argc, argv);
auto const node = std::make_shared<rclcpp::Node>(
"joint_constraints_node",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
// Create a ROS logger
auto const logger = rclcpp::get_logger("joint_constraints_node");
// Spin up a SingleThreadedExecutor
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
auto spinner = std::thread([&executor]()
{ executor.spin(); });
const std::string planning_group_name = "<planning_group_name>";
// Create the MoveIt MoveGroup Interface
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, planning_group_name);
robot_model_loader::RobotModelLoader robot_model_loader(node);
const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();
moveit::core::RobotStatePtr kinematic_state(new moveit::core::RobotState(kinematic_model));
const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup(planning_group_name);
auto current_pose = move_group_interface.getCurrentPose();
moveit_msgs::msg::Constraints mixed_constraints;
moveit_msgs::msg::JointConstraint joint1_constraint;
joint1_constraint.joint_name = "joint_1";
joint1_constraint.position = 0.0;
joint1_constraint.tolerance_below = 0.0872665;
joint1_constraint.tolerance_above = 1.65806;
joint1_constraint.weight = 0.2;
mixed_constraints.joint_constraints.emplace_back(joint1_constraint);
move_group_interface.setPathConstraints(mixed_constraints);
// Set a target Pose
auto const target_pose = [¤t_pose]
{
geometry_msgs::msg::Pose msg;
msg.orientation = current_pose.pose.orientation;
msg.position.x = 0.0;
msg.position.y = 1.125;
msg.position.z = 0.3;
return msg;
}();
double timeout = 1.0; // max time to find an IK soution
bool found_ik = kinematic_state->setFromIK(joint_model_group, target_pose, timeout,
[&](moveit::core::RobotState* goal_state, const moveit::core::JointModelGroup* group, const double* ik_solution) {
const std::vector<std::string>& joint_names = group->getVariableNames();
for (size_t i = 0; i < joint_names.size(); ++i)
{
RCLCPP_INFO(logger, "ik_solution[%s]: %.6f", joint_names[i].c_str(), ik_solution[i]);
}
// Apply the IK solution to the robot state. Otherwise, its joint values are 0.
goal_state->setJointGroupPositions(group, ik_solution);
RCLCPP_INFO(logger, "Evaluating IK solution...");
for (const auto& joint_constr : mixed_constraints.joint_constraints)
{
const double position = goal_state->getVariablePosition(joint_constr.joint_name);
RCLCPP_INFO(logger,
"Joint '%s': position=%.6f, constraint=[%.6f, %.6f]",
joint_constr.joint_name.c_str(), position,
joint_constr.position - joint_constr.tolerance_below,
joint_constr.position + joint_constr.tolerance_above);
// Check if the position is within the custom joint constraint bounds
if (position < joint_constr.position - joint_constr.tolerance_below ||
position > joint_constr.position + joint_constr.tolerance_above)
{
RCLCPP_WARN(logger, "Rejected IK solution: Joint '%s' out of bounds.",
joint_constr.joint_name.c_str());
return false; // Reject IK solution
}
}
RCLCPP_INFO(logger, "IK solution accepted.");
return true; // Accept IK solution
});
if (found_ik)
{
std::vector<double> joint_values;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); //joint_values can be used to set joint target for path-planning.
}
else
{
RCLCPP_ERROR(logger, "No valid IK solution found.");
}
// Shutdown ROS
rclcpp::shutdown(); // <--- This will cause the spin function in the thread to return
spinner.join(); // <--- Join the thread before exiting
return 0;
}
Unfortunately, the IK solver API doesn't allow the passing of constraints. Hence, constraints can only be considered via validation and rejection.
Thank you for your reply @rhaschke! I appreciate the clarification!
I have some additional questions for potential workarounds / enhancements.
- Is it possible to update API to include joint constraints?
- Could the available joint space (a combination of joint position limits and joint constraints) be pre-processed and passed to the IK solver API as "updated joint position limits"?
I'd suggest to fork bio_ik and add whatever constraints you need there. That's exactly what I've been doing. In other words, build bio_ik from source.
I know that bio_ik in principle can handle constraints. However, you need to call the service directly, don't you?
MoveIt's plugin API for IK solvers doesn't allow to pass constraints:
I modified bio_ik/src/kinematics_plugin.cpp
I modified
bio_ik/src/kinematics_plugin.cpp
Can you provide a source link for reference?
Reading #3114 (comment) again, I'm afraid you simply hard-coded your constraints into the plugin call?
My custom constraints were added here: https://github.com/PickNikRobotics/bio_ik/blob/ad75b2e8085338311a2f6e7224f43b9b1f35a79c/src/kinematics_plugin.cpp#L300
Thank you @AndyZe for your input!
However, I am unsure how flexible is the solution to manually modify the source code of IK solvers to include additional joint constraints.
I believe that an API enhancement could really benefit developers due to its convenience and ability to update constraints in a run time.
It is possible that I am not fully understanding the impact of these changes on the rest of the system. In this case, I would really appreciate further clarification of current implementation and its specifics.