Support for Relative TCP Pose IK in Dual-Arm Systems?

[Yucheng-Tang]

Background

I am working on using relative TCP pose as a target to calculate IK solutions for a dual-arm setup. While exploring MoveIt 2, I noticed that a Move Group must be a single kinematic chain. Based on this constraint, a common approach for dual-arm usage seems to be:

1. Each robot arm is defined as an individual Move Group.
2. A higher-level Move Group (dual_arm) is created that contains both arms as subgroups.
3. The IK solver computes solutions separately for each arm.

I would like to ask if anyone has already attempted to calculate IK considering the dual-arm setup as a single system rather than solving each arm independently.

Feature Request & Proposed Solutions

If MoveIt 2 does not currently support this, are there any existing requirements or discussions about such a feature?

I see two potential wrapper solutions that could make this work:

1. Reverse one arm’s kinematic chain to create a single continuous kinematic chain of 12-14 DOF (depending on the robots). This approach would allow MoveIt’s existing solvers to handle the problem as a redundant arm.

   • Concerns:

     • Joint limits (kinematic and dynamic) for the reversed arm must also be flipped, which may introduce complications for path planning and trajectory generation.

     • MoveIt’s default path planning and trajectory generation algorithms might not handle the reversed limits properly.

2. Extend MoveIt to support multi-chain systems natively.

   • I have already tried and implemented similar functionality using Relaxed IK and CuRoBo, and I believe this could work well with some modifications to MoveIt’s IK solver interface.

   • If this feature would be useful for others, I would be interested in contributing to MoveIt to enable multi-chain IK solving.

Discussion: Why Is Relative TCP Pose Rarely Used?

In general, I have noticed that relative TCP pose control is not widely supported in dual-arm or humanoid robot implementations.

• Is there a fundamental reason why relative pose IK is not commonly implemented?
• For tasks where only the relative pose (or object-related pose) is important, wouldn't this be a more natural and flexible approach?
• Low-level motion planning could still optimize additional metrics like manipulability, stiffness, or energy efficiency.

I know this is not strictly an issue related to MoveIt 2's codebase, but I’m really curious about this topic and would appreciate any insights from the community. Thanks in advance:)

[sea-bass]

Two things:

1. The requirement of the group being a single kinematic chain is not so much MoveIt wide as it is IK plugin dependent. The default IK implementation (KDL) requires a chain, but you could write your own non-chain plugin.

In fact, if you look at the interface, there is a function signature for solving IK with multiple tip links:

moveit2/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.hpp

Line 199 in dbf07b1

virtual bool getPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses,

2. You're right that MoveIt core doesn't have much foundation for relative motion via things like velocity IK and operational space control. The moveit_servo package is an attempt at doing this, but it similarly only operates on single-frame goals at the moment.

Furthermore, the IK solves are not velocity based, but rather position based, which is computationally inefficient. There is a configuration of MoveIt Servo that bypasses the expensive position IK checks and does a Jacobian transpose step, but it has none of the bells and whistles of nullspace projection tasks for joint limits, singularities, collisions, etc. These are all currently implemented as monitoring checks that will slow down the robot if approaching such limits... but it's not proactive about these constraints.

[Yucheng-Tang]

Thanks for the quick response! The first point you mentioned is a great hint—I’ll definitely check out that interface.

Regarding the second point, I think I didn’t explain my question clearly. What I want to calculate as the input for IK is actually the relative pose between the TCPs of a dual-arm setup, rather than the relative TCP pose of a single arm with respect to time (which you mentioned as Velocity IK above).

From what I’ve seen, most dual-arm setups handle path planning together but solve IK separately for each arm before merging the results while considering collisions. (dual_franka_moveit_config does the same stuff, which means two TCP target pose would be given for IK calculation). In brief, for example, if I have two Franka robots, the input of IK would consist of only a 6-dimensional Cartesian pose describing the relationship between the two TCPs. To achieve this, I need to load two kinematic chains into a single MoveGroup.

MoveIt-Specific Question:

I checked the moveit2_kinematics module, and from what I see, MoveIt’s IK plugins require the robot model to be a single kinematic chain. If I define my setup with a world or table as base_link, and two separate chains (one for each arm) parented to this base, the IK plugin fails to load correctly. I can show the error message from moveit2_kinematics later when I’m back from the trip. In other words, I cannot create a MoveGroup that contains both robot arms unless I either:

• Use two separate MoveGroups, solving IK for each arm independently (same as in dual_franka_moveit_config), or

• Manually modify the URDF to merge both arms into a single kinematic chain (e.g., setting one arm’s TCP as the base link of the entire kinematic chain).

So the question would be: Does MoveIt support handling dual kinematic chains as one MoveGroup in any way? Or is this inherently limited by the current MoveIt IK plugin structure?

