Skip to content
Keegan Neave edited this page Jun 15, 2022 · 25 revisions

Getting started

This tutorial will introduce you to the basic concepts of IKPy. You can test a live version in the corresponding IPython Notebook.

The Chain object

The Chain is the main object you will work with : It is a list of links you can move, inspect, and plot. There are multiple ways to create the object:

  • From pre-created JSONs
  • From a URDF file
  • Manually

Importing a pre-created JSON chain.

Pre-created JSON chains can be imported in one line. For example, to create the left arm of Baxter from a JSON found in the resources directory of the repo:

baxter_left_arm_chain = Chain.from_json_file("../resources/baxter/baxter_left_arm.json")

Go here for more info

Creating a chain from a URDF file

One great feature of IKPy is that you can create your chains from a URDF file. If your file is URDF-compliant, it will work!

my_chain = Chain.from_urdf_file("poppy_ergo.URDF")

To discover more advanced features of URDF parsing, go to the dedicated page.

Creating a chain manually

If you do not want to mess with URDF files (yet), you can create your chains manually:

from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink

left_arm_chain = Chain(name='left_arm', links=[
    OriginLink(),
    URDFLink(
      name="shoulder",
      origin_translation=[-10, 0, 5],
      origin_orientation=[0, 1.57, 0],
      rotation=[0, 1, 0],
    ),
    URDFLink(
      name="elbow",
      origin_translation=[25, 0, 0],
      origin_orientation=[0, 0, 0],
      rotation=[0, 1, 0],
    ),
    URDFLink(
      name="wrist",
      origin_translation=[22, 0, 0],
      origin_orientation=[0, 0, 0],
      rotation=[0, 1, 0],
    )
])

Getting the position of your chain (aka Forward Kinematics, aka FK)

Just call the forward_kinematics method of your chain using the positions of each joint.

my_chain.forward_kinematics([0] * 7)

This returns a 4x4 transformation matrix given the position in space and orientation of the tip of your chain. This matrix is in homogeneous coordinates:

  • matrix[:3, :3] (first 3 rows and 3 columns) is the orientation of the end effector
  • matrix[:3, 3] (first 3 rows of the last column) is the position of the end-effector

Setting the position of your chain (aka Inverse Kinematics, aka IK)

For example, with a target position of [2, 2, 2] and an orientation matrix being the identity :

my_chain.inverse_kinematics([2, 2, 2])

Would return :

[  0.00000000e+00,  -7.85169183e-01, -9.71977343e-01, 8.39302626e-01,   7.03536053e-05,   7.31439909e-01,  0.00000000e+00]

To have more information about the Inverse Kinematics options, follow this link.

You can also just pass your frame matrix returned by the forward_kinematics method to inverse_kinematics_frame.

Plotting your chain

You can display your kinematic chain using the plot method of your chain object and passing the position of each joint of your chain.

Here we use the position given by the inverse_kinematics :

import matplotlib.pyplot
from mpl_toolkits.mplot3d import Axes3D
ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')

my_chain.plot(my_chain.inverse_kinematics([2, 2, 2]), ax)
matplotlib.pyplot.show()

For example :

To use advanced plotting functions, such as displaying multiple chains on the same figure, follow this guide.