Interfacing the MoveIt! config package to Gazebo and run my own IK code

[Shawnnn]

In the book, CH4, section Interfacing the MoveIt! config package to Gazebo, it creates the connection between moveit, ros_control, gazebo.

I can start motion planning in rviz like in demo mode. But the weird thing is that the start state of motion planning is always stuck in initial position, which means the arm always plans from the initial straight position, though I had update the new start state to the last goal state, in order to make motion planning consistent and gazebo update simultaneously.

What's more, I try to run my own forward + inverse kinematics code to complete the plan and execution job. But the arm just completes the first motion I code( forward kinematics) in riviz, and it crashes.

[Shawnnn]

I am wondering how I can make my own fk&ik code + moveit + gazebo work together?

[qboticslabs]

Hi @Shawnnn

Here is the answer to your first question.

If there is no plan available from home position, you may need the initial position. Or you can change the home position by re-running move it wizard.

2. For integrating your IK solution to MoveIt, you have to write an IK Solver for MoveIt!.

Here is one of the tutorial to do it

Tutorial

[Shawnnn]

I m sorry, what I meant by using my own forward and Inverse kinematics code is that I just write a simple code to call moveit_commander python-moveit interface in order to move the manipulator, some functions like MoveGroupCommander(), plan(), execute() and so on.

I run my code along with demo.launch, the manipulator with fake joint controller seems work well, the motions are consistent from beginning to end. I think it means the whole motion plan is available, right?

However, when the code comes with moveit+ros_control+gazebo from CH4 (where all the configurations are well prepared, right?)
roslaunch seven_dof_arm_gazebo seven_dof_arm_bringup_moveit.launch
The manipulator is stuck in the first motion, which means it remains still after first move in gazebo, and the manipulator keeps planning and failing in moveit, however it plans from home position(not from the first move state) and the virtual planned path (shown in riviz)is absolutely not expected in my code.

Here is the part of error info

[ INFO] [1493476483.963476978, 563.672000000]: Computed Cartesian path with 19 points (followed 30.158730% of requested trajectory)
[ INFO] [1493476484.067674827, 563.767000000]: Received request to compute Cartesian path
[ INFO] [1493476484.068117741, 563.768000000]: Attempting to follow 6 waypoints for link 'grasping_frame' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[ INFO] [1493476484.132098486, 563.819000000]: Computed Cartesian path with 19 points (followed 30.158730% of requested trajectory)
[ INFO] [1493476484.236936832, 563.912000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1493476484.237124585, 563.912000000]: Planning attempt 1 of at most 5
[ INFO] [1493476484.239766430, 563.912000000]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ERROR] [1493476489.246474809, 568.526000000]: LBKPIECE1: Unable to sample any valid states for goal tree
[ INFO] [1493476489.246512559, 568.526000000]: LBKPIECE1: Created 1 (1 start + 0 goal) states in 1 cells (1 start (1 on boundary) + 0 goal (0 on boundary))
[ INFO] [1493476489.246538693, 568.526000000]: No solution found after 5.007101 seconds
[ INFO] [1493476489.261827040, 568.539000000]: Unable to solve the planning problem
[ INFO] [1493476489.261889371, 568.539000000]: Planning attempt 2 of at most 5
[ INFO] [1493476489.295049119, 568.570000000]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ERROR] [1493476494.302376820, 573.241000000]: LBKPIECE1: Unable to sample any valid states for goal tree
[ INFO] [1493476494.302415538, 573.241000000]: LBKPIECE1: Created 1 (1 start + 0 goal) states in 1 cells (1 start (1 on boundary) + 0 goal (0 on boundary))
[ INFO] [1493476494.302440671, 573.241000000]: No solution found after 5.007716 seconds
[ INFO] [1493476494.505855745, 573.436000000]: Unable to solve the planning problem
[ INFO] [1493476494.505909981, 573.436000000]: Planning attempt 3 of at most 5
[ INFO] [1493476494.507200175, 573.436000000]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ERROR] [1493476499.515737723, 578.158000000]: LBKPIECE1: Unable to sample any valid states for goal tree
[ INFO] [1493476499.515779339, 578.158000000]: LBKPIECE1: Created 1 (1 start + 0 goal) states in 1 cells (1 start (1 on boundary) + 0 goal (0 on boundary))
[ INFO] [1493476499.515801455, 578.158000000]: No solution found after 5.008862 seconds
[ INFO] [1493476499.641944337, 578.277000000]: Unable to solve the planning problem
[ INFO] [1493476499.642046851, 578.277000000]: Planning attempt 4 of at most 5
[ INFO] [1493476499.667544061, 578.295000000]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ERROR] [1493476504.672025596, 583.042000000]: LBKPIECE1: Unable to sample any valid states for goal tree
[ INFO] [1493476504.672075199, 583.042000000]: LBKPIECE1: Created 1 (1 start + 0 goal) states in 1 cells (1 start (1 on boundary) + 0 goal (0 on boundary))
[ INFO] [1493476504.672101042, 583.042000000]: No solution found after 5.004768 seconds
[ INFO] [1493476504.798828021, 583.165000000]: Unable to solve the planning problem
[ INFO] [1493476504.798886977, 583.165000000]: Planning attempt 5 of at most 5
[ INFO] [1493476504.802755914, 583.169000000]: LBKPIECE1: Starting planning with 1 states already in datastructure

---

If I only run demo.launch + my code, it can do the whole motion

Here is the successful info when running in demo mode with fake controllers

[qboticslabs]

Hi @Shawnnn

I think while sending trajectory to Gazebo, there may be collision occur in BW arm joints. If there is a collision, Gazebo controller will stop I think. In Fake execution, it will be ok. So in effect, the Arm in Gazebo will not reach the goal position.

What you can do is, if the arm is not reaching the goal position within some time, pre-empt it and try the next goal point.

Regards
Lentin Joseph

[Shawnnn]

What do you mean by BW arm joints?

In my case, arm in gazebo stop at first movement( the first movement is forward kinematics), it is corresponding to what you say, Gazebo controllers stop.

But how to explain the unusual phenomenon in MoveIt?
In the first figure, the arm stays still at that position(the first movement) in Gazebo, while in moveit, do you notice that transparent arm model? The transparent arm keeps reciprocating (while planning) from home position to that transparent pose, back and forth until planning attempt ends.