ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

UR5+velocity control: runaway robot after getting closer to goal pose

asked 2019-06-10 00:33:40 -0600

Rik1234 gravatar image

updated 2019-06-11 07:29:06 -0600

In my algorithm, the robot seemingly moves towards the right position, as the joint velocities reduce, after that the velocities start increasing as the robot moves away. I cannot understand why this is happening and need some urgent help. My github project link is given below: The object recognition and everything else is working as expected. Also the robot does not stop when I stop the code, I've checked rostopic echo and joint velocities are still being published. Maybe the two problems are related. Please help.

The codes are all mine.


This is a trial velocity publisher I've written just to test the lag issue

#!/usr/bin/env python
import rospy
from ur5_control_nodes.msg import floatList

import rospy
from std_msgs.msg import String

def talker():
    pub = rospy.Publisher('ur5_joint_velocities', floatList, queue_size=10)
    rospy.init_node('talker', anonymous=True)

    while not rospy.is_shutdown():


if __name__ == '__main__':

Code for color detection

#!/usr/bin/env python
import rospy
import numpy as np
import cv2
from sensor_msgs.msg import Image
from ur5_control_nodes.msg import floatList
from cv_bridge import CvBridge, CvBridgeError
import imutils


#RGB Image Callback
def rgb_callback(rgb_msg):
    global rgb_img   
    rgb_img=bridge.imgmsg_to_cv2(rgb_msg, "bgr8")     

#Depth Image Callback 
def d_callback(msg):

    global depth_img
    depth_img=bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough")

#Getting XY Points
def xy_points(frame):

    xypoints=np.array([0,0,0,0,0,0], dtype=np.int64)

    blurred = cv2.GaussianBlur(frame, (11, 11), 0)

    hsv=cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)

    #defining the Range of Blue color

    blue = cv2.erode(blue, None, iterations=2)
    blue = cv2.dilate(blue, None, iterations=2)

    cnts_b = cv2.findContours(blue.copy(), cv2.RETR_EXTERNAL,
    cnts_b = imutils.grab_contours(cnts_b)
    center_b = None
    if len(cnts_b) > 0:
        c_b = max(cnts_b, key=cv2.contourArea)
        # epsilon = 0.1*cv2.arcLength(c,True)
        # approx = cv2.approxPolyDP(c,epsilon,True)
        M_b= cv2.moments(c_b)
        center_b= (int(M_b["m10"] / M_b["m00"]), int(M_b["m01"] / M_b["m00"])), center_b, 5, (255, 255,255), -1)
    low_r = np.array([140,150,0],np.uint8)
    high_r = np.array([180,255,255],np.uint8)

    red = cv2.erode(red, None, iterations=2)
    red = cv2.dilate(red, None, iterations=2)

    cnts_r = cv2.findContours(red.copy(), cv2.RETR_EXTERNAL,
    cnts_r = imutils.grab_contours(cnts_r)
    center_r = None
    if len(cnts_r) > 0:
        c_r = max(cnts_r, key=cv2.contourArea)
        # epsilon = 0.1*cv2.arcLength(c,True)
        # approx = cv2.approxPolyDP(c,epsilon,True)
        M_r= cv2.moments(c_r)
        center_r= (int(M_r["m10"] / M_r["m00"]), int(M_r["m01"] / M_r["m00"])), center_r, 5, (255, 255,255), -1)

    low_g = np ...
edit retag flag offensive close merge delete


Two comments:

  1. please include all relevant code in your question. If (or more: when) your repository gets deleted, this question will lose all its value
  2. depending on the code, this is most likely not related to the fact that you're using a UR5 at all. I've updated the title of your question and left the 'UR5' reference in there for now.
gvdhoorn gravatar image gvdhoorn  ( 2019-06-10 01:57:45 -0600 )edit

Thank you for your reply, I've updated the question to include all my codes.

Rik1234 gravatar image Rik1234  ( 2019-06-10 03:59:43 -0600 )edit

I've reformatted it a bit, but there are still many, many empty lines. Please remove those.

gvdhoorn gravatar image gvdhoorn  ( 2019-06-10 06:36:02 -0600 )edit

There are quite a few things that could be improved here:

  1. don't create Publishers or Subscribers inside callbacks or while loops. Create them in the initialisation phase of your program and keep them around. Then reference them when you need them.
  2. I don't understand why you have rate.sleep() in various locations in your code. Sleeping at random places doesn't make loops or callbacks run at a specific frequency, it just introduces a delay.
  3. why are you using the joint_velocity topic? Are you using the master branch of the ur_modern_driver? If so, please switch to kinetic-devel and start using the JointGroupVelocityController
  4. overall the code is quite messy and hard to read. You may want to structure it a bit better
  5. topic names that start with a digit (ie: 3_point_features) are illegal.
