Source code for concepts.dm.crowhat.pybullet_interfaces.pybullet_manipulator_interface

#! /usr/bin/env python3
# -*- coding: utf-8 -*-
# File   : pybullet_manipulator_interface.py
# Author : Jiayuan Mao
# Email  : maojiayuan@gmail.com
# Date   : 07/23/2024
#
# This file is part of Project Concepts.
# Distributed under terms of the MIT license.

import numpy as np

from concepts.simulator.pybullet.components.robot_base import BulletArmRobotBase
from concepts.dm.crowhat.manipulation_utils.manipulator_interface import GeneralArmArmMotionPlanningInterface



[docs]
class PyBulletArmRobotInterface(GeneralArmArmMotionPlanningInterface):

[docs]
    def __init__(self, robot: BulletArmRobotBase):
        self._robot = robot
        self._joints = self._robot.get_joint_ids()
        self._joint_limits = self._robot.get_joint_ids()



[docs]
    def get_nr_joints(self) -> int:
        return len(self._joints)



[docs]
    def get_joint_limits(self):
        return self._robot.get_joint_limits()


    def _fk(self, qpos):
        return self._robot.fk(qpos)

    def _ik(self, pos, quat, qpos=None, max_difference=None):
        return self._robot.ikfast(pos, quat, qpos, max_distance=max_difference)

    def _jacobian(self, qpos):
        return self._robot.get_jacobian(qpos)

    def _coriolis(self, qpos: np.ndarray, qvel: np.ndarray) -> np.ndarray:
        return self._robot.get_coriolis_torque(qpos, qvel)