Gitter / Delay encountered while sending multiple points via the 'position_task'

[ShubhamNandi]

Trying to send consecutive positions via the 'position_task' but there is gitter/delay while going from one point to the other
Here is a Sample Code for further clarification

from edcon.edrive.com_modbus import ComModbus
from edcon.edrive.motion_handler import MotionHandler

import time

POS_MULTIPLIER = 0.000001  # 10^6
VEL_MULTIPLIER = 0.001  # 10^3

motor1, motor2, motor3 = None, None, None

def scale_up_pos(position):
    return int(position * (1 / POS_MULTIPLIER))

def scale_up_vel(velocity):
    if abs(velocity) < 0.001 :
        return 0
    return int(velocity * (1 / VEL_MULTIPLIER))

def scale_down_pos(position):
    return position * POS_MULTIPLIER

def scale_down_vel(velocity):
    return velocity * VEL_MULTIPLIER

def connect_to_festo_drivers():
    global motor1, motor2, motor3

    # Motor 1 - motor1
    com1 = ComModbus("192.168.10.12", timeout_ms=2000)
    motor1 = MotionHandler(com1)
    while not motor1.acknowledge_faults():
       print("Waiting to clear faults for Motor 1")
    while not motor1.enable_powerstage():
       print("Enabling power stage for Motor 1")

    # Motor 2 - motor2
    com2 = ComModbus("192.168.10.11", timeout_ms=2000)
    motor2 = MotionHandler(com2)
    while not motor2.acknowledge_faults():
       print("Waiting to clear faults for Motor 2")
    while not motor2.enable_powerstage():
       print("Enabling power stage for Motor 2")

    # Motor 3 - motor3
    com3 = ComModbus("192.168.10.10", timeout_ms=2000)
    motor3 = MotionHandler(com3)
    while not motor3.acknowledge_faults():
       print("Waiting to clear faults for Motor 3")
    while not motor3.enable_powerstage():
       print("Enabling power stage for Motor 3")
    print("All motors connected and enabled")

    motor1.configure_continuous_update(active=True)
    motor2.configure_continuous_update(active=True)
    motor3.configure_continuous_update(active=True)

# 4  Positions
pos_values = [[0, 0, 0],
              [0.3, -0.8, 0.42],
              [0.15, -0.4, 0.23]
              ]  

vel_values = [[0.07, 0.2, 0.2],
              [0.07, 0.2, 0.2],
              [0.07, 0.2, 0.2]
              ]  

def motor_set_values(positions, velocities):
    motor1.position_task(scale_up_pos(positions[0]), scale_up_vel(velocities[0]),absolute=True, nonblocking=True)
    motor2.position_task(scale_up_pos(positions[1]), scale_up_vel(velocities[1]),absolute=True, nonblocking=True)
    motor3.position_task(scale_up_pos(positions[2]), scale_up_vel(velocities[2]),absolute=True, nonblocking=True)

def wait_for_completion():
        while True:
            if motor1.target_position_reached() and motor2.target_position_reached() and motor3.target_position_reached():
                break

def handle_motor_command():
    i = 0
    while i <= len(pos_values):
        if i == len(pos_values):
            i = 0
        motor_set_values(pos_values[i], vel_values[i])
        wait_for_completion()
        # time.sleep(4)
        i += 1

if __name__ == '__main__':
    connect_to_festo_drivers()
    print("Drivers Connected!")
    handle_motor_command()

[elrosch]

Hello,

thank you for the example. I would need a little more information on the following:

• where exactly you encounter a delay
• how much delay are we talking about
• what is the resulting effect in your application

Depending on these information the delay might be expected. Here are some reasons:

• Your machine and it's operating system respectively have execution delays due to context switches etc. (on slow machines this can take up to several milliseconds of jitter)
• The position_tasks are started sequentially (there might be the possibility to add a prepare step to the library to reduce the latencies produced due to the preparation of the telegrams)
• The cycle time could be reduced (this is an optional argument for the ComModbus constructor)
• You could try using EtherNet/IP (may be faster, especially via X19 interface)

Probably there are more options..

[aaryanmurgunde]

Hi @EvilEli

THE SCENARIO: We are trying to actuate 3 motors driven by 3 Indvidual festo drivers for a robotic arm. we have a ROS system that is generating a set of points with velocity (trajectory) for the movement of the arm.