gvdhoorn gravatar image gvdhoorn  ( 2019-06-10 06:41:26 -0600 )edit

Thanks for your reply!

  1. I thought the infinite loop for callbacks could be used for publishing as well. I will rectify that.

  2. I thought we always had to specify the rate of publishing? I had a misconception there, most of the ros codes had it, so I assumed it was necessary. I have now changed it and the delay is a lot less. Not gone completely, but a lot lesser.

  3. Yes I am using the ur_modern_driver package. I will look into that package at my earliest.

  4. I am really sorry for this, I understand that the code has been very messy and I did plan to clean it up once I had the basic program running.

  5. I've seen this...I wasn't really bothered about it because it was just a warning. I'll change that.

I'll update the post once I've worked on that package.

Rik1234 gravatar image Rik1234  ( 2019-06-10 07:24:03 -0600 )edit

Okay, I cannot figure out how to implement ros_control to replace my ur_modern_driver package. Can you help or guide me in any way? I've gone through the documentations, most of it as well as the package I'm using is in CPP and i'm not very good at cpp. Can I email you, if you wouldn't mind?

Rik1234 gravatar image Rik1234  ( 2019-06-10 13:06:12 -0600 )edit

Okay, I cannot figure out how to implement ros_control to replace my ur_modern_driver package.

That's not what you should do.

Start ur_modern_driver in ros_control mode (with the appropriate .launch file) and start publishing messages to the command topic of the JointGroupVelocityController after you've made joint_group_vel_controller the active controller.

Can I email you, if you wouldn't mind?

No, let's keep this on ROS Answers.

gvdhoorn gravatar image gvdhoorn  ( 2019-06-11 02:32:59 -0600 )edit

okay so the launch shell code goes something like this

roslaunch ur_modern_driver ur5_bringup.launch robot_ip:=(robot_ip)

I've edited the following ur5_ros_control.launch code, made the changes in the yaml file, but i cannot see any topic named command in the rostopic list. I have edited the question to include the yaml and the launch file.

Rik1234 gravatar image Rik1234  ( 2019-06-11 03:38:16 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2019-06-11 09:32:18 -0600

gvdhoorn gravatar image

updated 2019-06-14 03:00:45 -0600

Reducing the situation to just looking at the lag reported by the OP, he implemented a MWE that just publishes a simple velocity to the command topic.

In the MWE we see:

  while not rospy.is_shutdown():

this publishes the same velocity at about the maximum rate your computer is able to achieve -- which for typical PCs these days could be 10000 Hz or faster with these small messages.

@Rik1234: you may want to add a sleep in there to moderate the rate at which this loop is executed. There is no point in publishing any faster than the maximum of the URs control loop, which, if you have a non-e-series controller, would be 125 Hz.

edit flag offensive delete link more


Yes, that was it. Now there is no lag between the robot. However, solving one problem leads to another, and for some reason, my jacobian isn't working anymore. I loaded the previous controller launch file and the jacobian worked differently in both the controllers.I checked the order of the joints in the yaml file, there is no change there. Why is this happening? Even if I can run the code using the old launch, I would like to know the issue behind this. Also, the operations on both the controllers are extremely jerky. Is there a problem with my visual servoing control algorithm?

Rik1234 gravatar image Rik1234  ( 2019-06-11 15:22:18 -0600 )edit

Maybe this will be a separate question. I'll ask in a new thread.

Rik1234 gravatar image Rik1234  ( 2019-06-12 23:51:12 -0600 )edit

I'm not sure why you posted a new question.

Afaik, both the JointGroupVelocityController and the old joint_speed topic use the exact same interface to the robot controller.

Again, as with your initial problem, reduce everything to the absolute minimum and start debugging from there.

gvdhoorn gravatar image gvdhoorn  ( 2019-06-13 02:14:14 -0600 )edit

I did try that. the joint velocities as well as the end effector velocities are different in both the controllers. I asked in a separate question because the original delay problem was solved, and this was a different issue. I was hoping if someone could take a closer look at the code.

Rik1234 gravatar image Rik1234  ( 2019-06-13 23:38:38 -0600 )edit

Just to give this thread some closure, I've restructured the comments into an answer.

Could you accept the answer if you feel we've sufficiently dealt with the lag issue?

gvdhoorn gravatar image gvdhoorn  ( 2019-06-14 03:02:13 -0600 )edit

Sure. I'll do that. Thanks for your help.

Rik1234 gravatar image Rik1234  ( 2019-06-15 04:18:16 -0600 )edit

Question Tools



Asked: 2019-06-10 00:33:40 -0600

Seen: 426 times

Last updated: Jun 14 '19