why "forward_kinematics" is true,but "inverse_kinematics_frame" is error

[DreamLFairy]

I input a joint1 to "forward_kinematics" function ,and return a position, then I input the position to "inverse_kinematics_frame", then return another joint2,but joint1 is not euqal to joint2.
I use moveit in ros and verified, the result of ikpy "forward_kinematics" is true,but the result of ikpy "inverse_kinematics_frame" is error.

[Dongbox]

I have the same question here. I use the function of forward_kinematics to solve joints and get a transformation matrix.
Then I use the function inverse_kinematics_frame to get a plan but it is not true. The joints of the plan can't reach the original position.

And Why the ik function just output one plan?

[Phylliade]

Hey @DreamLFairy @Dongbox, the inverse_kinematics_frame finds a solution to your inverse kinematics problem.
However there may be multiple solutions to the same inverse kinematics problem (i.e. multiple ways to achieve the same goal).

Have you cheked that the forward kinematics of your returned inverse kinematics (i.e. forward_kinematics(inverse_kinematics(target_position)) == target_position?

@Dongbox, what do you mean with "one plan"?

[Dongbox]

@Phylliade Thanks for your answer, the "one plan" here means the return(joints) of function inverse_kinematics_frame.

[Phylliade]

So it just returns one solution to this ik problem

[Dongbox]

yea, I just get one through the optimize and not true.

[Phylliade]

Is the forward kinematics associated with it different from the original target?

[Dongbox]

Right, it shows a little different.
I input the current joints to forward_kinematics and get the transformation matrix.
And input the TM to inverse_kinematics_frame and get the joints.
But I get different poses lastly.
(before)

(finally)

[Phylliade]

I see.

Did you ask IKpy to also do ik on orientation?

Can you paste the code used to generate this?

[Dongbox]

from ikpy.chain import Chain
from ikpy.utils import plot
import numpy as np



walker_left_arm_chain = Chain.from_json_file("./walker_left_arm.json")
walker_right_arm_chain = Chain.from_json_file("./walker_right_arm.json")

walker_left_arm_chain.links = walker_left_arm_chain.links[:8]
walker_right_arm_chain.links = walker_right_arm_chain.links[:8]

walker_left_arm_chain._length = 8
walker_right_arm_chain._length = 8

walker_left_arm_chain.active_links_mask = walker_left_arm_chain.active_links_mask[:8]
walker_right_arm_chain.active_links_mask = walker_right_arm_chain.active_links_mask[:8]
# Plot the initial position.
fig, ax = plot.init_3d_figure()
walker_right_arm_chain.plot([0] * (len(walker_right_arm_chain)), ax)
walker_left_arm_chain.plot([0] * (len(walker_left_arm_chain)), ax)
ax.legend()
plot.show_figure()

# Got my robot poses.
# ...

fig, ax = plot.init_3d_figure()

# That's my current joints from function `forward_kinematics`.
fk_right_value = np.array([
    [0.8949369693114659, 0.005596551731488306, -0.44615748292315355, 0.25711898797037824],
    [0.37640998035331, 0.5274535600526814, 0.7616483891390923, -0.21266271046787089],
    [0.2395899573229825, -0.8495654304386868, 0.4699310925587944, -0.1730059694722726],
    [0.0, 0.0, 0.0, 1.0]
])

fk_left_value = np.array([
    [-0.8958040931940782, -0.0033102726400291316, -0.4444367994572264, -0.2568128687599656],
    [0.37610421976113056, 0.52716402323706, -0.7619998087154543, -0.21273072025408649],
    [0.23681351839497194, -0.8497571033534035, -0.47099068229194213, -0.173620614536655],
    [0.0, 0.0, 0.0, 1.0]
])
initial_position = [0 for _ in range(8)]

# fk_right_value = walker_right_arm_chain.forward_kinematics(cur_right_joints)
# print(fk_right_value.tolist())
ik_right_value = walker_right_arm_chain.inverse_kinematics_frame(fk_right_value, initial_position=initial_position)
#
# fk_left_value = walker_right_arm_chain.forward_kinematics(cur_left_joints)
# print(fk_left_value.tolist())

ik_left_value = walker_right_arm_chain.inverse_kinematics_frame(fk_left_value, initial_position=initial_position)

walker_right_arm_chain.plot(ik_right_value, ax, target=fk_right_value[:3, 3])
walker_left_arm_chain.plot(ik_left_value, ax, target=fk_left_value[:3, 3])

ax.legend()
plot.show_figure()

names = ['' for _ in range(7)]
print("ik_right_value: ", ik_right_value.tolist()[1:8])
print("ik_left_value: ", ik_left_value.tolist()[1:8])

[Dongbox]

And here my JSON file.
I only use seven joints, and that's why [1:8] exists.

{
  "elements": [
    "base_link",
    "left_limb_j1",
    "left_limb_l1",
    "left_limb_j2",
    "left_limb_l2",
    "left_limb_j3",
    "left_limb_l3",
    "left_limb_j4",
    "left_limb_l4",
    "left_limb_j5",
    "left_limb_l5",
    "left_limb_j6",
    "left_limb_l6",
    "left_limb_j7",
    "left_limb_l7",
    "left_palm_joint",
    "left_palm_link",
    "left_thumb_j1",
    "left_thumb_l1",
    "left_thumb_j2",
    "left_thumb_l2",
    "left_index_j1",
    "left_index_l1",
    "left_index_j2",
    "left_index_l2",
    "left_middle_j1",
    "left_middle_l1",
    "left_middle_j2",
    "left_middle_l2",
    "left_ring_j1",
    "left_ring_l1",
    "left_ring_j2",
    "left_ring_l2",
    "left_pinky_j1",
    "left_pinky_l1",
    "left_pinky_j2",
    "left_pinky_l2"
  ],
  "urdf_file": "walker.urdf",
  "active_links_mask": [
    false,
    true,
    true,
    true,
    true,
    true,
    true,
    true,
    false,
    false,
    false,
    false,
    false,
    false,
    false,
    false,
    false,
    false,
    false
  ],
  "last_link_vector": null,
  "name": "walker_left_arm",
  "version": "v1"
}

[Phylliade]

Thanks!
Can you also provide the URDF?

[Dongbox]

Thanks for you.
https://raw.githubusercontent.com/UBTECH-Walker/WalkerSimulationFor2020WAIC/master/walker_WAIC_18.04_v1.2_20200616/ubt_sim_ws/src/example/config/walker.urdf

[stale]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

[Phylliade]

Hello @Dongbox,

In order to get the same orientation, you need to use the "orientation_mode" parameter of the inverse_kinematics_frame function (which is exactly the same as the one documented in the inverse_kinematics function)

P.S.: sorry for the late reply!

[Dongbox]

@Phylliade Thanks, I'm not working on a robot right now. I will try this method again next month.

[stale]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.