﻿ Robotics codes from scratch packages = ['numpy', 'matplotlib']

# Exercise 9Orientation with Riemannian manifold

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$.

• Change this function to send velocity commands 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?
• Change this function to send velocity commands 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.

x = np.array([-0.8, 1.8, 3.0]) # Initial robot state def controlCommand(x, param): f = fkin(x, param) # End-effector position and orientation J = Jkin(x, param) # Jacobian matrix r_ori = logmap(param.Mu[2:], f[2:]) # Orientation residual # r_ori = param.Mu[-1] - f[-1] # Orientation residual (Euclidean distance) u = np.zeros(param.nbVarX) # Implement here return u
x = np.array([-0.8, 1.8, 3.0]) # Initial robot state def controlCommand(x, param): f = fkin(x, param) # End-effector position and orientation J = Jkin(x, param) # Jacobian matrix J_ori = J[2:,:] # Jacobian matrix for orientation r_ori = logmap(param.Mu[2:], f[2:]) # Orientation residual # r_ori = param.Mu[2:] - f[2:] # Orientation residual (as Euclidean distance) u = 0.1 * np.linalg.pinv(J_ori) @ r_ori # Orientation tracking (with step size 0.1) return u
x = np.array([-0.8, 1.8, 3.0]) # Initial robot state def controlCommand(x, param): f = fkin(x, param) # End-effector position and orientation J = Jkin(x, param) # Jacobian matrix J_pos = J[:2,:] # Jacobian matrix for position J_ori = J[2:,:] # Jacobian matrix for orientation r_pos = (param.Mu[:2] - f[:2]) # Position residual r_ori = (param.Mu[2:] - f[2:]) # Orientation residual # Prioritized control (orientation tracking prioritized over position tracking) pinv_J_ori = np.linalg.inv(J_ori.T @ J_ori + np.eye(param.nbVarX) * 1E-2) @ J_ori.T # Damped pseudoinverse N_ori = np.eye(param.nbVarX) - pinv_J_ori @ J_ori # Nullspace projection operator u_ori = 0.1 * pinv_J_ori @ r_ori # Command for position tracking (with step size 0.1) J_pos_N_ori = J_pos @ N_ori pinv_J_pos_N_ori = J_pos_N_ori.T @ np.linalg.inv(J_pos_N_ori @ J_pos_N_ori.T + np.eye(2) * 1E1) # Damped pseudoinverse u_pos = 0.1 * pinv_J_pos_N_ori @ (r_pos - J_pos @ u_ori) # Command for orientation tracking (with position tracking prioritized) u = u_ori + N_ori @ u_pos # Control commands # # Prioritized control (position tracking prioritized over orientation tracking) # pinv_J_pos = np.linalg.inv(J_pos.T @ J_pos + np.eye(param.nbVarX) * 1E-2) @ J_pos.T # Damped pseudoinverse # N_pos = np.eye(param.nbVarX) - pinv_J_pos @ J_pos # Nullspace projection operator # u_pos = 0.1 * pinv_J_pos @ r_pos # Command for position tracking (with step size 0.1) # J_ori_N_pos = J_ori @ N_pos # pinv_J_ori_N_pos = J_ori_N_pos.T @ np.linalg.inv(J_ori_N_pos @ J_ori_N_pos.T + np.eye(1) * 1E1) # Damped pseudoinverse # u_ori = 0.1 * pinv_J_ori_N_pos @ (r_ori - J_ori @ u_pos) # Command for orientation tracking (with position tracking prioritized) # u = u_pos + N_pos @ u_ori # Control commands return u

${\color{#DD1177}\mathcal{S}^1}$ ${\color{#11DD77}\mathbb{R}^1}$ ${\color{#DD1177}\mu^\tp{ori}}$ ${\color{#DD1177}f^\tp{ori}}$ ${\color{#11DD77}\mu^\tp{ori}}$ ${\color{#11DD77}f^\tp{ori}}$ ${\color{#11DD77}-\pi}$ ${\color{#11DD77}\pi}$