How to use <group_rep>

[mohamedsayed18]

I'm working to doing collaboration between manipulator and external axis. I get this error

Error:   Provided positioner and manipulator base link are not the same
         at line 278 in space/src/motion-planner/tesseract/tesseract_kinematics/src/core/rep_inverse_kinematics.cpp

According to the code, It is normal to have this error
https://github.com/ros-industrial-consortium/tesseract/blob/3857b5252b9fc1fb9de6de976ac6733799fa476e/tesseract_kinematics/src/core/rep_inverse_kinematics.cpp#L276-L280

My question is why and how the external positioner and the manipulator could have the same base link?

[mohamedsayed18]

I defined the manipulator and external axis to have the same base link which is the world in this case

<group name="manipulator">
    <chain base_link="world" tip_link="tool0" />
</group>

<group name="positioner_only">
    <chain base_link="world" tip_link="positioner_tool0"/>
</group>

<group_opw group="manipulator" a1="0.15" a2="-0.115" b="0" c1="0.445" c2="0.7" c3="0.795" c4="0.085" offsets="0.000000 0.000000 -1.570796 0.000000 0.000000 0.000000" sign_corrections="1 1 1 1 1 1"/>

And I made the following group to represent the manipulator and the external axis

<group name="full_manipulator">
    <chain base_link="positioner_tool0" tip_link="tool0"/>
</group>
<group_rep group="full_manipulator">
  <manipulator group="manipulator" ik_solver="OPWInvKin" reach="2.3"/>
  <positioner group="positioner_only" fk_solver="KDLFwdKinChain">
    <joint name="positioner_base_joint" resolution="0.1"/>
    <joint name="positioner_joint_1" resolution="0.1"/>
  </positioner>
</group_rep>

I face the following error

Error:   SimplePlanner failed to generate problem: Forward and Inverse Kinematic objects joints are not ordered the same!.

And this because the first joint in the in forward kinematics is positioner_joint_1, while for inverse kinematics it ispositioner_base_joint
To solve this I decided to declare two chains in the group as following:

<group name="full_manipulator">
    <chain base_link="world" tip_link="positioner_tool0"/>
    <chain base_link="world" tip_link="tool0" />
</group>

Note: I used this file as a reference
Is this a correct approach or not?

[mohamedsayed18]

Using the Joint groups gives a warning
Warning: Joint groups are currently not supported by inverse kinematics!
And I tried to use them with different orders of joints, but as I didn't see any effect on the forward or the inverse kinematics

fwd_kin->getJointNames()
inv_kin->getJointNames()

[Levi-Armstrong]

Warning: Joint groups are currently not supported by inverse kinematics!

The warning is ok. It was not able to automatically generate an inverse kinematics object for the group but you provided one using the REP so one will exist.

[mohamedsayed18]

Defining the full_manipulator group as follows, fails to generate trajectory.

<group name="full_manipulator">
    <chain base_link="world" tip_link="positioner_tool0"/>
    <chain base_link="world" tip_link="tool0" />
</group>

When defining it as follows

<group name="full_manipulator">
    <chain base_link="positioner_tool0" tip_link="tool0"/>
</group>

It gives the following error:

if (inv_kin->getJointNames() != fwd_kin->getJointNames())
  throw std::runtime_error("Forward and Inverse Kinematic objects joints are not ordered the same!");

I checked what is the values of the joints using these two vectors fwd_kin and inv_kin and it looks like this

fwd_kin = {positioner_joint_1, positioner_base_joint, joint_1, ....., joint_6}
inv_kin = {positioner_base_joint, positioner_joint_1, joint_1, ....., joint_6}

so I was thinking to change the robot with an external axis rep to make the positioner joints in the right order
``
https://github.com/ros-industrial-consortium/tesseract/blob/c2aea9f114719533ade08f9dfe65e0589b8e7933/tesseract_kinematics/src/core/rep_inverse_kinematics.cpp#L386
I tried to change this method, but it gives an error when I'm trying to return anything other than the joint_names_
https://github.com/ros-industrial-consortium/tesseract/blob/c2aea9f114719533ade08f9dfe65e0589b8e7933/tesseract_kinematics/src/core/rep_inverse_kinematics.cpp#L148-L152
Is it a good idea to do such changes?

[Levi-Armstrong]

I am currently working on a permanent fix because this has been an issue multiple times for us also. I am going to add a synchronize method to the inverse kinematics which takes forward kinematics object. This way it is handled internally and the user does not need to worry about this going forward. I should have a PR this week for you to test.

[Levi-Armstrong]

Also, I do not think there is a way to make the current version work for the use case.

[Levi-Armstrong]

@mohamedsayed18 I have created the two PR's below where now the inverse kinematics object gets synchronized with the forward kinematics object so the internal data is aligned. I still need to write unit tests but if want to go ahead and test for your application and have an issue please comment on the PR's and I will work through the issues.

• Tesseract PR
• Tesseract Planning PR