#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""RobotiqHand 用の共通ハンド I/F 引数変換。"""
[docs]
class RobotiqHandInterface(object):
"""共通ハンド I/F の引数を RobotiqHand 用の値へ変換する。
このクラスはハンドの通信や制御を行わない。
共通 I/F の position [mm]、force [N]、speed [%] を、
対応する RobotiqHand が受け取る raw 値へ変換する。
"""
DEFAULT_SPEED_PERCENT = 100.0
DEFAULT_FORCE_RAW = 255
def __init__(
self,
hand,
):
"""対応する RobotiqHand インスタンスを保持する。"""
self._hand = hand
@property
def min_position(self):
"""完全に閉じたときの開口幅 [mm] を返す。"""
return self._hand.MIN_POSITION_MM
@property
def max_position(self):
"""完全に開いたときの開口幅 [mm] を返す。"""
return self._hand.MAX_POSITION_MM
[docs]
def convert(
self,
pos=None,
*,
position=None,
force=None,
direction=True,
relative=False,
speed=None,
):
"""共通 I/F の引数を RobotiqHand 用に一括変換する。
Args:
pos: 開口幅 [mm]。position の互換引数。
position: 開口幅 [mm]。
force: 把持力 [N]。指定時は力による開閉指令として扱う。
direction: force 指定時の方向。
True は閉方向、False は開方向。
relative: 相対位置指定かどうか。
現在は RobotiqHand では未対応。
speed: 動作速度 [%]。
Returns:
RobotiqHand の move() または
move_and_wait_for_position() に渡す辞書。
Raises:
ValueError: 引数または変換範囲が不正な場合。
"""
if relative:
raise ValueError(
"relative operation is not supported by RobotiqHand"
)
raw_speed = self._convert_speed(
speed,
)
raw_force = self._convert_force(
force,
)
if force is not None:
if direction:
raw_position = self._hand.get_closed_position()
else:
raw_position = self._hand.get_open_position()
return {
"position": raw_position,
"speed": raw_speed,
"force": raw_force,
}
if position is not None:
resolved_position = float(position)
elif pos is not None:
resolved_position = float(pos)
else:
raise ValueError(
"position, pos, or force is required"
)
raw_position = self._convert_position(
resolved_position,
)
return {
"position": raw_position,
"speed": raw_speed,
"force": raw_force,
}
[docs]
def _convert_speed(
self,
speed,
):
"""動作速度 [%] を Robotiq raw 値へ変換する。"""
resolved_speed = self.DEFAULT_SPEED_PERCENT
if speed is not None:
resolved_speed = float(speed)
if resolved_speed < 0.0:
resolved_speed = 0.0
if resolved_speed > 100.0:
resolved_speed = 100.0
raw_speed_range = (
self._hand.MAX_SPEED
- self._hand.MIN_SPEED
)
raw_speed = round(
resolved_speed
* raw_speed_range
/ 100.0
+ self._hand.MIN_SPEED
)
return raw_speed
[docs]
def _convert_force(
self,
force,
):
"""把持力 [N] を Robotiq raw 値へ変換する。
force が指定されていない位置指令では、Robotiq の通信上必要な
force 値として内部既定値を使用する。
"""
if force is None:
return self.DEFAULT_FORCE_RAW
resolved_force = float(force)
if resolved_force < self._hand.MIN_FORCE_N:
resolved_force = self._hand.MIN_FORCE_N
if resolved_force > self._hand.MAX_FORCE_N:
resolved_force = self._hand.MAX_FORCE_N
force_range = (
self._hand.MAX_FORCE_N
- self._hand.MIN_FORCE_N
)
if force_range <= 0.0:
raise ValueError(
"invalid Robotiq force range: "
f"min={self._hand.MIN_FORCE_N}, "
f"max={self._hand.MAX_FORCE_N}"
)
raw_force_range = (
self._hand.MAX_FORCE
- self._hand.MIN_FORCE
)
raw_force = round(
(
resolved_force
- self._hand.MIN_FORCE_N
)
* raw_force_range
/ force_range
+ self._hand.MIN_FORCE
)
return raw_force
[docs]
def _convert_position(
self,
position,
):
"""開口幅 [mm] を Robotiq raw 位置へ変換する。"""
resolved_position = float(position)
if resolved_position < self._hand.MIN_POSITION_MM:
resolved_position = self._hand.MIN_POSITION_MM
if resolved_position > self._hand.MAX_POSITION_MM:
resolved_position = self._hand.MAX_POSITION_MM
position_range = (
self._hand.MAX_POSITION_MM
- self._hand.MIN_POSITION_MM
)
if position_range <= 0.0:
raise ValueError(
"invalid Robotiq position range: "
f"min={self._hand.MIN_POSITION_MM}, "
f"max={self._hand.MAX_POSITION_MM}"
)
raw_open_position = self._hand.get_open_position()
raw_closed_position = self._hand.get_closed_position()
raw_position_range = (
raw_closed_position
- raw_open_position
)
if raw_position_range <= 0:
raise ValueError(
"invalid calibrated Robotiq position range: "
f"open={raw_open_position}, "
f"closed={raw_closed_position}"
)
raw_position = round(
raw_closed_position
- (
resolved_position
- self._hand.MIN_POSITION_MM
)
* raw_position_range
/ position_range
)
return raw_position