def plot(self, joints, ax, target=None, show=False):
"""Plots the Chain using Matplotlib
Parameters
----------
joints: list
The list of the positions of each joint
ax: matplotlib.axes.Axes
A matplotlib axes
target: numpy.array
An optional target
show: bool
Display the axe. Defaults to False
"""
from ikpy.utils import plot
if ax is None:
# If ax is not given, create one
ax = plot.init_3d_figure()
plot.plot_chain(self, joints, ax, name=self.name)
# Plot the goal position
if target is not None:
plot.plot_target(target, ax)
if show:
plot.show_figure()
def test_chain():
fig, ax = plot.init_3d_figure()
torso_right_arm = chain.Chain.from_urdf_file(
"../resources/poppy_torso/poppy_torso.URDF",
base_elements=[
"base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x",
"chest", "r_shoulder_y"
],
last_link_vector=[0, 0.18, 0],
active_links_mask=[
False, False, False, False, True, True, True, True, True
])
torso_left_arm = chain.Chain.from_urdf_file(
"../resources/poppy_torso/poppy_torso.URDF",
base_elements=[
"base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x",
"chest", "l_shoulder_y"
],
last_link_vector=[0, 0.18, 0],
active_links_mask=[
False, False, False, False, True, True, True, True, True
])
# Plot right arm
joints = [0] * len(torso_right_arm.links)
torso_left_arm.plot(joints, ax)
# Plot left arm
joints = [0] * len(torso_left_arm.links)
torso_right_arm.plot(joints, ax)
plt.savefig("out/torso.png")
def test_ergo(resources_path, interactive):
a = chain.Chain.from_urdf_file(resources_path + "/poppy_ergo.URDF")
target = [0.1, -0.2, 0.1]
frame_target = np.eye(4)
frame_target[:3, 3] = target
joints = [0] * len(a.links)
ik = a.inverse_kinematics_frame(frame_target, initial_position=joints)
fig, ax = plot.init_3d_figure()
a.plot(ik, ax, target=target)
plt.savefig("out/ergo.png")
if interactive:
plot.show_figure()
def plotChain(my_chain, args):
print("Relative object-position is ", args)
target_position = [args["x"], args["y"], args["z"]]
# plot chain vectors
fig, ax = plot_utils.init_3d_figure()
print("The angles of each joints are : ",
my_chain.inverse_kinematics(target_position))
my_chain.plot(getChainPos(my_chain, target_position), ax, target=None)
# my_chain.plot(getChainPos(my_chain, target_position), ax, target=target_vector)
# show the vectors in a matplotlib-window
plt.xlim(-2, 2)
plt.ylim(-2, 2)
plt.show()
def test_ik(torso_right_arm):
fig, ax = plot.init_3d_figure()
# Objectives
target = [0.1, -0.2, 0.1]
joints = [0] * len(torso_right_arm.links)
joints[-4] = 0
frame_target = np.eye(4)
frame_target[:3, 3] = target
ik = torso_right_arm.inverse_kinematics_frame(frame_target,
initial_position=joints)
torso_right_arm.plot(ik, ax, target=target)
np.testing.assert_almost_equal(torso_right_arm.forward_kinematics(ik)[:3,
3],
target,
decimal=3)
def test_urdf_chain(resources_path, interactive):
"""Test that we can open chain from a URDF file"""
chain1 = chain.Chain.from_urdf_file(
os.path.join(resources_path, "poppy_torso/poppy_torso.URDF"),
base_elements=[
"base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x",
"chest", "r_shoulder_y"
],
last_link_vector=[0, 0.18, 0],
active_links_mask=[
False, False, False, False, True, True, True, True, True
])
joints = [0] * len(chain1.links)
joints[-4] = 0
fig, ax = plot.init_3d_figure()
chain1.plot(joints, ax)
plt.savefig("out/chain1.png")
if interactive:
plt.show()
@staticmethod
def _ik2pupperarm(joints):
return np.rad2deg([joints[1], -joints[2], -joints[3], 0.0])
@staticmethod
def _makeMoveCmd(degs):
print(f"move([{','.join([str(x) for x in np.around(degs, 2)])}])")
def pos2joints(self, target):
joints = self.chain.inverse_kinematics(target)
return self._ik2pupperarm(joints)
armik = ArmIK()
target_position = np.array([-0.10, 0.15, 0.10])
joints = armik.pos2joints(target_position)
print(joints)
armik._makeMoveCmd(joints)
import matplotlib.pyplot as plt
fig, ax = plot_utils.init_3d_figure()
armik.chain.plot(armik.chain.inverse_kinematics(target_position),
ax,
target=target_position)
ax.set_xlim(-0.2, 0.2)
ax.set_ylim(-0.2, 0.2)
ax.set_zlim(-0, 0.4)
plt.show()