petercorke
commented
2 years ago Thanks for your comment. The mass matrix is indeed singular
>>> r.inertia(qk)
array([[ 1.524, -0.3125, 0.01157, 0.01721, 0.01786, 0],
[ -0.3125, 2.391, 0.8585, -0.008231, -0.001254, 0],
[ 0.01157, 0.8585, 0.6007, 0.03719, -0.00411, 0],
[ 0.01721, -0.008231, 0.03719, 0.01472, -0.001271, 0],
[ 0.01786, -0.001254, -0.00411, -0.001271, 0.001574, 0],
[ 0, 0, 0, 0, 0, 0]])with a zero row and column. Something going on with the last link
>>> r.dynamics()
┌──────┬───────┬───────────────────────┬────────────────────────┬────┬────┬────────┬────┐
│ j │ m │ r │ I │ Jm │ B │ Tc │ G │
├──────┼───────┼───────────────────────┼────────────────────────┼────┼────┼────────┼────┤
│link1 │ 3.7 │ 0, -0.0256, 0.00193 │ 1, 2, 3, 0, 0, 0 │ 0 │ 0 │ 0, 0 │ 1 │
│link2 │ 8.39 │ 0.212, 0, 0.113 │ 0, 0, 0, 0, 0, 0 │ 0 │ 0 │ 0, 0 │ 1 │
│link3 │ 2.33 │ 0.15, 0, 0.0265 │ 0, 0, 0, 0, 0, 0 │ 0 │ 0 │ 0, 0 │ 1 │
│link4 │ 1.22 │ 0, -0.0018, 0.0163 │ 0, 0, 0, 0, 0, 0 │ 0 │ 0 │ 0, 0 │ 1 │
│link5 │ 1.22 │ 0, -0.0018, 0.0163 │ 0, 0, 0, 0, 0, 0 │ 0 │ 0 │ 0, 0 │ 1 │
│link6 │ 0.19 │ 0, 0, -0.00116 │ 0, 0, 0, 0, 0, 0 │ 0 │ 0 │ 0, 0 │ 1 │
└──────┴───────┴───────────────────────┴────────────────────────┴────┴────┴────────┴────┘which I'm guessing is due to zero inertia for the last link, even though it has mass, I think the fact it is on the z-axis of the link frame might be an issue. If I hack the inertia of that link
>>> r[5].I = [1, 2, 3]then the mass matrix is
>>> r.inertia(qk)
array([[ 3.761, -0.7269, -0.4028, -0.3972, -1.608, -1.613],
[ -0.7269, 3.674, 2.142, 1.275, 0.01087, 1.128],
[ -0.4028, 2.142, 1.884, 1.32, 0.008014, 1.128],
[ -0.3972, 1.275, 1.32, 1.298, 0.01085, 1.128],
[ -1.608, 0.01087, 0.008014, 0.01085, 2.001, 0],
[ -1.613, 1.128, 1.128, 1.128, 0, 3]])Adding motor inertia Jm would also help, with gearing that's a fairly dominant source of inertia and will set a non-zero diagonal for the matrix.
Describe the bug The mass matrix of the DH vision of the UR3( and UR5, UR10) is singular no matter what values of the joint positions are.
Version information pip 21.3.1
To Reproduce Code: import spatialgeometry as sg import roboticstoolbox as rtb import spatialmath as sm import numpy as np
robot = rtb.models.DH.UR5() # UR3 UR10
qInitial = robot.qrandom # random initial position qdInitial = np.array([0, 0, 0, 0, 0, 0])
torqueLim = np.array([[-56, -56, -28, -12, -12, -12], [56, 56, 28, 12, 12, 12]]) torque = np.random.uniform(low=torqueLim[0, :], high=torqueLim[1, :], size=(6,)) acc = robot.accel(qInitial, qdInitial, torque) print(acc)
Error Info.: numpy.linalg.LinAlgError: Singular matrix
Expected behavior Joint accelerations of the robot should be returned.
Environment (please complete the following information):
Additional context Possible reason: I guess the DH parameters in the DH/UR5.py are wrong.
original DH parameters:
a = [0, -0.42500, -0.39225, 0, 0, 0] d = [0.089459, 0, 0, 0.10915, 0.09465, 0.0823] alpha = [pi/2, zero, zero, pi/2, -pi/2, zero] when I change these parameters into a = [0, 0, -0.42500, -0.39225, 0, 0] d = [0.089459, 0, 0, 0.10915, 0.09465, 0.0823] alpha = [zero , pi/2, zero, zero, pi/2, -pi/2] the codes obtain the joint accelerations of the robot which looks reasonable, but the when I plot the robot, the number of links of the robot has changed from 6 to 5. I have no idea what happened.