trapezoidal trajectory fails if initial value is equal to final value

[bedaberner]

Describe the bug rtb.trapezoidal fails when initial value and final value are identical, this leads mtraj to fail when the initial pose is shares one joint orientation with the final pose.

Version information pip 1.0.1

To Reproduce

import roboticstoolbox as rtb
import numpy as np

robot = rtb.models.URDF.Panda()

start_j = np.array([0, 0, 0, 0, 0, 0, 0])
end_j = np.array([0, 0, 0, 0, 0, 0, 1])

traj=rtb.mtraj(rtb.trapezoidal,start_j,end_j,100)

which leads to

/home/beda/miniconda3/envs/rosbag-anonymizer/bin/python "/home/beda/.config/JetBrains/CLion2022.1/scratches/test git.py"
Traceback (most recent call last):
  File "/home/beda/.config/JetBrains/CLion2022.1/scratches/test git.py", line 9, in <module>
    traj=rtb.mtraj(rtb.trapezoidal,start_j,end_j,100)
  File "/home/beda/miniconda3/envs/rosbag-anonymizer/lib/python3.8/site-packages/roboticstoolbox/tools/trajectory.py", line 649, in mtraj
    traj.append(tfunc(q0[i], qf[i], t))
  File "/home/beda/miniconda3/envs/rosbag-anonymizer/lib/python3.8/site-packages/roboticstoolbox/tools/trajectory.py", line 425, in trapezoidal
    traj = trapezoidalfunc(t)
TypeError: 'NoneType' object is not callable

Expected behavior rtb.trapezoidal should work even when initial and final value are identical.

Environment (please complete the following information):

• Your OS: Ubuntu 20.04
• Your Python version: 3.8.12

Additional context There is no code implemented for the case of initial and final Value being identical. https://github.com/petercorke/robotics-toolbox-python/blob/4b3e82a6522757ffde1f83aef8d05b3ad475e9de/roboticstoolbox/tools/trajectory.py#L447-L452