Consecutive cartesian motions do not slow down

[zevbo]

I am running into the following issue: if I queue up 2 cartesian motions going in the same direction as waypoints, and the first one has a higher speed then the second such that once the robot reaches the first pose it is faster than it needs to be for the second motion, the xarm will not slow down for the second motion.

I have a few videos showing this: https://photos.app.goo.gl/jchLbSujc6UwcNW86

Each video is a 450 mm descent. The first video does the entire descent at 50 mm/s. The second one does the first 50 mm at 800 mm/s and the rest at 50 mm/s. The third video does the first 150 mm at 800 mm/s, and the rest 50 mm/s. Each video has acceleration of 5000m/s^2. The videos relatively clearly show that the robot doesn't slow down when it makes it to the second part of the motion; it only stops at the end of the second motion.

Here is the script I used to run this test:

xarm.set_mode(0)
xarm.set_state(0)

target_joints = [0, 0.3, -np.pi / 2 - 0.3, 0, np.pi /  2, 0]

code, (x, y, z, roll, pitch, yaw) = xarm.get_forward_kinematics(
    target_joints, input_is_radian=True, return_is_radian=True
)
# TEST 1:
poses_and_speeds = [
    (-400, 50)
]
# TEST 2:
poses_and_speeds = [
    (0, 800), 
    (-400, 50)
]
# TEST 3:
poses_and_speeds = [
    (-100, 800), 
    (-400, 50)
]
# RESET
# poses_and_speeds = [(50, 500)]
for pos, speed in poses_and_speeds:
    xarm.set_position(
        x=x,
        y=y,
        z=z + pos,
        roll=roll,
        pitch=pitch,
        yaw=yaw,
        speed=speed,
        mvacc=5000,
        radius=0,  # https://github.com/xArm-Developer/xArm-Python-SDK/issues/59
    )

[penglongxiang]

hi @zevbo, please update the controller firmware to the latest v1.12.9 as the issue #59 has been solved already. radius=0 was a temporary workaround for situation where the velocity do not need to change in that issue particularly.

After the firmware update, please make thexarm.set_cartesian_velo_continuous(True) configuration if you would like the speed to be continuous between consecutive set_position() commands, and remember to call xarm.set_cartesian_velo_continuous(False) if no longer needed. Thus your test script would be modified to something like this:

xarm.set_mode(0)
xarm.set_state(0)

target_joints = [0, 0.3, -np.pi / 2 - 0.3, 0, np.pi /  2, 0]

code, (x, y, z, roll, pitch, yaw) = xarm.get_forward_kinematics(
    target_joints, input_is_radian=True, return_is_radian=True
)
# TEST 1:
poses_and_speeds = [
    (-400, 50)
]
# TEST 2:
poses_and_speeds = [
    (0, 800), 
    (-400, 50)
]
# TEST 3:
poses_and_speeds = [
    (-100, 800), 
    (-400, 50)
]
# RESET
# poses_and_speeds = [(50, 500)]

xarm.set_cartesian_velo_continuous(True)

for pos, speed in poses_and_speeds:
    xarm.set_position(
        x=x,
        y=y,
        z=z + pos,
        roll=roll,
        pitch=pitch,
        yaw=yaw,
        speed=speed,
        mvacc=5000,
        is_radian=True
    )

xarm.set_cartesian_velo_continuous(False)

[zevbo]

Thanks @penglongxiang . I updated my firmware and now it seems like radius <= 0, ie: MoveLine, works fine. It goes to the first point, comes to a complete stop, and then goes to the second point at the correct speed. However, I am still having trouble with radius > 0.

