Md[i, i] = (GEAR_RATIO**2) * MOTOR_INERTIA
# acceleration upper bounds as a function of the joint position
ddq_ub_of_q = np.zeros((nq, N_SAMPLING, 2))
for i in range(N_SAMPLING):
q = se3.randomConfiguration(robot.model, Q_MIN, Q_MAX)
while (CHECK_COLLISIONS and robot.isInCollision(q)):
print("* Robot is in collision at sample %d" % i)
q = se3.randomConfiguration(robot.model, Q_MIN, Q_MAX)
if (DISPLAY_ROBOT_CONFIGURATIONS):
robot.display(q)
v = np.multiply(DQ_MAX, 2.0 * rand(nv) - 1.0)
M = robot.mass(q) + Md
h = robot.biais(q, v)
# dM = dcrba(q); # d/dq M(q) so that d/dqi M = Mp[:,:,i] (symmetric), then dtau = tensordot(Mp,dq,[2,0])
dh_dv = coriolis(q, v)
# d/dvq RNEA(q,vq) = C(q,vq)
dh_dq = drnea(q, v, dv)
# d/dq RNEA(q,vq,aq)
ddqUb = np.divide(TAU_MAX - h, mat_diag(M))
ddqLb = np.divide(-TAU_MAX - h, mat_diag(M))
for j in range(nq):
ddq_ub_of_q[j, i, 0] = q[j]
ddq_ub_of_q[j, i, 1] = TAU_MAX[j] - h[j]
for k in range(nv):
if (k != j):
ddq_ub_of_q[j, i, 1] -= abs(M[j, k] * DDQ_MAX[k])
ddq_ub_of_q[j, i, 1] /= M[j, j]
def mass(self, q):
self.q_def[NQ_OFFSET:] = q.reshape(self.nq - NQ_OFFSET)
return RobotWrapper.mass(self, self.q_def)[NV_OFFSET:, NV_OFFSET:]
