4 IKFast plugins with MoveIt!
I have created 4 separate IKFast plugins (in different packages) for a quadruped robot and created plugins for MoveIt!.
In order to test each IKFast plugin individually, I kept kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin, except for the planning group I was testing, in which kinematics_solver: leg_XX_kinematics/IKFastKinematicsPlugin. I then launched demo.launch and set a position target I knew was reachable through a python node running MoveGroupCommander.
For the next test, I reset kinematics_solver: leg_XX_kinematics/IKFastKinematicsPlugin back to kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin, and changed the new planning group to kinematics_solver: leg_XX_kinematics/IKFastKinematicsPlugin.
For each leg (XX = RR, RF, LR, or LF), the plan was generated and successfully executed.
Now I tried running all 4 IKFast plugins together by changing them all to kinematics_solver: leg_XX_kinematics/IKFastKinematicsPlugin. However, only the first planning group in the kinematics.yaml file was able to plan to the position target tested above. The other three planning groups failed. Below is the output for the first two planning groups in the kinematics.yaml file. The first succeeded, but the other one failed (same as the last two in the kinematics.yaml file). Appears to be different when it reaches the line in which it is sampling valid states for goal tree.
[ INFO] [1478811497.752041786]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1478811497.752709217]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1478811497.775881219]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1478811497.776011916]: Solution found in 0.023363 seconds
[ INFO] [1478811497.776170580]: SimpleSetup: Path simplification took 0.000057 seconds and changed from 4 to 2 states
[ INFO] [1478811497.780874798]: Received new trajectory execution service request...
[ INFO] [1478811497.781173505]: Fake execution of trajectory
[ INFO] [1478811499.581798402]: Execution completed: SUCCEEDED
[ INFO] [1478811509.153249036]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1478811509.153772008]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1478811509.174940381]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1478811509.174985629]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1478811509.175004149]: No solution found after 0.021306 seconds
[ INFO] [1478811509.175031377]: Unable to solve the planning problem
Has anyone else run into this problem, or know of a moveit_config package that uses more than one IKFAST plugin?
Another option if I cannot get it to work is running IKFast in my python node (if possible), then passing the IKFast joint angles to group.set_joint_value_target(). Is it possible to run IKFast in a python node for your robot, similar to pykdl_utils KDLkinematics?
Any help would be appreciated.
