Threading Error?

[Brian Zhan]

When running the attached ik_tests.cpp file on the pr2_arm.launch file, using roslaunch trac_ik_examples pr2_arm.launch, rc frequently resolves to 0, preventing the display of all the Cartesian values that are supposed to result from the given joint values. At other times, rc resolves to 1, allowing the display of given joint values.

I put the trac_ik_lib and trac_ik_tests file in the src folder. To successfully build, I also had to modify trac_ik/build/CMakeFiles/2.8.12.2$/CMakeCXXCompiler.cmake to include set(CMAKE_CXX_FLAGS "--std=c++11").

[Patrick Beeson]

1) The reason you need set(CMAKE_CXX_FLAGS "--std=c++11") is due to your own additions of dq_epsilon. It has nothing to do with my code.

2) You are calculating the desired Cartesian end pose by seeding vector q with only 6 joints but the pr2 chain you are requesting is actually 7 joints, so the final angle is undefined. This unassigned joint could be a random number not within the joint limits, thus you are calculating an end effector pose that is truly unreachable. So IK should fail rc < 0 when the end effector truly is unreachable.

You could test for this by making sure for each joint i: ll(i) <= q(i) <= ul(i)

3) You are also seeding the IK search with a completely undefined nominal vector. Generally, nominal is something like nominal(i)=(ll(i)+ul(i))/2 or some other "home" state. Right now, it's undefined so is likely starting the IK solver is some really weird configuration that cannot converge to a solution in to 5ms you have requested. You need to seed the solver with something reasonable. Either the last known joint values or a (defined) nominal configuration.

[Patrick Beeson]

Actually your second joint value -0.5659 is not within the joint limits [-0.3536, 1.2963] for that joint. So you are indeed calculating and end effector pose that is unreachable by this robot. So IK should indeed fail here.

[Brian Zhan]

I see, thank you!