Motion skipped when updating TCP position.

[WWimshurst]

I have a script that: updates the TCP, completes various motions in the joint space and cartesian space, updates TCP again and then completes some further motion.

When the wait parameter of each (set_position or set_servo_angle) motion is left at default (False), the motion before the second TCP change is ignored entirely. If I add a wait=True to any motion before the second TCP change, then the motions before the second TCP change also execute. Is this a bug? It is unclear if the functionality of wait means: "physically stop the robot after this motion and therefore prevent any blend" or "wait for this line of code to execute before moving onto the next line".

The TCP of 0,0,0. is just an example but we often have multiple TCP on one end effector that we need to change between. I would also be very grateful if you spot any improvements to the style of the code below. For example, any necessary lines. Thanks!

#!/usr/bin/env python3
import os
import sys
import time
from xarm.wrapper import XArmAPI

arm = XArmAPI('192.168.1.235',is_radian=False)
arm.clean_error()
arm.clean_warn()

#None of this block executes
arm.set_tcp_offset([0, 0, 172, 0, 0, 0])
arm.set_tcp_load(0.75, [0, 0, 0])
arm.clean_error()
arm.clean_warn()
arm.motion_enable(enable=True)
arm.set_mode(0)
arm.set_state(state=0)
arm.set_servo_angle(angle=[0,0,-90,30,0], speed=10, mvacc=5) #Go to arbitrary home position
time.sleep(1)
arm.set_servo_angle(angle=[21.799,-43.22,-40.248,83.468,21.799], speed=16.92, mvacc=16.92)
print(arm.get_position(),arm.get_servo_angle())
arm.set_position(x=250, y=100, z=280, roll=-180, pitch=0, yaw=0, radius=0, speed=50, mvacc=40)
arm.set_position(x=250, y=-100, z=280, roll=-180, pitch=0, yaw=0, radius=0, speed=50, mvacc=40)
print(arm.get_position(),arm.get_servo_angle())
time.sleep(1)
arm.set_servo_angle(angle=[0,0,-90,30,0], speed=10, mvacc=5) #If I add wait=True here, then all of the above code executes
#None of the above executes

arm.set_tcp_offset([0, 0, 0, 0, 0, 0])
arm.set_tcp_load(0, [0, 0, 0])
arm.clean_error()
arm.clean_warn()
arm.motion_enable(enable=True)
arm.set_mode(0)
arm.set_state(state=0)
arm.set_servo_angle(angle=[21.801,-35.873,-15.749,51.622,21.801], speed=16.92, mvacc=16.92)
print(arm.get_position(),arm.get_servo_angle())
arm.set_position(x=250, y=100, z=280, roll=-180, pitch=0, yaw=0, radius=0, speed=50, mvacc=40)
arm.set_position(x=250, y=-100, z=280, roll=-180, pitch=0, yaw=0, radius=0, speed=50, mvacc=40)
arm.set_servo_angle(angle=[0,0,-90,30,0], speed=10, mvacc=5)
arm.disconnect()

[penglongxiang]

HI @WWimshurst, I think the cause is the second arm.set_tcp_offset() in your code.

In our system design, each function in the code represents one coded instruction to be sent to the controller. During program execution process, the instructions are sent to controller one by one in the coded sequence and order. If wait=True is specified, the next instruction will be held until the current motion finishes. If wait=False is specified, the execution will not block so that the following instruction will be sent to controller immediately afterwards.

Some frequently used configuration instruction like acceleration/jerk, load and GPIO change will be put in a queue upon receiving and will not be executed immediately, thus to ensure a proper time when the configuration change takes place. However, in our current design, unlike set_tcp_load() which is executed in a queued fashion after the previous motion commands, set_tcp_offset() will be executed immediately upon instruction receiving, since it is not supposed to be changing frequently during a user program.

Those configuration operation can automatically reset the system (state = 5) and clear the instruction buffer for safety. In this way, I guess the wait=False and the second set_tcp_offset() combined contribute to the failure of execution of the first part, and the successful execution after setting wait=True in the previous joint motion can also be explained. If you have to change TCP offset in the middle of a program, the current solution will be just making sure the motion right before it to be specified as wait=True.

[WWimshurst]

Thank you @penglongxiang for such a fast and clear reply!