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)