Source code for concepts.simulator.pybullet.control_utils

#! /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_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