# Copyright (c) 2022-2024, The ORBIT Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from __future__ import annotations
import torch
from typing import TYPE_CHECKING
from omni.isaac.orbit.utils.math import apply_delta_pose, compute_pose_error
if TYPE_CHECKING:
from .differential_ik_cfg import DifferentialIKControllerCfg
[docs]class DifferentialIKController:
r"""Differential inverse kinematics (IK) controller.
This controller is based on the concept of differential inverse kinematics [1, 2] which is a method for computing
the change in joint positions that yields the desired change in pose.
.. math::
\Delta \mathbf{q} = \mathbf{J}^{\dagger} \Delta \mathbf{x}
\mathbf{q}_{\text{desired}} = \mathbf{q}_{\text{current}} + \Delta \mathbf{q}
where :math:`\mathbf{J}^{\dagger}` is the pseudo-inverse of the Jacobian matrix :math:`\mathbf{J}`,
:math:`\Delta \mathbf{x}` is the desired change in pose, and :math:`\mathbf{q}_{\text{current}}`
is the current joint positions.
To deal with singularity in Jacobian, the following methods are supported for computing inverse of the Jacobian:
- "pinv": Moore-Penrose pseudo-inverse
- "svd": Adaptive singular-value decomposition (SVD)
- "trans": Transpose of matrix
- "dls": Damped version of Moore-Penrose pseudo-inverse (also called Levenberg-Marquardt)
.. caution::
The controller does not assume anything about the frames of the current and desired end-effector pose,
or the joint-space velocities. It is up to the user to ensure that these quantities are given
in the correct format.
Reference:
[1] https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf
[2] https://www.cs.cmu.edu/~15464-s13/lectures/lecture6/iksurvey.pdf
"""
[docs] def __init__(self, cfg: DifferentialIKControllerCfg, num_envs: int, device: str):
"""Initialize the controller.
Args:
cfg: The configuration for the controller.
num_envs: The number of environments.
device: The device to use for computations.
"""
# store inputs
self.cfg = cfg
self.num_envs = num_envs
self._device = device
# create buffers
self.ee_pos_des = torch.zeros(self.num_envs, 3, device=self._device)
self.ee_quat_des = torch.zeros(self.num_envs, 4, device=self._device)
# -- input command
self._command = torch.zeros(self.num_envs, self.action_dim, device=self._device)
"""
Properties.
"""
@property
def action_dim(self) -> int:
"""Dimension of the controller's input command."""
if self.cfg.command_type == "position":
return 3 # (x, y, z)
elif self.cfg.command_type == "pose" and self.cfg.use_relative_mode:
return 6 # (dx, dy, dz, droll, dpitch, dyaw)
else:
return 7 # (x, y, z, qw, qx, qy, qz)
"""
Operations.
"""
[docs] def reset(self, env_ids: torch.Tensor = None):
"""Reset the internals.
Args:
env_ids: The environment indices to reset. If None, then all environments are reset.
"""
pass
[docs] def set_command(
self, command: torch.Tensor, ee_pos: torch.Tensor | None = None, ee_quat: torch.Tensor | None = None
):
"""Set target end-effector pose command.
Based on the configured command type and relative mode, the method computes the desired end-effector pose.
It is up to the user to ensure that the command is given in the correct frame. The method only
applies the relative mode if the command type is ``position_rel`` or ``pose_rel``.
Args:
command: The input command in shape (N, 3) or (N, 6) or (N, 7).
ee_pos: The current end-effector position in shape (N, 3).
This is only needed if the command type is ``position_rel`` or ``pose_rel``.
ee_quat: The current end-effector orientation (w, x, y, z) in shape (N, 4).
This is only needed if the command type is ``position_*`` or ``pose_rel``.
Raises:
ValueError: If the command type is ``position_*`` and :attr:`ee_quat` is None.
ValueError: If the command type is ``position_rel`` and :attr:`ee_pos` is None.
ValueError: If the command type is ``pose_rel`` and either :attr:`ee_pos` or :attr:`ee_quat` is None.
"""
# store command
self._command[:] = command
# compute the desired end-effector pose
if self.cfg.command_type == "position":
# we need end-effector orientation even though we are in position mode
# this is only needed for display purposes
if ee_quat is None:
raise ValueError("End-effector orientation can not be None for `position_*` command type!")
# compute targets
if self.cfg.use_relative_mode:
if ee_pos is None:
raise ValueError("End-effector position can not be None for `position_rel` command type!")
self.ee_pos_des[:] = ee_pos + self._command
self.ee_quat_des[:] = ee_quat
else:
self.ee_pos_des[:] = self._command
self.ee_quat_des[:] = ee_quat
else:
# compute targets
if self.cfg.use_relative_mode:
if ee_pos is None or ee_quat is None:
raise ValueError(
"Neither end-effector position nor orientation can be None for `pose_rel` command type!"
)
self.ee_pos_des, self.ee_quat_des = apply_delta_pose(ee_pos, ee_quat, self._command)
else:
self.ee_pos_des = self._command[:, 0:3]
self.ee_quat_des = self._command[:, 3:7]
[docs] def compute(
self, ee_pos: torch.Tensor, ee_quat: torch.Tensor, jacobian: torch.Tensor, joint_pos: torch.Tensor
) -> torch.Tensor:
"""Computes the target joint positions that will yield the desired end effector pose.
Args:
ee_pos: The current end-effector position in shape (N, 3).
ee_quat: The current end-effector orientation in shape (N, 4).
jacobian: The geometric jacobian matrix in shape (N, 6, num_joints).
joint_pos: The current joint positions in shape (N, num_joints).
Returns:
The target joint positions commands in shape (N, num_joints).
"""
# compute the delta in joint-space
if "position" in self.cfg.command_type:
position_error = self.ee_pos_des - ee_pos
jacobian_pos = jacobian[:, 0:3]
delta_joint_pos = self._compute_delta_joint_pos(delta_pose=position_error, jacobian=jacobian_pos)
else:
position_error, axis_angle_error = compute_pose_error(
ee_pos, ee_quat, self.ee_pos_des, self.ee_quat_des, rot_error_type="axis_angle"
)
pose_error = torch.cat((position_error, axis_angle_error), dim=1)
delta_joint_pos = self._compute_delta_joint_pos(delta_pose=pose_error, jacobian=jacobian)
# return the desired joint positions
return joint_pos + delta_joint_pos
"""
Helper functions.
"""
def _compute_delta_joint_pos(self, delta_pose: torch.Tensor, jacobian: torch.Tensor) -> torch.Tensor:
"""Computes the change in joint position that yields the desired change in pose.
The method uses the Jacobian mapping from joint-space velocities to end-effector velocities
to compute the delta-change in the joint-space that moves the robot closer to a desired
end-effector position.
Args:
delta_pose: The desired delta pose in shape (N, 3) or (N, 6).
jacobian: The geometric jacobian matrix in shape (N, 3, num_joints) or (N, 6, num_joints).
Returns:
The desired delta in joint space. Shape is (N, num-jointsß).
"""
if self.cfg.ik_params is None:
raise RuntimeError(f"Inverse-kinematics parameters for method '{self.cfg.ik_method}' is not defined!")
# compute the delta in joint-space
if self.cfg.ik_method == "pinv": # Jacobian pseudo-inverse
# parameters
k_val = self.cfg.ik_params["k_val"]
# computation
jacobian_pinv = torch.linalg.pinv(jacobian)
delta_joint_pos = k_val * jacobian_pinv @ delta_pose.unsqueeze(-1)
delta_joint_pos = delta_joint_pos.squeeze(-1)
elif self.cfg.ik_method == "svd": # adaptive SVD
# parameters
k_val = self.cfg.ik_params["k_val"]
min_singular_value = self.cfg.ik_params["min_singular_value"]
# computation
# U: 6xd, S: dxd, V: d x num-joint
U, S, Vh = torch.linalg.svd(jacobian)
S_inv = 1.0 / S
S_inv = torch.where(S > min_singular_value, S_inv, torch.zeros_like(S_inv))
jacobian_pinv = (
torch.transpose(Vh, dim0=1, dim1=2)[:, :, :6]
@ torch.diag_embed(S_inv)
@ torch.transpose(U, dim0=1, dim1=2)
)
delta_joint_pos = k_val * jacobian_pinv @ delta_pose.unsqueeze(-1)
delta_joint_pos = delta_joint_pos.squeeze(-1)
elif self.cfg.ik_method == "trans": # Jacobian transpose
# parameters
k_val = self.cfg.ik_params["k_val"]
# computation
jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2)
delta_joint_pos = k_val * jacobian_T @ delta_pose.unsqueeze(-1)
delta_joint_pos = delta_joint_pos.squeeze(-1)
elif self.cfg.ik_method == "dls": # damped least squares
# parameters
lambda_val = self.cfg.ik_params["lambda_val"]
# computation
jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2)
lambda_matrix = (lambda_val**2) * torch.eye(n=jacobian.shape[1], device=self._device)
delta_joint_pos = (
jacobian_T @ torch.inverse(jacobian @ jacobian_T + lambda_matrix) @ delta_pose.unsqueeze(-1)
)
delta_joint_pos = delta_joint_pos.squeeze(-1)
else:
raise ValueError(f"Unsupported inverse-kinematics method: {self.cfg.ik_method}")
return delta_joint_pos
