Inverse kinematics solution with additional custom joint constraints.

[aizmailovs]

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

[aizmailovs]

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 = [&current_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;

}

[rhaschke]

Unfortunately, the IK solver API doesn't allow the passing of constraints. Hence, constraints can only be considered via validation and rejection.

[aizmailovs]

Thank you for your reply @rhaschke! I appreciate the clarification!

I have some additional questions for potential workarounds / enhancements.

1. Is it possible to update API to include joint constraints?
2. 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"?

[AndyZe]

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.

[rhaschke]

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:

moveit2/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h

Lines 174 to 176 in 6487249

	getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state,
	std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
	const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;

[AndyZe]

I modified bio_ik/src/kinematics_plugin.cpp

[rhaschke]

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?

[AndyZe]

My custom constraints were added here: https://github.com/PickNikRobotics/bio_ik/blob/ad75b2e8085338311a2f6e7224f43b9b1f35a79c/src/kinematics_plugin.cpp#L300

[aizmailovs]

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.