Issue with MoveIt! Realtime Arm Servoing when using from a clean install. Patch proposed to fix the problem.

[ETS-J-Boutin]

I did a clean install on a new laptop and encountered a behavior that did not occur on my old laptop, which I used for most of my project work. When all the scripts from the example are activated, MoveIt! sends a command of zeros between all the desired commands from Unity. This results in jerky movements, which are not intended with this method.

To patch this problem, I added a script to filter the zeros from MoveIt!, so it only sends the final command to stop the robot when multiple zeros in a row are detected, which happens when MoveIt! receives a zero as input. The script made look like this and you'll have to make it executable:

#!/usr/bin/env python3

import rospy
import numpy as np
from std_msgs.msg import Float64MultiArray

'''
This script is a patch script used with the doosan as
the Moveit! servo publishes zeros between desired command.
'''

# Global publisher to create once
pub = rospy.Publisher('/dsr_joint_velocity_controller/command', Float64MultiArray, queue_size=1)

counter = 0

def callback(msg):
    '''
    Receive the message from Moveit! and filter non-zero values unless its the last message
    '''
    global counter

    if np.count_nonzero(msg.data):
        pub.publish(msg)
        counter = 0
    else:
        counter = counter +1
        if counter >=5:
            pub.publish(msg)
            counter = 0

    rospy.loginfo(counter)

def patch_RT_command():
    '''
    Create the node to listen to Moveit! servo.
    '''

    rospy.init_node('moveit_rt_patch')
    rospy.Subscriber('/moveit/patch_command', Float64MultiArray, callback, queue_size=1)
    rospy.spin()

if __name__ == '__main__':
    try:
        patch_RT_command()
    except rospy.ROSInterruptException:
        pass

Additionally, you need to modify the vr_unity_ros_doosan/config/doosan_m1013_config.yaml file. On the line command_out_topic: /dsr_joint_velocity_controller/command, change the topic to /moveit/patch_command.

With this change, the robot rarely, if ever, jerks during movement. This resolves the unusual behavior from MoveIt!, although it is more of a patch than a complete fix for the issue.