Servoj URScript (abrupt final motion)

[RobotNoob]

Hi everyone,
I am using a servoj command to operate the robot, to move from point A to point B.
I am not 100% sure I am using the command correctly, because at the end of the motion, robot doesn’t slow down noticeable and it stops abruptly as can be seen on the video (UR5e Servoj issue - YouTube)

I have done a trajectory planning with an initial and final velocity/ acceleration to be zero (my conditions). Even though the graphs I received show me that the velocity and acceleration behave in the desired manner, it doesn’t seem so from the video I attached.

As you can see from the attached code, I run servoj in the loop and update the desired position as each time step.
Any suggestions for where the issue comes from?

HOST = '192.168.1.103'  # UR IP address
PORT_30003 = 30003

print("Starting Program")

count = 0
home_status = 0
program_run = 0

s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.settimeout(10)
s.connect((HOST, PORT_30003))
time.sleep(1.00)
print("")

# starting position
pos_start = [-0.165, -0.501, 0.234, -0.327, -3.109, 0.065]
pos_target = [0.37, -0.405, 0.099, 1.241, -2.871, -0.196]
# s.send((f"movel(p{pos_start}, 0.1, 0.2)" + "\n").encode())

# pos_start[3:] = trans.AxisAngle2Euler(pos_start[3], pos_start[4], pos_start[5])
# pos_target[3:] = trans.AxisAngle2Euler(pos_target[3], pos_target[4], pos_target[5])

t_total = 4
planner = traj.path_plan(pos_start, pos_target, t_total)
t_step = 0.002
t = 0
time_range = []
posx = []
posy = []
posz = []
vx = []
vy = []
vz = []
ax = []
ay = []
az = []

while t <= t_total:
    t += t_step
    [position, orientation, velocity, ang_vel, acceleration, ang_acc, jerk] = planner.trajectory_planning(t)
    next_position = [position[0], position[1], position[2], orientation[0], orientation[1], orientation[2]]
    s.send((f"servoj(get_inverse_kin(p{next_position}), 0, 0, {t}, {t_step}, 150)" + "\n").encode())
    print(velocity)

[RobotNoob]

If I operate at 125Hz then my time step would be 8ms. I am confused about the meaning of t and lookahead_time.

From my understanding:
t is the current time at which the servoj is evaluated
lookahead_time is a time step (for the next predicted servoj position)

Is it right> ?

[RobotNoob]

Would anyone be able to help me out with this issue please? @ajp @Ebbe
Any help would be appreciated, I am stuck with this issue for a few days now

[Ebbe]

Have youbtried with the default servoj settings?

[RobotNoob]

Yes, I did but the behavior is not as intended in the trajectory planning
The point that confuses me is, when i write a loop to update the joint values according to the trajectory planning,i am running the loop at some time step, and evaluating the trajectory at some time t.
In servoj what is the meaning of t and lookahead_time? Can one be treated as a time at a current stage and lookeahed_time as a value of timestep? i.e. in the case of RTDE 0.002[sec]?

[Ebbe]

No, for the servej function lookahead_time and gain is affecting how aggressive the robot is chasing the target. The default value should not be way of for your use.
You shall use your step time as t and make sure you are giving the new target when the last call is returning. If you are having issue it can help you running it local on the robot, so you are sure you are not struggling with a network delay.

[ajp]

There’s nothing setting the timing of your s.send command in the python while loop is there? The t argument in the URScript command doesn’t help here. You’re probably sending considerably more than one command in 2ms (or 8ms) so the system will skip the intermediate ones and just execute the last one it received when it comes to start the next cycle… Would be much easier to control the timing if you used the RTDE.

[RobotNoob]

@ajp
If I want to use servoj in RTDE, then I still need to input t, lookahead_time and gain.
If I can generate the path at each instance t_now, than whats the proper usage of the servoj command? I do understand the point you are making but I am still unsure how to implement the solution.

What I was thinking to do in python is the following:

t_init = time.time()

while time.time() - t_init < t_total:
   
    current_pose = robot.get_actual_tcp_pose()
    current_joint = robot.get_actual_joint_positions()

    # Trajectory planning
    [position, orientation, velocity, ang_vel, acceleration, ang_acc, jerk] = planner.trajectory_planning(t)
    next_pose = [position[0], position[1], position[2], orientation[0], orientation[1], orientation[2]]
    
    #   Inverse Kinematics
    init_matrix = kin.create_transform_matrix(current_pose)
    fin_matrix = kin.create_transform_matrix(next_pose)
    inverse = kin.inv_kin(init_matrix, fin_matrix, 'R', 'U', 'D')

    t = time.time() - t_intial
    robot.servoj(inverse, t)
    robot.sync()

