Abnormal Trajectories in Cluttered Environments Using Toppra

[zorro]

In version 4.8, I use zmqRemoteApi for a seven-degree-of-freedom pathToTrajectory task. The code is largely based on the previous path planning demo, while the part that generates trajectories using toppra refers to the latest demo. Part of the code is shown below:

Code: Select all

...
    for i in range(len(targets)):
        print('i: ', i)
        pose = sim.getObjectPose(targets[i], simBase)
        simIK.setObjectPose(ikEnv, ikTarget, pose, ikBase)

        config = None
        while not config:
            config = simIK.findConfig(ikEnv, ikGroup, ikJointHandles, 0.65, 0.1, [1, 1, 1, 0.1],
                                      lambda x: configuration_validation_callback(sim, simJointHandles, roboCollection,
                                                                                  x))
            if not config:
                print("No valid config found. Retrying...")

        if config:
            config = [round(num, 6) for num in config]
            print('config: ', config)
            task = simOMPL.createTask('task')
            # print(task)

            # simOMPL.setAlgorithm(task, simOMPL.Algorithm.RRTConnect)
            simOMPL.setAlgorithm(task, algorithm)

            # simOMPL.setCollisionPairs(task, [roboCollection, collisionCollection])
            # simOMPL.setCollisionPairs(task, [roboCollection, sim.handle_all])
            simOMPL.setCollisionPairs(task, [roboCollection, roboCollection, roboCollection, sim.handle_all])
            # simOMPL.setCollisionPairs(task, [roboCollection, roboCollection, roboCollection, collisionCollection])
            simOMPL.setStateSpaceForJoints(task, simJointHandles, useForProjection)
            simOMPL.setStartState(task, get_config(sim, simJointHandles))
            simOMPL.setGoalState(task, config)

            simOMPL.setup(task)
            # simOMPL.simplifyPath(task, -1.0)
            valid = simOMPL.isStateValid(task, config)

            print('valid: ', valid)
            solution = False
            res = False
            path = False
            while not solution:
                res, path = simOMPL.compute(task, 6, -1, 300) 
                print('OMPL res: ', res)
                solution = simOMPL.hasExactSolution(task)
                print('Exact_solution: ', solution)

            # config = None

            if res and path:
                followPathScript = None
                fkVel = 20
                fkAccel = 20
                fkJerk = 80
                fkMaxVel = [fkVel*math.pi/180,fkVel*math.pi/180,fkVel*math.pi/180,fkVel*math.pi/180,fkVel*math.pi/180,fkVel*math.pi/180,fkVel*math.pi/180]
                fkMaxAccel = [fkAccel*math.pi/180,fkAccel*math.pi/180,fkAccel*math.pi/180,fkAccel*math.pi/180,fkAccel*math.pi/180,fkAccel*math.pi/180,fkAccel*math.pi/180]
                minMaxVel= (-fkMaxVel[0],fkMaxVel[0],-fkMaxVel[1],fkMaxVel[1],-fkMaxVel[2],fkMaxVel[2],-fkMaxVel[3],fkMaxVel[3],-fkMaxVel[4],fkMaxVel[4],-fkMaxVel[5],fkMaxVel[5],-fkMaxVel[6],fkMaxVel[6])
                minMaxAccel = (-fkMaxAccel[0],fkMaxAccel[0],-fkMaxAccel[1],fkMaxAccel[1],-fkMaxAccel[2],fkMaxAccel[2],-fkMaxAccel[3],fkMaxAccel[3],-fkMaxAccel[4],fkMaxAccel[4],-fkMaxAccel[5],fkMaxAccel[5],-fkMaxAccel[6],fkMaxAccel[6])
                pl = sim.getPathLengths(path,7)
                print('pl: ', pl)
                if followPathScript == None:
                    followPathScript = -1
                pathPts = False
                times = False
                pathPts, times, followPathScript = sim.generateTimeOptimalTrajectory(path, pl[0], minMaxVel, minMaxAccel, 1000, 'not-a-knot', 5, followPathScript)

                st = sim.getSimulationTime()
                dt = 0
                sw = sim.setStepping(True)
                while dt<times[-1]:
                    print('dt: ', dt, 'times[-1]: ', times[-1])
                    p = sim.getPathInterpolatedConfig(pathPts, times, dt)
                    set_config(sim, simJointHandles, p, 1)
                    sim.step()
                    dt = sim.getSimulationTime()-st
                p = sim.getPathInterpolatedConfig(pathPts, times, times[-1])
                set_config(sim, simJointHandles, p, 1)
                sim.setStepping(sw)

            simOMPL.destroyTask(task)

    joint_config = get_config(sim, simJointHandles)

