Code: Select all
def calculateIK3(feet_positions, jointDamping):
pos_body = np.array(p.getBasePositionAndOrientation(hexapod_ID)[0])
orn_body = p.getBasePositionAndOrientation(hexapod_ID)[1]
rest_poses = []
target_pos = []
link_indexes = [x for x in range(3, 24, 4)]
for j in range(6):
rest_poses.append(pos_body + p.multiplyTransforms([0, 0, 0], orn_body, baseToEndEffectorConstVec[j], [0, 0, 0, 1])[0])
tp = p.multiplyTransforms(rest_poses[j], orn_body, feet_positions[j], [0, 0, 0, 1])
target_pos.append(tp[0])
ik2 = [0] * 18
# ik = p.calculateInverseKinematics2(
# hexapod_ID,
# [link_indexes[0]],
# [target_pos[0]],
# lowerLimits=ll,
# upperLimits=ul,
# jointRanges=jr,
# restPoses=rest_poses
# )
# print(ik, len(ik))
iks = []
for j in range(6):
temp_ik = p.calculateInverseKinematics(
hexapod_ID,
link_indexes[j],
target_pos[j]
)
temp_ik = list(temp_ik[(3 * j):3 + (3 * j)])
# print(len(temp_ik), temp_ik)
iks.extend(temp_ik)
# print(iks)
# iks = list(temp_ik[:3]) + [0] * 15
return iks