Thanks again for the patient response!

[sea-bass]

Ah, yes, so for the second point I misunderstood that question. Best bet for doing that is to do the math yourself to convert from the relative coordinates to absolute coordinates for each end effector.

For your new question, I think you may need to check the source code / try it out, as I am not sure.

I know that SRDF itself supports making a group that contains multiple subgroups. For example:

<group name="both_arms">
  <group name="left_arm"/>
  <group name="right_arm"/>
</group>

Source: https://wiki.ros.org/srdf

Then, somewhere along the line of MoveIt parsing the models you get errors about chains. I think (but I could be wrong), these errors are only specific to the KDL inverse kinematics plugin, so in theory (going back to my first answer) you can make or find a different IK plugin that does support one of these "mega groups" containing two independent chains.

But again, I'm not sure myself ... hopefully the rest of the MoveIt core beyond the IK plugin supports this structure?

[Yucheng-Tang]

Thanks again for the response!:)

I still have some follow-up questions, and I would really appreciate it if you or someone from the community could answer them.

First, regarding MoveIt, I have indeed used the method you mentioned, where I define a group that contains two subgroups.

However, the errors I encountered are not from moveit_kinematics but rather frommoveit_core/kinematic_base:

bool KinematicsBase::supportsGroup(const moveit::core::JointModelGroup* jmg, std::string* error_text_out) const
{
  // Default implementation for legacy solvers:
  if (!jmg->isChain())
  {
    if (error_text_out)
    {
      *error_text_out = "This plugin only supports joint groups which are chains";
    }
    return false;
  }

  return true;
}

Since I am not very familiar with MoveIt's internal structure, I am wondering:

• Would you still recommend modifying MoveIt myself to support a relative coordinate dual-arm setup, or do you think it would be too much overhead, given that MoveIt does not seem to support multi-chain structures at the core level?

Discussion: Why Is Relative TCP Pose Rarely Used?

Let's revisit my first and most important question: Why isn't relative TCP pose more widely used in robotics?

In your response, you mentioned:

"Best bet for doing that is to do the math yourself to convert from the relative coordinates to absolute coordinates for each end effector."

According to this part, you mean converting the relative coordinate back to absolute coordinates for each EE.

I understand this approach, but the main advantage of using a relative TCP coordinate is that the entire kinematic chain (with more than 12 DOF) can be treated as a highly redundant manipulator. This allows us to exploit the null space or infinite solution space to optimize additional metrics. The most commonly used method here is the Relative Jacobian Method, which, for example, can maintain the relative TCP pose while allowing both robotic arms to move in Cartesian space to optimize stiffness or manipulability

But this method is a kind of velocity IK, making it suitable for servoing and jogging IK problem (target pose is close to the current pose). For "jumping" IK problem (target pose is far away from the current pose) velocity IK still needs to go through a process to reach the target pose, which is obviously prone to local minima (local optimization method).

The real question here (which I realize is now beyond MoveIt) is:

• Why has there been so little research on this topic?
• Why do we not see more work on handling "jumping" IK problems with global optimization?

From what I’ve seen, Relative Jacobian methods were actively researched between 2015–2018, but since then, there seems to be a lack of further exploration.

Is there a fundamental challenge in extending these methods for global optimization:

• impractical computing time?
• difficulty in converting an NP-hard problem into a convex optimization problem?
• not that much improvement compared to absolute coordinate methods?

, or is it simply a gap in research interest?

Thanks again for your patience and help!

[sea-bass]

I think that KinematicsBase::supportsGroup() can be overridden by your child class (a new plugin) to not error when the group is not a chain. This is just a default implementation.

As for your second discussion point... that's right. Most of the underlying tools like MoveIt's RobotModel, or Pinocchio, or Drake give you what you need to compute Jacobians at specific joint configurations. At this point, your application could convert these Jacobians to things like collision, relative to other (non-base) frames, relative motion for 2 end effectors, and so on.

I think these problems are all super interesting, just that nobody seems to have made tools that specifically go this far to assist you in formulating these problems. It's probably a good opportunity to develop something to help with these problems.

Also, there have been tons of methods to handle global minima in IK! Most of them nowadays deal with using GPUs to parallelize random seeds and/or to apply ML.approaches. Some standout ones are cuRobo, IKFlow, and RelaxedIK.

[Yucheng-Tang]

Okay, I’ll go through the MoveIt source code to check for any other issues and try to override them with my own modifications.

Regarding the second discussion point, thanks a lot for the explanation based on your experience! I’ve already modified RelaxedIK and cuRobo to work with a relative coordinate dual-arm setup, and I’ll continue running more experiments to explore this further.

I really appreciate the insights and advice—thanks again!:)