Under normal circumstances, the robot arm operates correctly. However, in more complex environments, such as those with obstacles, some abnormal phenomena occur. Normally, the timestamped trajectory generated by toppra has a total duration of around 10 seconds. But when I add obstacles along the path, sometimes the trajectory generated by toppra can last up to 50 seconds. During this time, some joints of the robot arm often move very slowly, and at times, only the end-effector moves. Ultimately, the robot arm may reach the target configuration, or it might collide with obstacles. If I do not use toppra and only use ompl, this phenomenon does not occur.
I want to know the reason for this phenomenon and how to solve this problem. I speculate that in environments with obstacles, the path planning algorithm used by ompl may generate paths that are too winding or have abrupt turns, leading to suboptimal trajectories generated by toppra. However, I don't know how to resolve this issue. I have tried increasing the number of waypoints for toppra and extending the computation time, but these measures have not improved the situation.

[coppelia]

Hello,

the important thing is to first make sure OMPL doesn't produce a path that collides. To that end, make sure that the arm or object can't jump over obstacles because two adjacent collision-free nodes are too far away. You will have to play with the state validity checking resolution (i.e. adjust the resolution downwards). You do this with simOMPL.setStateValidityCheckingResolution.

Then make sure that the path simplification procedure that OMPL applies does not produce a collision that wasn't there previously.

Regarding the trajectory generated by TOPPRA: perfectly following the input path is rarely possible, because as you say, it would generate infinite velocities when changing direction. So if the produces trajectory generates collisions, this usually means that you do not have enough input waypoints. Have a look at the demo scene in scenes/trajectoryAndMotion/pathToTrajectory.ttt where you can see the effect of path resampling (i.e. adding additional waypoints).

Another effect that might happen is that during path planning the generated path effectively comes very close to obstacles, without however touching them (if perfectly following the path would be possible). To avoid this, use collision shapes that are slightly inflated versions of the robot, in order to obtain a clearance of at least x mm.

Cheers

[zorro]

I found the issue. The joint angles output by ompl are mapped within ±pi. Therefore, when the actual joint angles are greater than pi or less than -pi, there is a numerical jump, which results in abnormal trajectories when input into toppra. I'm not sure if this is a worthwhile issue to improve, or if it's just a problem with my settings.

[coppelia]

This can happen if yur joints are cyclic. If your joints have lower and upper limits, then this shouldn't happen.

Cheers

[zorro]

I am certain that I have set the joint limits, but I noticed that after using sim.resamplePath, the values are mapped within ±pi(as well as some peculiar sign mappings).
I have tested many times and I am almost sure the problem lies in the interpolation function, but I still do not know how to resolve it.
My interpolation function is as follows:

Code: Select all

                path_interp = sim.resamplePath(path, pl[0], 500,
                                        {'type': 'linear', 'strength': 1.0, 'forceOpen': False}, [1, 1, 1, 1, 1, 1, 1])
                pl = sim.getPathLengths(path_interp,7)
                print('path points num: ', len(path_interp))
                print('pl: ', pl[0])
                path_interp = sim.resamplePath(path_interp, pl[0], 2*len(pl[0]),
                                        {'type': 'quadraticBezier', 'strength': 1.0, 'forceOpen': False}, [1, 1, 1, 1, 1, 1, 1])
                pl = sim.getPathLengths(path_interp,7)
                print('path points num: ', len(path_interp))
                print('pl: ', pl[0])

[zorro]

I am certain that I have set the joint limits, but I noticed that after using sim.resamplePath, the values are mapped within ±pi(as well as some peculiar sign mappings).
I have tested many times and I am almost sure the problem lies in the interpolation function, but I still do not know how to resolve it.
My interpolation function is as follows:

Code: Select all

                path_interp = sim.resamplePath(path, pl[0], 500,
                                        {'type': 'linear', 'strength': 1.0, 'forceOpen': False}, [1, 1, 1, 1, 1, 1, 1])
                pl = sim.getPathLengths(path_interp,7)
                print('path points num: ', len(path_interp))
                print('pl: ', pl[0])
                path_interp = sim.resamplePath(path_interp, pl[0], 2*len(pl[0]),
                                        {'type': 'quadraticBezier', 'strength': 1.0, 'forceOpen': False}, [1, 1, 1, 1, 1, 1, 1])
                pl = sim.getPathLengths(path_interp,7)
                print('path points num: ', len(path_interp))
                print('pl: ', pl[0])

Wait, when I removed the parameter [1,1,1,1,1,1,1] from

Code: Select all

# python
path_interp = sim.resamplePath(path, pl[0], 500, {'type': 'linear', 'strength': 1.0, 'forceOpen': False}, [1,1,1,1,1,1,1])

everything worked fine. It appears that I didn't read the API documentation carefully enough. I don't need to add this parameter at all, right?

[coppelia]

Yes, this is correct: if you omit that argument, it defaults to [0, 0, 0, .., 0] which indicates that the underlying dimension is not cyclic nor limited otherwise.

Cheers