MoveIt Inverse Kinematics Issue

edit

Hi everyone,

I am using MoveIt to develop applications for six-axis robot, ROS version is Melodic, IK solver is IKFast plugin.

I have two joint states they are:
joint_state1: [-0.04252107813954353, -0.24837873876094818, 0.7497109174728394, 0.0010598073713481426, 1.0707587003707886, -0.04357996582984924]
joint_state2: [-0.031201616628110537, 0.19195213717054138, 0.7344664769973024, -0.0003093520778767291, 0.64462084527768, 3.1084943068455577]

I planned a linear motion in Cartesian space, so I used FK and plan a linear motion.

However, I met an issue when I transformed the motion back to joint space. I noticed that only start joint state or end joint state in the motion fits the joint state I have.

I did two experiments,
First, I set "ik seed state" to joint_state1, then I got the result:

ik_joint_state1: [-0.042522, -0.248376, 0.74971, 0.00106106, 1.07075, -0.0435781]
ik_joint_state2: [-0.031204, 0.191954, 0.734466, -0.000310565, 0.644616, -3.17469]

In this experiment, ik_joint_state1 fits joint_state1 I have but ik_joint_state2 it did not.

Second, I set "ik seed state" to joint_state2, then I got the result:

ik_joint_state1: [-0.0425236, -0.248376, 0.749711, 0.00106229, 1.07075, 6.23961]
ik_joint_state2: [-0.031204, 0.191954, 0.734466, -0.000310565, 0.644616, 3.1085]

In this experiment, ik_joint_state2 fits joint_state2 I have but ik_joint_state1 it did not.

That's what I was talking about "only start joint state or end joint state in the motion fits the joint state I have".

I want them both fit to the joint states I have.

When I set "ik seed state" to joint_state1, I noticed that joint_6 rotated 360 degree to Negative direction, but actually I want it rotated to Positive direction to reach "3.10849".

Also, I tried to set "ik seed state" to previous joint state in the motion. For example: use IK to calculate "ik_joint_state2" with ik_joint_state1.
But the result is the same as experiment one above.

By the way, I use compute_ik this ROS service to solve IK.

Does anyone can help me solve this issue?

Thanks for viewing my post.