Relative movement in sequence is broken

[agutenkunst]

When using relative motion in a sequence the relative seems to reference to the position before the sequence. This is especially bad when transitioning from non-sequence to sequence since the robot could crash.

Commit

pilz-robot-programming 0.3.3

Steps to reproduce

Run

#!/usr/bin/env python

from geometry_msgs.msg import Pose, Point
from pilz_robot_programming.robot import *
from pilz_robot_programming.commands import *
import math
import time

def start_program():
    print("Executing " +__file__)

    #initial variables
    r = Robot('1')
    vel = 0.1

    orientation_pick = from_euler(0, math.radians(180), math.radians(45))
    
    r.move(Ptp(goal=Pose(position=Point(-0.499, -0.195, 0.3), orientation=orientation_pick), vel_scale = vel, relative=False))

    print("High 1 before: ", r.get_current_pose().position.z)
    r.move(Lin(goal=Pose(position=Point(0,0,-0.05)), vel_scale = vel, relative=True))
    print("Low 1: ", r.get_current_pose().position.z)
    r.move(Lin(goal=Pose(position=Point(0,0,0.05)), vel_scale = vel, relative=True))
    print("High 1 after: ", r.get_current_pose().position.z)
    

    seq = Sequence()
    seq.append(Lin(goal=Pose(position=Point(0,0,-0.05)), vel_scale = vel, relative=True))
    seq.append(Lin(goal=Pose(position=Point(0,0,0.05)), vel_scale = vel, relative=True))
    print("High 2 before: ", r.get_current_pose().position.z)
    r.move(seq)
    print("High 2 after: ", r.get_current_pose().position.z)

if __name__ == "__main__":
    # Init a rosnode
    rospy.init_node('robot_program_node', anonymous=True)

    start_program()

Expected behavior

The two seperate LIN should move the robot exactly in the same way like the two LIN in the sequence.

Observed behavior

During the sequence the robot moves 0.05 down but 0.1 up.

Logfiles

Output of rostopic echo /sequence_move_group/goal output_goal.txt

Output in console where program runs:

Executing ./row_pick_place.py
('High 1 before: ', 0.29999482722152604)
('Low 1: ', 0.25003443220807708)
('High 1 after: ', 0.30001076941836813)
('High 2 before: ', 0.30001076941836813)
('High 2 after: ', 0.34993052902416205)

[agutenkunst]

@dbakovic Do you agree on the expected behaviour? Or do you have something different in mind?

[agutenkunst]

@PilzDE/robotics Can this be closed?

[JonathanGruner]

No, the expected behavior is the same to mine point of view. Because i mostly use relative commands, Sequence is very difficult to use. I would appreciate if you could solve this bug.

[martiniil]

I would suggest that we do not treat this as a bug but as a missing feature. Whilst I agree on the expected behaviour, this cannot be realized in the Python API. It would at least require a substantial architectural change. So the question arises, if we want to support it at all. @agutenkunst would you agree if we do not support this feature? (i.e. do not execute such a sequence)

[jschleicher]

At least for the given example you could implement the desired behaviour by passing the previous goal instead of the current pose in https://github.com/PilzDE/pilz_industrial_motion/blob/bd5a71ccb64559fbd050525b761af6b7afa70fb0/pilz_robot_programming/src/pilz_robot_programming/commands.py#L277

This would at least work for pose-only or joint-only-sequences. Mixed sequences should imho throw an exception instead of moving to a possibly colliding position.

[JonathanGruner]

But what about sevaral relative commands in line? Can the program handle this? Because than the previous point wouldn't be given directly...

i.e. a sequence like:

• Move to point X
• Move 10 cm upwards (relative)
• Move 10 cm forward (relative)
• Move 3 cm diagonal in x/z (relative)
• Move to exit point

[jschleicher]

Yes, I'd suggest to support cartesian-only-sequences with relative. I think that covers your use case @JonathanGruner

Once an intermediate point is given in joint angles, the pose would have to be calculated using forward kinematics (possible, but currently out-of-scope of the python API level).