using ur_driver/joint_speed to do real-time trajectory tracking, but joints don't move as what I publish. There are some zero points.

[happygaoxiao]

@ThomasTimm hello, thanks for your driver. I am doing trajectory tracking for my research. The UR5 is controlled with ur_driver/joint_speed published at 125Hz. The control structure is as below

The J means Jacobian matrix of current joint array. d_theta = inverse(J)*d_x, joint_speed = d_theta/0.008;

The tracking result is as below

The solid lines are curves of reference end position, and dotted lines are actual position calculated by forward kinematics.

The joint_states are subscribed and recorded to a data file. There is a comparison for actual speed and published speed of sixth joint :

I have also designed a PI controller at joint space to tracking joint array which is calculated as the inverse kinematic of reference trajectory point each 8ms. The input of the PI controller is joint error and the output of controller is joint speed, through a lowpass filter and the joint speed is published as 125Hz.
The joint speed is often out of limit and UR5 is jerking in Rviz and real robot. I want to smooth the joint speed by the filter but it has little improvement in the control loop.

So finally, here are my questions. Hope someone could give me some advice. Thanks very much.

1. How could I get a nice trajectory tracking result？ Should I design controller in Cartesian space or joint space? I have tried two ways but both don't get a good tracking effect. I am using ur_driver/joint_speed to control the UR5. Maybe using servoj of ros_control is better? Because I don't know how to start learning servoj of ros_control. Is there any advice?

2. In the joint speed figure, there are many zero points of speed. How can I make the UR move the same as the published speed ? This may be the reason why robot is jerking sometimes . I don't know where is wrong.

[HTugal]

A quick comment;

You are calculating Cartesian velocity (Cvel(k) ) buy using one step ahead Cartesian position, reference, ( Cpos(k+1)) with (Cpos(k+1) - Cpos(k))/Deltat. I presume the Jacobean matrix is not a linear operator, so you should use Deltat before implementing the inverse Jacb.

[happygaoxiao]

@HTugal Thanks for you response. We can consider the Jacobian matrix as constant in a small time period, as 8ms here. " d_theta = inverse(J)*d_x " is an approximate calculation. Using delta_time before or after the inverse Jacb is the same. As there is a control loop, the small error can be reduced.

[happygaoxiao]

I still have the problem of "zero speed". The real joint speed doesn't match the published joint speed with ur_driver/joint_speed at 125Hz. Here is the result of the sixth joint comparison of speed
(1) ur-sim

(2) real robot of UR5

The real speed is recorded by the joint_states topic. I don't know what is wrong. Maybe the published joint speed isn't smooth enough ?

Could you give me some help? @ThomasTimm

[gavanderhoorn]

@happygaoxiao: can you describe the network setup you're using? Is there any wireless segment between you and the controller?

Additionally: which controller version is this? And which Polyscope version?

[HTugal]

I have similar problem with ur3 #137, @gavanderhoorn I have tested with a network switch where all the robots and PCs connected to it with a fixed IP, nothing changed. But some sort of connection loss happening as robot's status transiently turns into 'stopped' from 'running', this is the when joint's velocity became zero.

Meanwhile, Polyscope version was 3.3.3.292 (and above in some tests).

[ThomasTimm]

Hmmm, this is quite strange. I've seen cases where i needed a dedicated network card due to many packet losses on a busy network, but somehow I don't think this has something to do with the network.

The reason being that you are using velocity control, and that should work in a way where the robot moves at the commanded velocity until it gets another command. You can see this by sending a single velocity command to the robot, and the commanded joint should just keep moving until you stop it (the driver has a safety feature that should stop the robot after 300 samples/2.4 sec of no new commands)

UR has changed the behavior of jspeed some times though, as you can see at https://github.com/ThomasTimm/ur_modern_driver/blob/9397f47ac158fe5976719f33101334371bc15d44/src/ur_realtime_communication.cpp#L103 - you can try and change the 0.008 value to 0.016 or 0.02 and see if that helps anything.

But that still doesn't explain why the driver sometimes publishes a velocity of 0.0. I'm assuming that when you say "real velocity", you are talking about the velocity of the arm published in ROS, and not the actual physical movement of the arm? How is the arm actually moving?

