Source code for concepts.hw_interface.franka.remote_client

#! /usr/bin/env python3
# -*- coding: utf-8 -*-
# File   :
# Author : Jiayuan Mao
# Email  :
# Date   : 02/8/2024
# This file is part of Project Concepts.
# Distributed under terms of the MIT license.

import atexit
import itertools
from typing import Optional, Union, List

import numpy as np
import cv2

from concepts.hw_interface.franka.server import FrankaServiceClient
from concepts.simulator.pybullet.rotation_utils import quat_mul, quat_conjugate, rotate_vector

[docs]class FrankaRemoteClient(object):
[docs] def __init__(self, server: str, name='franka-client', port_pair=None, automatic_reset_errors: bool = True): self.service = FrankaServiceClient(server, name, port_pair, automatic_reset_errors) self.service.initialize() atexit.register(self._atexit) self.ikfast_wrapper = None
def _atexit(self): if self.service is not None: self.service.finalize() self.service = None def __del__(self): self._atexit()
[docs] def open_gripper(self) -> None: self.service.open_gripper() self.service.reset_errors()
[docs] def close_gripper(self) -> None: self.service.close_gripper() self.service.reset_errors()
[docs] def grasp(self, width: float = 0.05, force: float = 40): self.service.grasp_gripper(width, force) self.service.reset_errors()
[docs] def visualize_camera_rgb(self): rgb, _ = self.service.capture_image() cv2.imshow('Camera RGB', rgb) cv2.waitKey(0)
[docs] def print_pose(self): pos, quat = self.get_ee_pose() print(f'Actual pose: x={pos[0]:.4f}, y={pos[1]:.4f}, z={pos[2]:.4f}') print(f'Actual quat: x={quat[0]:.4f}, y={quat[1]:.4f}, z={quat[2]:.4f}, w={quat[3]:.4f}')
[docs] def get_ee_pose(self): return self.service.get_ee_pose()
[docs] def get_qpos(self): return self.service.get_qpos()
[docs] def move_home(self): self.service.move_home() self.service.reset_errors()
[docs] def move_qpos(self, qpos: np.ndarray, timeout: float = 10) -> None: self.service.move_qpos(qpos, timeout) self.service.reset_errors()
[docs] def move_pose(self, pos, quat=None, timeout: float = 10): if quat is None: quat = (0, 1, 0, 0) pos = np.array(pos) quat = np.array(quat) last_qpos = self.get_qpos() ik_solution = self.ikfast(pos, quat, last_qpos, error_on_fail=False) if ik_solution is None: print('Failed to find IK.') return False self.move_qpos(ik_solution, timeout) return True
[docs] def move_qpos_trajectory(self, qpos_list, timeout: float = 10) -> None: current_qpos = self.get_qpos() qpos_list = [current_qpos] + qpos_list self.service.move_qpos_trajectory(qpos_list, timeout) self.service.reset_errors()
[docs] def reset_errors(self) -> None: return self.service.reset_errors()
[docs] def set_manual_control(self): self.service.reset_errors() self.service.set_cart_impedance_freemotion() self.service.reset_errors()
[docs] def run_calibration_fix(self): """Helpful function for fixing an object at the current position. The robot will grasp, open, and move home.""" self.grasp() self.open_gripper() self.move_home() self.reset_errors()
def _init_ikfast(self): if self.ikfast_wrapper is None: from concepts.simulator.pybullet.ikfast.ikfast_common import IKFastWrapperBase import concepts.simulator.pybullet.ikfast.franka_panda.ikfast_panda_arm as ikfast_module self.ikfast_wrapper = IKFastWrapperBase( ikfast_module, joint_ids=[0, 1, 2, 3, 4, 5, 6], free_joint_ids=[6], use_xyzw=True )
[docs] def ikfast( self, pos: np.ndarray, quat: np.ndarray, last_qpos: np.ndarray, max_attempts: int = 1000, max_distance: float = float('inf'), error_on_fail: bool = True, ) -> Optional[Union[List[np.ndarray], np.ndarray]]: self._init_ikfast() pos_delta = [0, 0, 0.1] quat_delta = (0.0, 0.0, 0.9238795325108381, 0.38268343236617297) inner_quat = quat_mul(quat, quat_conjugate(quat_delta)) inner_pos = np.array(pos) - rotate_vector(pos_delta, quat) try: ik_solution = list(itertools.islice(self.ikfast_wrapper.gen_ik(inner_pos, inner_quat, last_qpos=last_qpos, max_attempts=max_attempts, max_distance=max_distance, verbose=False), 1))[0] except IndexError: if error_on_fail: raise return None return ik_solution