Undercutting blend trajectory violates acceleration limits

[martiniil]

Commit

ebeaab24df19f23f261076949c4a0e56becac738

In the program below the generation of the blending trajectory fails if the blend radius is between 0.02 and 0.07, even though both LINs have been generated successfully.

Steps to reproduce

Execute the following program:

from geometry_msgs.msg import Point
from pilz_robot_programming.robot import *
from pilz_robot_programming.commands import *
from numpy import linspace

__REQUIRED_API_VERSION__ = "1"


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

    r = Robot(__REQUIRED_API_VERSION__)

    n_samples = 10
    radii = linspace(0.01, 0.1, n_samples)
    success = [False] * n_samples

    for i in range(n_samples):
        # Simple ptp movement
        r.move(Ptp(goal=Pose(position=Point(0.2, 0, 0.7))))

        # Blend sequence
        blend_sequence = Sequence()
        blend_sequence.append(Lin(goal=Pose(position=Point(0.2, 0.5, 0.7))), blend_radius=radii[i])
        blend_sequence.append(Lin(goal=Pose(position=Point(0.2, 0, 0.8)), vel_scale=0.1))

        try:
            r.move(blend_sequence)
            success[i] = True
        except RobotMoveFailed:
            pass
        
    for i in range(n_samples):
        print("Blend with radius " + str(radii[i]) + (" succeeded" if (success[i]) else " failed"))


if __name__ == "__main__":
    # Init a ros node
    rospy.init_node('robot_program_node')

    start_program()

Output

Blend with radius 0.01 succeeded Blend with radius 0.02 failed Blend with radius 0.03 failed Blend with radius 0.04 failed Blend with radius 0.05 failed Blend with radius 0.06 failed Blend with radius 0.07 failed Blend with radius 0.08 succeeded Blend with radius 0.09 succeeded Blend with radius 0.1 succeeded

More log

[ERROR] [WallTime: 1549608496.828700481 /home/imartini/ros/src/pilz_industrial_motion/pilz_trajectory_generation/src/trajectory_functions.cpp 190]: Joint deceleration limit of prbt_joint_3 violated. Set the acceleration scaling factor lower! Actual joint deceleration is -4.06842, while the limit is -3.49. [ERROR] [WallTime: 1549608496.828762843 /home/imartini/ros/src/pilz_industrial_motion/pilz_trajectory_generation/src/trajectory_functions.cpp 388]: Inverse kinematics solution of the 5th sample violates the joint velocity/acceleration/deceleration limits. [ INFO] [WallTime: 1549608496.828794933 /home/imartini/ros/src/pilz_industrial_motion/pilz_trajectory_generation/src/trajectory_blender_transition_window.cpp 83]: Failed to generate joint trajectory for blending trajectory.

[martiniil]

Limiting the acceleration of the blend trajectory could be a new feature of the implementation.