Could you do a data trace in wireshark or print something out whenever data is received so see if the actual robot is really publishing 0 velocity, or where the value of 0 originates from.

[HTugal]

@ThomasTimm, I tested speedj time value before with ur3 to solve #137 unfortunately nothing changed with respect to this issue (I presume it is the same issue that @happygaoxiao revealed here). Here is some data plots where a sinusoidal signal is published in /ur_driver/joint_speed and the arm sensor values subscribed from /joint_states have been recorded. Namely, Act. is /joint_states and Ref. is /ur_driver/joint_speed .

Fig1: with 0.008

Fig2: with 0.008 as well, nothing changed from the previous experiment. Just to illustrate randomness of the behaviour.

Fig3: with 0.016, in a similar manner I can show worse performance graphs by just running another experiment with 0.016.

Fig4. with 0.02, as expected there are more velocity shows more oscillatory behaviour, yet jerky movement is still alive.

How is the arm actually moving?
It actually stops/breaks in a really short time of period.

[gavanderhoorn]

I wouldn't really be able to explain it (as /ur_driver/joint_speed is essentially pass-through, no control) , but perhaps the change suggested in #140 improves things.

@HTugal: I would recommend you verify that after applying #140 state reporting still works as normal, and only then try commanding the robot.

[happygaoxiao]

@gavanderhoorn My real robot of UR5 shows PolyScope 3.4.0.265, URControl 3.4.0.285(Mar 08 2017). The UR-sim simulator is PolyScope 3.4.4.412, URControl 3.4.4.438(Sep 14 2017) from UR official website .
For the real robot, My laptop and UR5 robot are in a local area network with network cables, no wireless segment.
@ThomasTimm The "real speed" in my figure is recorded from ROS topic joint_states. real_speed = state.velocity, and I record this. I will learn how to use a data trace in wireshark.

Maybe the zero points are due to some time inconsistency？ I am publishing joint speed in below way to track a time trajectory sequence. Actual I am a newcomer for ROS and C++.

Move_to_first_point();    // move to the starting point of the trajectory
ros::Rate loop_rate(125);
while (ros::ok() & end_trajectory_state == false) {
    ros::spinOnce();  // update joint states from joint_states topic
   // get current time, interpolate trajectory to get target Position and Pose
   ...
   //calculate target joint speed
   ...
   trajectory_msgs::JointTrajectory traj;
   trajectory_msgs::JointTrajectoryPoint pt;
   for (int x = 0; x < 6; ++x) {
         pt.velocities.push_back(vel(x));
    }
     traj.points.push_back(pt);
     pub_speed_.publish(traj);      // publish joint speed to ur_driver/joint_speed topic
     record_jnt_vel(time_now,vel,current_jnt_vel,current_jnt_arr); // record velocity to date file
     loop_rate.sleep();
}

I have found the time interval of time_now is not exactly 8ms. Maybe sometimes the ur_driver/joint_speed topic doesn't receive data, so joint_states topic published zero speed.
In this way, I have achieved 0.4mm trajectory-tracking error with my own position closed-loop , but sometimes the robot is shaking，which is too bad for my next work of hybrid force/position control. The speed of joint_states doesn't match the speed I publish to ur_driver/joint_speed topic with too many strange fluctuation and noise. Maybe what I have done is wrong. Any suggestions?

Now I am learning how to using ros_control of /vel_based_pos_traj_controller. Seems to be same thing of speedj . I creat a client named "vel_based_pos_traj_controller/follow_joint_trajectory". Using sendGoal(goal) with 125Hz loop to track a joint sin curve of 0.1Hz. The result is too bad to track the sin curve. Amplitude couldn't be reached. Sometimes the ur-sim simulator is stopped by protection. I don't know why. Could you please give me some advice about how to use the speedj?

[ghost]

I did some experiments using speedj with UR5 (as attached), I hope it is useful.
https://goo.gl/r2xstk
(I used ROS, python script, ready for details)

[UyaSong]

@sarahamdan Hi,
Is there some docs or tutorials on how to use speedj in python? I find that the relevant information is a bit less.

[gavanderhoorn]

I'm closing this as we've officially deprecated this package.

Refer to the announcement on ROS Discourse.