print("\t CoM world orientation: {}".format(state[1])) print("\t Local inertial frame orientation: {}".format(state[3])) print("\t World link frame orientation: {}".format(state[5])) 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,
print("\t World link frame position: {}".format(state[4])) print("\t CoM world orientation: {}".format(state[1])) print("\t Local inertial frame orientation: {}".format(state[3])) print("\t World link frame orientation: {}".format(state[5])) 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))