if __name__ == "__main__":
real = False
simulate = True
animate = True
# Paramètres du bras
third_arm_parameters = test_sets.third_arm_parameters
third_arm_starting_angles = test_sets.third_arm_default_angles
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]
# 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()
