#!/usr/bin/env python
"""
author: zhouxin, inksci@qq.com

with speedj():
rostopic pub /ur_driver/URScript std_msgs/String -r 125 -- 'speedj([0, 0, 0, 0, 0, 0.5], *acc: double, t: 0.008 )'

Move the robot randomly.

"""

from multiprocessing import Process, Queue
import os, time, random

import rospy
from std_msgs.msg import String

from trajectory_msgs.msg import JointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint

import threading
import time

import numpy as np

action_ampl = 0.2 # rad/s^2

mutex = threading.Lock()

qvel_global = [0, 0, 0, 0, 0,  ... ...