Interpolated Position (IP) Mode for ASDA-A2 servo drives

I've started using the this library to get my Delta ASDA-A2 drives up and running and I really love using the library so far. Everything seems to be working great when directly reading and writing to the SDO what has allowed me to implement the Profile position, velocity and torque modes of the drive.

However ideally I'd also get the drive up and running using the SYNC messages that would allow for continuous readout of the current position, velocity and torque as well as allowing me to use the Interpolated Position Mode of the drive to stream new set-points to the drive. I'm wondering slightly confused how to best set this up as my current approach seems to get the drive in an enabled mode but remains unresponsive to the new target positions being sent.

I'm currently following the CANopen manual from the supplier found here: ASDA-A2 CANopen manual

#!/usr/bin/env python

"""
Minimal test to get Interpolated Position Mode working over CANopen
on a Delta ASDA-A2 drive. 
"""

import canopen
import time
from enum import Enum

class DriveMode(Enum):
    POSITION = 1  # Profile position mode
    VELOCITY = 3  # Profile velocity mode
    TORQUE = 4    # Profile torque mode
    IP_MODE = 7   # Interpolated position mode

class ASDA_A2_Drive:
    def __init__(self, node_id, network, eds_file):
        self.network = network
        self.node_id = node_id
        self.node = canopen.RemoteNode(node_id, eds_file)
        self.network.add_node(self.node)
        self.node.load_configuration()  # Load configuration from EDS file
        self.current_mode = None

    def set_drive_mode(self, mode):
        self.node.sdo['Modes of operation'].raw = mode.value
        self.current_mode = mode

    def enable_drive(self):
        # Step by step enabling of the drive as per manual for P1-01 = 0x0C
        self.node.sdo[0x6040].raw = 0x0006  # Ready to switch on
        self.node.sdo[0x6040].raw = 0x0007  # Switched on
        self.node.sdo[0x6040].raw = 0x000F  # Enable operation

    def disable_drive(self):
        # Disable the drive
        self.node.sdo[0x6040].raw = 0x0007  # Switched on
        self.node.sdo[0x6040].raw = 0x0006  # Ready to switch on
        self.node.sdo[0x6040].raw = 0x0000  # Shutdown

    def configure_ip_mode(self):
        # Configure for IP Mode (specific PDO settings to be adjusted as needed)
        self.node.sdo['Interpolation sub mode select'].raw = 0  # Assuming default sub mode
        self.node.sdo['Interpolation time period'].raw = 10     # time in units (e.g. 1ms)
        self.node.sdo['Interpolation time period']['Interpolation time units'].raw = 1   # Time units 1-20ms
        self.node.sdo['Interpolation time period']['Interpolation time index'].raw = -3  # Interpolation time index -3 = 10^-3s

        self.node.tpdo.read()
        self.node.rpdo.read()

        self.node.rpdo[4].clear()
        self.node.rpdo[4].add_variable('Interpolation data record', 'Parameter1 of ip function')
        self.node.rpdo[4].add_variable('Controlword')
        self.node.rpdo[4].enabled = True

        # Save new configuration (node must be in pre-operational)
        self.node.nmt.state = 'PRE-OPERATIONAL'
        self.node.tpdo.save()
        self.node.rpdo.save()

    def send_position_update(self, position):
        # Send a position update to the drive
        self.node.rpdo[4]['Interpolation data record.Parameter1 of ip function'].phys = position
        self.node.rpdo[4]['Controlword'].raw = 0x003F

    def execute_ip_mode_trajectory(self, trajectory_points):
        # Execute trajectory updates in IP Mode
        self.node.rpdo[4]['Interpolation data record.Parameter1 of ip function'].phys = trajectory_points[0]
        self.node.rpdo[4]['Controlword'].raw = 0x000F  # Enable operation

        # Start sync messages at 100 Hz
        self.node.rpdo[4].start(0.01)
        self.node.nmt.state = 'OPERATIONAL'

        # Loop over trajectory points active point at moment of sync should be sent
        for position in trajectory_points:
            print(position)
            self.send_position_update(position)
            time.sleep(0.005)

        # Stop sync messages
        self.node.rpdo[4].stop()

def main():
    # Initialize CANopen network
    network = canopen.Network()
    network.connect(channel="can0", bustype="socketcan")

    # Create drive instance
    drive = ASDA_A2_Drive(1, network, './ASDA_A2_1042sub980_C.eds')

    # Setup IP mode
    drive.enable_drive()
    drive.set_drive_mode(DriveMode.IP_MODE)
    drive.configure_ip_mode()

    # Example trajectory points (modify as needed)
    trajectory_points = range(0, 50000, 100)

    # Execute the trajectory
    drive.execute_ip_mode_trajectory(trajectory_points)

    # Stop the movement
    drive.disable_drive()

if __name__ == "__main__":
    # sudo ip link set can0 type can bitrate 250000
    # sudo ip link set up can0
    # sudo ifconfig can0 txqueuelen 1000

    main()

Using candump it indeed seems the messages are being sent out at 100Hz and not at the frequency the rpdo is updated as other have had previously pointed out (see log file). I'm however at a loss as to why the drive will not follow the updates and hope someone here can point me in the right direction. I've also added the EDS file I'm using below.

candump.log ASDA_A2_1042sub980_C.txt

christiansandberg / canopen

Interpolated Position (IP) Mode for ASDA-A2 servo drives #407