#! /usr/bin/env python3
# -*- coding: utf-8 -*-
# File : control_utils.py
# Author : Jiayuan Mao
# Email : maojiayuan@gmail.com
# Date : 01/09/2024
#
# This file is part of Project Concepts.
# Distributed under terms of the MIT license.
import numpy as np
from typing import Optional
from concepts.simulator.pybullet.rotation_utils import quat_diff_in_axis_angle
from concepts.utils.typing_utils import Vec3f, Vec4f
[docs]
def get_default_joint_pd_control_parameters(nr_joints: int):
"""Return the default parameters for the joint-space PD control."""
KP = np.asarray([2000.] * nr_joints)
KD = 2 * np.sqrt(KP)
config = {
'kP': KP,
'kD': KD,
'max_torque': np.asarray([100.] * nr_joints)
}
return config
[docs]
def get_joint_explicit_pd_control_command(q: np.ndarray, dq: np.ndarray, target_q: np.ndarray, target_dq: Optional[np.ndarray] = None, config: Optional[dict] = None):
"""Joint-space PD control. It uses goal joint states from the feedback thread and current robot states from the subscribed messages to compute
joint torques.
Args:
q: current joint positions.
dq: current joint velocities.
target_q: goal joint positions.
target_dq: goal joint velocities.
config: control parameters.
"""
if target_dq is None:
target_dq = np.zeros_like(dq)
if config is None:
config = get_default_joint_pd_control_parameters(q.shape[0])
delta_q = target_q - q
delta_dq = target_dq - dq
# Desired joint torques using PD law
tau = config['kP'].dot(delta_q) + config['kD'].dot(delta_dq)
tau = np.clip(tau, -config['max_torque'], config['max_torque'])
return tau
[docs]
def get_default_os_imp_control_parameters():
"""Return the default parameters for the operation-space impedance control."""
KP_P = np.asarray([2000., 2000., 2000.])
KP_O = np.asarray([100., 100., 100.])
config = {
'P_pos': KP_P,
'D_pos': 2 * np.sqrt(KP_P),
'P_ori': KP_O,
'D_ori': np.asarray([0.01, 0.01, 0.01]),
'error_thresh': np.asarray([0.005, 0.005]),
'start_err': np.asarray([200., 200.])
}
return config
[docs]
def get_os_imp_control_command(curr_pos: Vec3f, curr_quat: Vec4f, target_pos: Vec3f, target_quat: Vec4f, curr_vel: Vec3f, curr_omg: Vec3f, J: np.ndarray, config: Optional[dict] = None):
"""Operation-space impedance control. It uses goal pose from the feedback thread and current robot states from the subscribed messages to compute
task-space force, and then the corresponding joint torques.
Implementation is based on: https://github.com/justagist/pybullet_robot/blob/master/src/pybullet_robot/controllers/os_impedance_ctrl.py
Also reference: https://github.com/NVIDIA-Omniverse/orbit/blob/57e766cf68c942191a74e24269b780ee9a817535/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/operational_space.py#L307
Args:
curr_pos: current end-effector position.
curr_quat: current end-effector orientation.
target_pos: goal end-effector position.
target_quat: goal end-effector orientation.
curr_vel: current end-effector velocity.
curr_omg: current end-effector angular velocity.
J: end-effector Jacobian.
config: control parameters.
Returns:
joint torques.
"""
delta_pos = (target_pos - curr_pos).reshape([3, 1])
delta_ori = quat_diff_in_axis_angle(target_quat, curr_quat).reshape([3, 1])
# print self._goal_pos, curr_pos
# Desired task-space force using PD law
F_p = np.vstack([config['P_pos'].dot(delta_pos), config['P_ori'].dot(delta_ori)])
F_d = np.vstack([config['D_pos'].dot(curr_vel.reshape([3, 1])), config['D_ori'].dot(curr_omg.reshape([3, 1]))])
F = F_p - F_d
return np.dot(J.T, F).flatten()
[docs]
def get_os_imp_control_command_robot(robot, target_pos: Vec3f, target_quat: Vec4f, config: Optional[dict] = None):
"""Alias of get_os_imp_control_command, but with a robot instance as the input.
Args:
robot: robot instance. The robot should have implemented the following methods:
get_ee_pose: return the current end-effector pose.
get_ee_velocity: return the current end-effector velocity.
get_jacobian: return the current end-effector Jacobian.
target_pos: goal end-effector position.
target_quat: goal end-effector orientation.
config: control parameters.
"""
curr_pos, curr_quat = robot.get_ee_pose()
curr_vel, curr_omg = robot.get_ee_velocity()
J = robot.get_jacobian()
return get_os_imp_control_command(curr_pos, curr_quat, target_pos, target_quat, curr_vel, curr_omg, J, config)
[docs]
def get_default_joint_imp_control_parameters():
"""Return the default parameters for the joint-space impedance control."""
KP = np.asarray([2000., 2000., 2000., 2000., 2000., 2000., 2000.])
KD = 2 * np.sqrt(KP)
config = {
'P_joint': KP,
'D_joint': KD,
}
return config
[docs]
def get_joint_imp_control_command(q: np.ndarray, dq: np.ndarray, target_q: np.ndarray, target_dq: Optional[np.ndarray] = None, config: Optional[dict] = None):
"""Joint-space impedance control. It uses goal joint states from the feedback thread and current robot states from the subscribed messages to compute
joint torques.
Implementation is based on a simplified version of:
https://github.com/NVIDIA-Omniverse/orbit/blob/57e766cf68c942191a74e24269b780ee9a817535/source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/joint_impedance.py
Args:
q: current joint positions.
dq: current joint velocities.
target_q: goal joint positions.
target_dq: goal joint velocities.
config: control parameters.
"""
if target_dq is None:
target_dq = np.zeros_like(dq)
if config is None:
config = get_default_joint_imp_control_parameters()
delta_q = target_q - q
delta_dq = target_dq - dq
# Desired joint torques using PD law
tau_p = config['P_joint'].dot(delta_q)
tau_d = config['D_joint'].dot(delta_dq)
tau = tau_p + tau_d
return tau