As you can see the servoj command is executed in real-time thanks to the usage of time.time() function in python. I will leave the lookahead_time and gain to the default values.

But it still doesnt behave in the desired manner

[RobotNoob]

@ajp

There’s nothing setting the timing of your s.send command in the python while loop is there?

Not sure what are you trying to say here^

Its just hard for me to understand how to use the servoj command. It doesnt work for me to just simply write.
s.send(''servoj(target_joint)"). It has to be placed inside a loop where the values of q are updated.

[ajp]

Are you using a 3rd party UR python library in this last example? I’m not familiar with the robot. object. Perhaps this will help to synchronize your communications for you.

I’ve just looked back at your original example too though, and I see you’re incrementing the t parameter. That’s not how it should be. This determines how long the robot will take to execute this one step. This should be constant throughout your whole movement. I’d recommend just not setting it at first and letting the system use the default of 2ms for e-Series. In fact you can leave out everything except the q joint angles as Ebbe suggested for initial trials.

Perhaps this clarification will help to understand what I mean in my last message when I say nothing is setting the timing. Lets say python is sending these servoj commands over the socket to the controller at 5000Hz, but the robot is only executing them at 500Hz… 9 out of 10 will be thrown away by the controller. You could have something trying to make your python code only send them at 500Hz, but that’s also a bit tricky to synchronise.

What I’d recommend is running a loop on your robot (either written in polyscope or sent over client interface) that just takes values from 6x input_double_register_X (one for each joint) and passes them into servoj. Then your python program passes your intended q targets into those registers that the robot program is reading. The python RTDE library will sync with the robot and pass these values at the intended frequency to ensure one is executed per cycle.

Does that make sense? I did have a simple example once upon a time, but not sure if I can find it now.

[RobotNoob]

@ajp
First of all thank you very much for all the help, your replies help me a lot move forward with my project.

1. Robot.command is an object that comes from the following source, which allows for RTDE communication with the robot using python
   Python 3 library to communicate with robot
2. I understand now what you meant.
   Just to make sure. When I run my trajectory planning code, a set of 6 joint values is generated (different at each time t). I should run a loop that passes the output of my trajectory planning into servoj allowing it to follow that motion. That’s very similar to what I did but you are not using time increments instead you are saying to pass it to the input_double_registers and only trough them to the command servoj (which will assure that values are not skipped)

How can I create a loop condition? Before I had it to stop after some predefined time was reached, but how can I write the execution loop when using RTDE? (what will be the stopping condition?)

If you have any examples that would be priceless, if not I will manage

[ajp]

Well if your target joint angles don’t update from the current position your robot shouldn’t move, even if servoj is still running.

Alternatively you could stop your loop by sending a halt() command to one of the client interfaces, or even use another boolean register as an enabling bit, set it low when you’ve finished streaming trajectories and stop the loop on that, or just wait for it to go high again before you start reading the joint angle registers again?

I presume you’re not using moveit to calculate your trajectories? If you are it would be a lot easier to use our ROS driver?

[RobotNoob]

Great Thank you for your help, I will try it out.

I am calculating trajectories using another piece of code I wrote, not moveit

[lucas.schneider]

Is that a problem if i use a path with waypoints that have same distance (like 10mm) and change the “t” to get different speed/acceleration in each part of the patch?

[ajp]

That should work as long as the speed/acc variations between each section are reasonably smooth. I’d probably recommend splitting it into smaller say 1mm sections and ramping the values across those instead of sudden change at the end of a 10mm section.

[lucas.schneider]

Nice!!!

Is that ok stop the servoJ patch with stopj(1) ?

[ajp]

Yes, that’s the correct way to do it.

[erikkockar]

Hi,

I would like to ask question about servoJ. Here in this case RobotNoob used it real-time with time library for python. My question how to servoJ robot if I have pre-recorded poses with time-stamp. I computed inverse kinematics so now I have array of time stamps (in ms) and joint positions at each time stamp (each 5-6 ms I receive pose)

I have tried something like this, but its completely wrong.

for i in invKinematics:
    rtde_c.initPeriod()
    rtde_c.servoJ(i,0.2,0.2,0.006,0.05,300)
    rtde_c.waitPeriod(0.006)

Do you have some suggestions @ajp @Ebbe ?

[friend]

Seconded. Would like to know this as well.

[ajp]

Hi @erikkockar,

I assume you are using the SDU ur_rtde library for this? I’m not as familiar with that as with our standard RTDE python interface (Real-Time Data Exchange (RTDE) Guide - 22229).

Your code looks similar to the servoj example on the below page so it should work. Does that sample work unmodified?
https://sdurobotics.gitlab.io/ur_rtde/examples/examples.html?highlight=servoj

When you say completely wrong? What is actually happening?