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.
↧