Update welding demo to latest moveit version

[sjahr]

@dyackzan The servo solver is not doing exactly what it should do at the moment. It starts with a very jerky motion and loses the reference trajectory eventually. I think it might be a parameter issue. Next time I have time to continue working on this my plan was to try:

• Plot the tracked frame and the current frame to see if the diff and twist is correctly calculated
• Increase the loop frequency
• Play around with the parameters Feel free to try this or something else, if you get the chance to continue working on this earlier

[dyackzan]

I'm not sure if you got around this another way, but in order to build the demo I had to include moveit and moveit_msgs from source (the binaries had some incompatibilities). Took me a while to get things building and running on rolling, but I had some time for investigation...I plotted the error coming out of the joint trajectory controller with PlotJuggler during execution of the hybrid planning goal and recorded a video which I'll attach below. It looks like the error stays pretty low for all the joints (doesn't exceed 0.00006 radians) which definitely points to servo as the issue. We're following commands well here, but we're just not receiving the right commands.

controller_error_low.webm

[dyackzan]

Plotting the target twists using humble while running the demo yields:

And the target twists using rolling yields:

[dyackzan]

Plotting the target_pose that is created by the line:

Eigen::Isometry3d target_pose = target_state.getFrameTransform("tcp_welding_gun_link");

on humble:

on rolling:

Notes:

• the target pose values for the demo on rolling are static the entire time...
• the starting target_pose on the humble version was:

  -0.0574379   0.990606    0.124101
  0.618934     0.132868   -0.774123
  -0.78334       0.0323465 -0.620751
  0.133007    -0.226503    0.875935

• All of the target_pose values on the rolling version were:

  -0.0576455  0.990528     0.124622                                                                                                                                                                                                                                                                                                             
  0.618536    0.13342      -0.774346                                                                                                                                                                                                                                                                                                             
  -0.783639    0.0324459  -0.620368                                                                                                                                                                                                                                                                                                             
  0.131905   -0.226736     0.876283

[dyackzan]

Current position plots from the line:

Eigen::Isometry3d current_pose = current_state->getFrameTransform("tcp_welding_gun_link");

humble version:

rolling version:

[dyackzan]

On the rolling version of the demo, I've plotted the current positions, target positions, and computed diff positions for the linear x, y, and z components of the pose. As you can see in the following charts, the computed diff does not appear to be accurate

Linear X Component:

Linear Y Component:

Linear Z Component:

[dyackzan]

Found the issue with some help from @MarqRazz ! Looks like we were setting the frame in the TwistCommand to base_link when it was actually computed in the tcp_welding_gun_link frame. Switching those (in the last commit) gets things running as desired again.