-
Notifications
You must be signed in to change notification settings - Fork 155
Orientation
In 3D, orientation is not only one vector, but a full referential.
In IKPy, you have two options to define the orientation:
- Doing IK orientation on only one axis, as in the example above.
- Doing IK orientation on a full referential
Let's see the differences with a robot with a hand with a pointing finger:
- By doing orientation on only one axis, you only want your finger to point at a specific direction, but don't care at how the palm is
- By doing orientation on a full referential, you want your finger to point at a specific direction, but also want your palm to be at a specific orientation
Orientation on a single axis is quite straightforward. You need to provide:
- A target unit vector, in the absolute referential
- The axis of your link you want to match that target unit vector
In the example below, we ask Baxter's arm's X axis (so relative to the link) to match the absolute Z axis
# Let's ask baxter to put his left arm's X axis to the absolute Z axis
orientation_axis = "X"
target_orientation = [0, 0, 1]
# Compute the inverse kinematics with position
ik = baxter_left_arm_chain.inverse_kinematics(
target_position=[0.1, 0.5, -0.1],
target_orientation=target_orientation,
orientation_mode=orientation_axis)
We see that the arm's X axis (in green) is aligned with the absolute referential's Z axis (in orange):
In this setting, you just provide a frame to which the link's frame must align with, in the absolute referential.
This frame is a 3x3 orientation matrix (i.e. an orthogonal matrix, i.e. a matrix where all columns norm is one, and all columns are orthogonal)
# Let's ask baxter to put his left arm as
target_orientation = np.eye(3)
# Compute the inverse kinematics with position
ik = baxter_left_arm_chain.inverse_kinematics(
target_position=[0.1, 0.5, -0.1],
target_orientation=target_orientation,
orientation_mode="all")
We see that the arm frame's axes are all aligned with the absolute axes (in green/light blue/orange):
When dealing with orientation, you may also want to set a target position. IKPy can natively manage both orientation and position at the same time.
However, in some difficult cases, reaching a target position and orientation may be difficult, or even impossible.
In these cases, a solution is to cut this problem into two steps:
- Reach the desired position
- From this position, reach the desired orientation
This is what is done in the example below: