In this exercise, we will extend the inverse kinematics problem to orientation tracking.
Inverse kinematics consists of finding a configuration (joint angles) that reaches a target pose (position and/or orientation) with the end-effector of the robot. In Exercise 4, we have seen how inverse kinematics can be used to reach a desired target position, without taking into account the orientation.
The goal of this exercise is to implement an inverse kinematics function to track a desired target orientation for the 3-axis manipulator displayed on the right. We would like that the end-effector of this robot orients itself with the same orientation as the pink object (with desired grip depicted in transparency), which can be changed by using the slider below the robot.
The orientation of a planar robot is on a Riemannian manifold $\mathcal{S}^1$, which requires a logarithmic map function (logmap() function in the code) to compute the residual error between the target orientation and the current orientation, represented as a light pink circle in the figure. This residual can be represented in the tangent space of the desired target point (light pink segment tangent to the light pink circle in the figure). This residual vector in pink corresponds to a geodesic distance that needs to be decreased to reach the desired orientation. When projected back to the manifold, the resulting geodesic path in pink corresponds to the shortest distance between the current orientation and the desired orientation while staying on the manifold.
The goal of this exercise is to compare the use of a geodesic distance and a standard Euclidean distance for different initial orientation and desired target orientation.
The function controlCommand(x,param) continuously sends joint velocity commands to the robot. In the initial code, the robot is not moving because u=np.zeros(param.nbVarX) sends zero commands to the robot.
param.Mu contains the target pose in the form of $[\mu^\tp{pos}_1, \mu^\tp{pos}_2, \mu^\tp{ori}]$, fkin(x,param) returns the end-effector pose of the robot in the form of $[f^\tp{pos}_1, f^\tp{pos}_2, f^\tp{ori}]$, and Jkin(x,param) returns a Jacobian matrix of size $3\times3$.
u to reach the target orientation.
Compute the residual either as a geodesic distance or as a Euclidean distance and observe the behavior when the current orientation is close to $\pi$ and the target orientation is close $-\pi$. Note that $\pi$ and $-\pi$ are the same point on the manifold $\mathcal{S}^1$, which is depicted in the figure as a short segment on the left side of the circle. What do you observe? Can you explain the difference in the tracking behaviors?
u to reach the target orientation as a primary task and reaching the object position as a secondary task. You can do this by using nullspace control as presented in Section 5.2 of the RCFS documentation.