When I use radius > 0 (I've tried 10 and 50), it doesn't slow down from the first motion to the second. Perhaps that is the expected behavior, as it isn't super clear in the docs where it says "(4) Radius> 0. For example, setting Radius = 5, the turning trajectory is as shown in the black arc in the figure below, which can achieve a smooth turning effect." But I would have assumed that with radius > 0, it would attempt to slow down in maximum 2* the radius length.

[penglongxiang]

Hi @zevbo , please make sure you have added the set_cartesian_velo_continuous() configuration like my code shown above. This configuration will make a difference, or radius>=0 alone will assume the speed to be consistent. Our documentation is indeed not clear, we will improve the instructions later. Additionally, radius is rather more suitable for connecting motions with different directions, than mitions in the same direction.

[zevbo]

Just got back to this @penglongxiang; I tried it with set_cartesian_velo_continous(True), and it still doesn't slow down. Here are videos of what each the three tests looked like with your code (with radius=50): https://photos.app.goo.gl/HQL9EVJowfVZKsWH6

[alonks1234]

Hi, do we have any guidance here?

[penglongxiang]

My apology, I used a smaller velocity(500mm/s) to test the functionality here. It looks like the our solver failed to solve between a large velocity gap with large acceleration. Please try using a smaller velocity gap or a smaller acceleration value first, I will see if the solving algorithm can be optimized.

[zevbo]

I'm also wondering if the blending radius works when going from cartesian motions to joint space motions. With our robots, the transition is smooth when going from joint space motion to cartesian motion, but when going from cartesian motion to joint space motion there is a noticeable pause.

[zevbo]

@penglongxiang notably, this pause seems to happen even when we are moving decently slowly.

[penglongxiang]

hi @zevbo @alonks1234 please try increasing the value of "TCP jerk" in Advanced Parameters from UFACTORY Studio, the small default value (7000) might be the cause since it takes longer to decelerate and deceleration distance will also be long when there is big gap between command velocities, this might be the major limitation. After this change, you may also use a larger acceleration value in the code to make the transition even faster.

[zevbo]

The TCP Jerk change did not work unfortunately. But, after a decent bit of investigation, I'm pretty sure I've diagnosed the issue.

I did it with the following test. The motions here are in order: moving over, then down, then back up, then back over. I ran 3 different versions. In the first one, the horizontal motions were in joint space, and the vertical motions in cartesian. The second is the same as the first except there is a 0.4 second sleep after the motion back up. The third test is the same as the second except all of the motions are in joint space. Here is the code for the test:

xarm.set_mode(0)
xarm.set_state(0)

print(xarm.get_all_joint_angles())

target_joints = [0, 0.3, -np.pi / 2 - 0.3, 0, np.pi / 2, 0]
target_joints2 = [0.4, 0.3, -np.pi / 2 - 0.3, 0, np.pi / 2, 0]
final_joints = [-0, 0.600242, -1.267552, -0., 0.66731,  -0.]

code, (x, y, z, roll, pitch, yaw) = xarm.get_forward_kinematics(
    target_joints, input_is_radian=True, return_is_radian=True
)

test_number = 1
# TEST NUMBER 0: use cartesian motions, no sleep
# TEST NUMBER 1: use cartesian motions, 0.4 second sleep after third motion
# TEST NUMBER 2: no cartesian motions, 0.4 second sleep after third motion

if test_number != 2:
    poses = [
        target_joints2,
        target_joints,
        (0, 0, -350),
        (0, 0, 0),
        target_joints2,
    ]
else:
    poses = [
        target_joints2,
        target_joints,
        final_joints,
        target_joints,
        target_joints2,
    ]

info = VGC10Info
xarm.set_tcp(info.tcp_cog, info.tcp_weight)

xarm.set_cartesian_velo_continuous(True)

for i, pos_or_angles in enumerate(poses):

    use_cartesian = len(pos_or_angles) == 3
    if use_cartesian:
        pos_x, pos_y, pos_z = pos_or_angles
        xarm.set_position(
            x=x + pos_x,
            y=y + pos_y,
            z=z + pos_z,
            roll=roll,
            pitch=pitch,
            yaw=yaw,
            speed=500,
            mvacc=5000,
            is_radian=True,
            radius=100,
        )
    else:
        xarm.set_servo_angle(
            angle=pos_or_angles,
            wait=False,
            speed=1.125,
            mvacc=18,
            radius=100,
        )
    if test_number != 0 and i == 3:
        time.sleep(0.4)
print("SENT ALL")

Both the first test (cartesian + no wait) and the third test (no cartesian + wait) did the blending just fine. However, the wait + cartesian did not do the blending in between last two motions. Here are the videos of the tests (right most video is the first test, left most video is the last test): https://photos.app.goo.gl/TzdsgZaUJJPbuosj7

You can also see that the "SENT ALL" print happens well before the robot starts the motion back up in all three tests.

It seems weird to me that we would need to send the motions so quickly in order for the XArm to be able to do the blending from cartesian to joint space. @penglongxiang , is this something that can be fixed?

The only other explanation I can think of is that maybe it has something to do with set_cartesian_velo_continuous. I tried it both as set to True and False, and got the same results though.

[penglongxiang]

Hi @zevbo, modifying jerk is for solving the initial question in this thread. Which is: robot not slowing down when in continuous Cartesian motion with varing target velocity. I recorded a video showing the behavior on my side after increasing the TCP jerk value.

https://user-images.githubusercontent.com/6412261/226784573-444b0641-a23e-4aa0-98e0-6247aa9a443b.mp4

For the blending issue, sorry I forgot to mention in my last reply that blending from Linear motion to joint motion is not supported yet due to the non-deterministic IK of xArm, but we will see in the future if this issue can be overcome.

[penglongxiang]

please also note set_cartesian_velo_continuous is only effective for continuous CARTESIAN motions and not suitable for any trajectories with joint motion involved.

[zevbo]

Thanks so much! I believe this answers my questions, and I'll close this thread for now.