print("\t Dimensions: {}".format(visuals[i]))
for _ in count():
world.step(sleep_dt=1. / 240)
raw_input('press enter')
print("Inertia matrix: {}".format(
np.array(sim.calculate_mass_matrix(robot.id, [0., 0., 0., 0., 0., 0.]))))
linkId = 2
com_frame = robot.get_link_states(linkId)[2]
q = robot.get_joint_positions()
print(com_frame)
# com_frame = [0.,0.,0.]
print("Jacobian matrix: {}".format(
sim.calculate_jacobian(robot.id, linkId, com_frame)))
Jlin = robot.get_jacobian(linkId + 1)[:3]
print("Jacobian matrix: {}".format(Jlin))
robot.draw_velocity_manipulability_ellipsoid(linkId + 1, Jlin)
Jlin = robot.get_jacobian(linkId)[:3]
robot.draw_velocity_manipulability_ellipsoid(linkId,
Jlin,
color=(1, 0, 0, 0.7))
cnt = 0
for i in count():
if i % 240 == 0:
if cnt < 3:
print("\t Joint axis: {}".format(info[-4]))
if i in visuals:
print("\t Dimensions: {}".format(visuals[i]))
for _ in count():
world.step(sleep_dt=1./240)
raw_input('press enter')
print("Inertia matrix: {}".format(np.array(sim.calculate_mass_matrix(robot.id, [0., 0., 0., 0., 0., 0.]))))
linkId = 2
com_frame = robot.get_link_states(linkId)[2]
q = robot.get_joint_positions()
print(com_frame)
# com_frame = [0.,0.,0.]
print("Jacobian matrix: {}".format(sim.calculate_jacobian(robot.id, linkId, com_frame)))
Jlin = robot.get_jacobian(linkId + 1)[:3]
print("Jacobian matrix: {}".format(Jlin))
robot.draw_velocity_manipulability_ellipsoid(linkId + 1, Jlin)
Jlin = robot.get_jacobian(linkId)[:3]
robot.draw_velocity_manipulability_ellipsoid(linkId, Jlin, color=(1, 0, 0, 0.7))
cnt = 0
for i in count():
if i%240 == 0:
if cnt < 3:
Jlin = robot.get_jacobian(linkId + 1)[:3]
robot.draw_velocity_manipulability_ellipsoid(linkId + 1, Jlin)
