third_arm_bounds = test_sets.third_arm_bounds # Cible target = [0.2, 0.2, -0.2] # target = [1, 0, 0] # Calcul de la position angles = ik.inverse_kinematic(third_arm_parameters, third_arm_starting_angles, target, bounds=third_arm_bounds) print(angles * 180 / np.pi) # Affichage du résultat simulé if (simulate): fig = matplotlib.pyplot.figure() ax = fig.add_subplot(111, projection='3d') plot_utils.plot_robot(third_arm_parameters, angles, ax) ax.scatter(target[0], target[1], target[2], c="red", s=80) if (real): third_arm = PoppyRightArm() init_third_arm(third_arm) indirect_joints = [1, 3, 6, 8] joints = [third_arm.r_shoulder_y, third_arm.r_shoulder_x, third_arm.r_arm_z, third_arm.r_elbow_y, third_arm.r_m1, third_arm.r_m2, third_arm.r_m3, third_arm.r_m4, third_arm.r_m5] for index, joint in enumerate(joints): if index in indirect_joints: if index == 1: joint.goal_position = -angles[index] / np.pi * 180 - 90 else:
# coding: utf8 from poppy_inverse_kinematics import inverse_kinematic as ik from poppy_inverse_kinematics import test_sets from poppy_inverse_kinematics import plot_utils import matplotlib.pyplot from mpl_toolkits.mplot3d import Axes3D if (__name__ == "__main__"): # Définition des paramètres robot_parameters = test_sets.classical_arm_parameters starting_nodes_angles = test_sets.classical_arm_default_angles target = [3, 1.7, 3] # Calcul de la réponse angles = ik.inverse_kinematic(robot_parameters, starting_nodes_angles, target) # Affichage du résultat fig = matplotlib.pyplot.figure() ax = fig.add_subplot(111, projection='3d') plot_utils.plot_robot(robot_parameters, angles, ax) plot_utils.plot_target(target, ax) print(ik.get_squared_distance_to_target(robot_parameters, angles, target)) matplotlib.pyplot.show()