Threading Error?
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").
Comments (3)
-
Account Deactivated -
Account Deactivated - changed status to invalid
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.
-
reporter I see, thank you!
- Log in to comment
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.