dh_dv_min = mat_zeros((nv, nv)) + 1e10
q_h_max = mat_zeros((nq, nv))
q_h_min = mat_zeros((nq, nv))
q_M_max = createListOfLists(nv, nv)
q_M_min = createListOfLists(nv, nv)
Md = mat_zeros((nv, nv))
for i in range(nv):
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))
if (rightAnswerCounter > 10):
ans = input(
"The model has been right for %d times in a row. Do you wanna stop?"
% rightAnswerCounter)
if 'y' in ans:
print("Stopping script")
print("Active collision pairs are: [", end=' ')
for (cp, active) in enumerate(
robot.collision_data.activeCollisionPairs):
if (active):
print("%d, " % cp, end=' ')
print("]")
break
q = se3.randomConfiguration(robot.model, Q_MIN, Q_MAX)
if (robot.isInCollision(q)):
print("\n *** Robot is in collision at sample %d ***" % i)
robot.display(q)
collPairList = robot.findAllCollisionPairs()
ans = input("Do you think the robot is in collision [y/n]? ")
if ('n' in ans):
rightAnswerCounter = 0
print("Deactivating all these collisions")
for cp in collPairList:
print(" collision %d:" % cp[0], cp[1], end=' ')
robot.collision_data.deactivateCollisionPair(cp[0])
else:
rightAnswerCounter += 1
else:
print("\n *** Robot NOT IN COLLISION at sample %d ***" % i)
robot.display(q)