Observations: so far we have observed that the api function position_task() when send a point to execute to the driver, the driver internally accelerates and then decelerates per point. Since this is happening per point, after one point the motor tends to go to zero velocity and then for the next point it accelerates back. We have concluded that this is the reason why the jitter is visible.

What we are trying to achieve is that we want to update point on the fly i.e. send continuous points so that the trajectory is executed properly.

Any inputs on this would be of help

Thank you :)

[elrosch]

Given this information I assume the problem lays in the wait_for_completion() part.

By issuing a position task the drive calculates the complete trajectory from the starting point to the ending point (with resulting speed of zero) using the parametrized acc/dec and velocity limits. That means the wait_for_completion() task will return as soon as the drives have all reached (or almost reached) velocity zero. However, it is possible to "update" ongoing position tasks when using configure_continuous_update(True) which you are already doing. You should just be able to overwrite with a new position_task. I guess you'll need to skip the wait_for_completion() part and determine the point when to issue the next position task in a different manner.

[aaryanmurgunde]

@EvilEli I was under the same assumption that using configure_continuous_update(True) should do the job. Below is an example for the same, but still the jitter exists also after checking the velocity curve from the moto, it is seen that the motor tends to zero after every point.

def motor_set_values(positions, velocities):
    motor1.position_task(scale_up_pos(positions[0]), scale_up_vel(velocities[0]),absolute=True, nonblocking=True)
    motor2.position_task(scale_up_pos(positions[1]), scale_up_vel(velocities[1]),absolute=True, nonblocking=True)
    motor3.position_task(scale_up_pos(positions[2]), scale_up_vel(velocities[2]),absolute=True, nonblocking=True)

# Callback for the trajectory points
def trajectory_callback(data): 

    num_traj_points = len(data.result.planned_trajectory.joint_trajectory.points)
    print("Number of points in trajectory: ", num_traj_points)

    if num_traj_points == 0:
        print("No points in trajectory")
        return

    for i in range(num_traj_points):
        if i == 0:
            continue
        if i == num_traj_points - 1:
            break
        print("Point ", i, " : ", data.result.planned_trajectory.joint_trajectory.points[i].positions)
        print("Velocities: ", data.result.planned_trajectory.joint_trajectory.points[i].velocities)
        motor_set_values(data.result.planned_trajectory.joint_trajectory.points[i].positions, data.result.planned_trajectory.joint_trajectory.points[i].velocities)
        sleep_time = data.result.planned_trajectory.joint_trajectory.points[i+1].time_from_start - data.result.planned_trajectory.joint_trajectory.points[i].time_from_start
        rospy.sleep(sleep_time.to_sec() * 0.5 )
        print("Point ", i, " reached")

    print("#"*50)
    print("Trajectory completed")
    print("#"*50)

This code is running based on time and trying to update the points on the fly but still no visible changes observed

[elrosch]

Just for me to clarify: Does the motor velocity tend to zero before or after a new position_task has been started?

[aaryanmurgunde]

Just for me to clarify: Does the motor velocity tend to zero before or after a new position_task has been started?

Before the new position like it ends the previous position with vel tending to zero then gets it up once the new position task is received

[elrosch]

Alright. This suggests to me that the drive behaves as I would expect. A position task is a complete point-to-point motion. That means given the target position it calculates a trajectory to arrive at that position (while satisfying velocity, acceleration and jerk constraints) with a resulting velocity 0 at arrival time. As a result your drive will at some point prior to arrival start to decelerate in order to arrive at the target position with velocity 0. You have a few options to overcome this with your time based approach:

1. Put the target position behind your actual target:
    The point must be further away than the distance travelled during the deceleration phase.

3. Use velocity task instead to avoid the deceleration ramp at all:
    Be aware that the drive only registers an updated velocity when the absolute value of the velocity changes.
    See example velocity_nonblock.py for reference.

[aaryanmurgunde]

Okay will try this! but this seems like no so reliable workaround, any other method that you can suggest ?

[elrosch]

Another option I could think of would involve changing the telegram (i.e. switch from point-to-point to velocity mode). If you want to avoid positioning trajectories to be used, you might probably go for a velocity telegram (i.e. telegram 1 or telegram 102). You can find an example here: https://github.com/Festo-se/festo-edcon/blob/main/src/edcon/cli/tg1.py

Also make sure to configure the correct telegram in the drive.