moveit_servo won't move arm in gazebo, only oscillates

edit

Hi,

I am trying to use moveit_servo with a 4-DOF robot arm in gazebo to control it in real time using twist commands, just like in this tutorial. However, as soon as I launch spacenav_cpp.launch, the arm instantly goes back to it's initial pose (if it was moved prior the launching spacenav) and oscillates (shakes) in place. If I send it a twist command, it will keep shaking, but in the direction specified in the command.

Although it is a 4-DOF arm, I am using a custom IKFast solver which works without any problems with the motion planner so I don't see why it couldn't work with moveit_servo as well.

How to replicate the bug

1. I have put all of the necessary files in this repo which you can clone: https://github.com/JeremieBourque1/op...
2. Start MoveIt along with the gazebo simulation roslaunch open_manipulator_moveit_config demo_gazebo.launch
3. Switch to the joint_group_position_controller, which is used for moveit_servo instead of the arm_controller wich is used by the motion planner. rosservice call /controller_manager/switch_controller "start_controllers: ['joint_group_position_controller'] stop_controllers: ['arm_controller'] strictness: 0 start_asap: false timeout: 0.0"
4. Launch the servo server. If you look closely in gazebo you should se the robot shaking roslaunch open_manipulator_moveit_config spacenav_cpp.launch
   1. Publish a twist command. You should see the arm shaking in the direction specified in the twist command. rostopic pub -r 100 -s /servo_server/delta_twist_cmds geometry_msgs/TwistStamped "header: auto twist: linear: x: 0.0 y: 0.01 z: -0.01 angular: x: 0.0 y: 0.0 z: 0.0"

Additional information

• The two controller are specified in https://github.com/JeremieBourque1/op...
• The spacenav configuration file is located here: https://github.com/JeremieBourque1/op...