# robot_support.py
# copyright 2022- https://robot-creation-diary.com/
# 20230115 get_normal get_place でベクトルのリストを渡していたのを3つのベクトルを渡すように変更
import numpy as np
import quaternion
import time
if __name__ == '__main__':
from axis_angle import AxisAngle as AxisAngle
from serial_support import SerialSuppot as SerialSuppot
else:
from hmdcontroller.axis_angle import AxisAngle as AxisAngle
import copy
ERROR_NO_ERROR = 0
ERROR_ARCCOS_OVER_1 = 1
RIGHT_ARM = 0
LEFT_ARM = 1
ELBOW_LOW = 0
ELBOW_HEIGH = 1
class RobotSupport():
def __init__(self):
self.hand_zero = np.array([0, 0, 0, 0])
self.hand_pos = np.array([0, 0, 0, 0])
self.hand_quat = np.quaternion(1.0, 0, 0, 0)
self.hand_plane = [0, 0, 0, 0] # abcd
self.shoulder_pos = [0, 0, 0]
self.hand_calibration_y_angle = 0
self.arm_length1 = 125
self.arm_length2 = 75
self.magnification = 1
self.handposition_calibrate_aa = AxisAngle()
self.calibration_stage = 0
self.calibration_shoulder = [0, 0, 0]
self.cal_pos1 = [0, 0, 0] # caliblation position 1
self.cal_pos2 = [0, 0, 0] # caliblation position 1
self.calibration_head_positoin = [0, 0, 0]
self.calibratoin_quaternion_aa = AxisAngle([1.0, 0, 0, 0])
self.debug_on = False
self.__pre_wrist_axis_angles = [0, 0, 0] # 手首計算に使う。一回前の各軸の角度。
self.last_error = ERROR_NO_ERROR
self.calibration_head_euler = [0, 0, 0]
# 領域制限
self.arm_left_right = None
self.elbow_height = None
self.wrist_no_entry_area_box = None
def set_arm_no_entry_area(self, arm_left_right,
elbow_height, wrist_no_entry_area_box):
self.arm_left_right = arm_left_right
self.elbow_height = elbow_height
self.wrist_no_entry_area_box = wrist_no_entry_area_box
# z奥が正から手前が正に変更
self.wrist_no_entry_area_box[0][2] *= -1
self.wrist_no_entry_area_box[1][2] *= -1
def get_last_error(self):
return self.last_error
def get_plane(self, vec1, vec2, vec3):
"""
平面の式のパラメターを得る
"""
xy = np.array(vec2)-np.array(vec1)
xz = np.array(vec3)-np.array(vec1)
c = np.cross(xy, xz)
result = np.array([c[0], c[1], c[2],
(vec1[0] * c[0]
+ vec1[1] * c[1]
+ vec1[2] * c[2]) * -1])
return result
def __get_vectors_length(self, vec1, vec2):
"""
ベクトルの大きさを得る
"""
return np.sqrt(np.dot(np.array(vec1) - np.array(vec2),
np.array(vec1)-np.array(vec2)))
def __get_vectors_deff_dot(self, vec1, vec2):
"""
二つのベクトルの差を取った後の内積・大きさを比べる時平方根を撮らないことで時間短縮
"""
return np.dot(np.array(vec1) - np.array(vec2),
np.array(vec1) - np.array(vec2))
def get_angle(self, vec0, vec1, vec2):
"""
vec0を頂点とした3点の角度を得る
"""
v01 = np.array(vec1) - np.array(vec0)
v02 = np.array(vec2) - np.array(vec0)
vd = np.dot(v01, v02)
vd /= np.linalg.norm(v01) * np.linalg.norm(v02)
return np.arccos(vd)
def get_normal(self, vec1, vec2, vec3):
"""
3点を頂点とする三角形の法線を得る
"""
xy = np.array(vec2)-np.array(vec1)
xz = np.array(vec3)-np.array(vec1)
cross = np.cross(xy, xz)
if self.check_float(0, np.sqrt(np.dot(cross, cross))):
print("cross/np.sqrt(np.dot(cross, cross)) error",
vec1, vec2, vec3, cross, cross)
return None
else:
normal = cross/np.sqrt(np.dot(cross, cross))
# 法線は最も軸成分が大きい軸が正になるように整える。
if np.fabs(normal[1]) < np.fabs(normal[0]) and\
np.fabs(normal[2]) < np.fabs(normal[0]):
if normal[0] < 0:
normal *= -1
elif np.fabs(normal[0]) < np.fabs(normal[1]) and\
np.fabs(normal[2]) < np.fabs(normal[1]):
if normal[1] < 0:
normal *= -1
elif np.fabs(normal[1]) < np.fabs(normal[2]) and\
np.fabs(normal[0]) < np.fabs(normal[2]):
# ｚ軸はマイナス方向をメインとする。
if normal[2] < 0:
normal *= -1
return normal
def set_magnification(self, vec):
"""
操作者の腕の長さと、ロボットの腕の長さの比率から位置合わせの伸縮倍率を設定
"""
self.magnification = (self.arm_length1 + self.arm_length2) /\
np.sqrt(np.dot(np.array(vec), np.array(vec)))
def check_float(self, n1, n2):
"""
floatの値が等しいか、誤差を考慮して判断
"""
if ((n1 - 0.000001 < n2) and (n2 < n1 + 0.000001)):
return True
return False
def check_float_axis(self, a1, a2):
"""
floatで表された軸が同じか判断
"""
if (self.check_float(a1[0], a2[0]) and
self.check_float(a1[2], a2[2]) and
self.check_float(a1[2], a2[2])):
return True
return False
def check_float_list(self, l1, l2):
"""
floatリストの値が全て同じか判断
"""
if (len(l1) == len(l2)):
count = 0
for d1, d2 in zip(l1, l2):
if (self.check_float(d1, d2)):
count += 1
if len(l1) == count:
return True
return False
# ############################################202406追加
#
def get_near_surface_point(self, box, point, left_right):
a_box = [[min(box[0][0], box[1][0]),
min(box[0][1], box[1][1]),
min(box[0][2], box[1][2])],
[max(box[0][0], box[1][0]),
max(box[0][1], box[1][1]),
max(box[0][2], box[1][2])]]
# ボックス内にあるか判定
if a_box[0][0] < point[0] and point[0] < a_box[1][0] and\
a_box[0][1] < point[1] and point[1] < a_box[1][1] and\
a_box[0][2] < point[2] and point[2] < a_box[1][2]:
# ボックス内にある
if left_right:
# 左
x = np.fabs(a_box[0][0] - point[0])
else:
# 右
x = np.fabs(a_box[1][0] - point[0])
y = np.fabs(a_box[1][1] - point[0])
z = np.fabs(a_box[0][2] - point[0])
check_point = [x, y, z]
min_index = check_point.index(min(check_point))
result = point
if 0 == min_index:
if LEFT_ARM == left_right:
# 左
result[0] = a_box[0][0]
else:
# 右
result[0] = a_box[1][0]
elif 1 == min_index:
# 高さは下ではなく上にそろえる
# 奥行きは奥（遠いほう）にそろえる。
result[min_index] = a_box[1][min_index]
elif 2 == min_index:
result[min_index] = a_box[0][min_index]
return result
else:
# box内にないときはNone
return None
# 肘の位置矯正使う終了
# ############################################202406追加終了
def tow_link_ik(self, goal, ELBOW_HEIGH_AND_LOW=ELBOW_LOW):
"""
2リンク問題を解く
"""
# 反時計回りが正 クウォータニオンの計算が時計回りが正なので出力反転
# 2リンク問題を解く
if 1 < np.fabs((goal[0] * goal[0] +
goal[1] * goal[1] +
self.arm_length1 * self.arm_length1 -
self.arm_length2 * self.arm_length2) /
(2 * self.arm_length1 *
np.sqrt(goal[0] * goal[0] +
goal[1] * goal[1]))):
print("error: arccos over 1")
self.last_error = ERROR_ARCCOS_OVER_1
return None, None
self.last_error = ERROR_NO_ERROR
acos_buf = np.arccos((goal[0] * goal[0] +
goal[1] * goal[1] +
self.arm_length1 * self.arm_length1 -
self.arm_length2 * self.arm_length2) /
(2 * self.arm_length1 *
np.sqrt(goal[0] * goal[0] +
goal[1] * goal[1])))
atan_buf = np.arctan2(goal[1], goal[0])
if ELBOW_LOW == ELBOW_HEIGH_AND_LOW:
# 小さいほうを使う 0<acos なので -のほうを使う
angle1 = -acos_buf + atan_buf
else:
# 大きいほうを使う acos<0 なので +のほうを使う
angle1 = acos_buf + atan_buf
angle2 = np.arctan2(goal[1]-self.arm_length1*np.sin(angle1),
goal[0]-self.arm_length1*np.cos(angle1)) - angle1
return angle1, angle2
def four_link_ik(self, goal_vec):
"""
4リンク問題を解く
肩から手首までの四リンクの逆運動学を解く
"""
# 肩、ひじ、手首が上面図で一直線上になるように位置決めする。
# 手首の位置で、y軸周りの回転を取得
# 肩が原点
vec_buf = copy.deepcopy(goal_vec)
vec_buf[1] = 0
# print("goal_vec",goal_vec)
yangle = self.get_axis_points_angle([0, 1, 0], [1, 0, 0], vec_buf)
yaa = AxisAngle([0, 1, 0], -yangle)
goal_buf = yaa.get_rotate_vector(goal_vec)
# 2リンク問題を解く
angle1, angle2 = self.tow_link_ik([goal_buf[0], goal_buf[1]], ELBOW_HEIGH)
# angle1, angle2 = self.tow_link_ik([goal_buf[0], goal_buf[1]], ELBOW_LOW)
if angle1 is None and angle2 is None:
return None, None, None, None, None
# 2リンク問題を解く終了
# 2リンクの結果で肘と手首の位置を計算最終的な肘と手首の位置を求める。
elbow_position, wrist_position, wristaa =\
self.__get_xy_elbow_wrist_position(angle1, angle2)
# zy平面にしたときのyの分だけ回転する。
iyaa = AxisAngle([0, 1, 0], yangle)
elbow_position = iyaa.get_rotate_vector(elbow_position)
wrist_position = iyaa.get_rotate_vector(wrist_position)
# ############################################202406追加
# 手首が指定の領域に入らないようにする。胴体にぶつからないようにする
if None is not self.wrist_no_entry_area_box:
w_buf = self.get_near_surface_point(self.wrist_no_entry_area_box, wrist_position, self.arm_left_right)
if None is not w_buf:
wrist_position = w_buf
# ############################################
# 肩の回転角を取得
shoulder_normal = self.get_normal([0, 0, 0],
[0, 0, -1],
elbow_position)
if None is shoulder_normal:
shoulder_angle = 0
else:
shoulder_angle = self.get_angle([0, 0, 0],
[0, 0, -1],
elbow_position)
# 肩の回転角から肘の位置を計算
shoulder_aa1 = AxisAngle(shoulder_normal, shoulder_angle)
posi1 = shoulder_aa1.get_rotate_vector([0, 0, -self.arm_length1])
shoulder_angle = self.get_angle_sine(shoulder_normal,
shoulder_angle,
posi1,
elbow_position)
shoulder_aa = AxisAngle(shoulder_normal, shoulder_angle)
# 各ロボットのモータ間接に合わせて回転角を変換
# 肩を2軸の回転角として取得
shoulder_xangle, shoulder_yangle =\
self.get_shoulder_angles(elbow_position, shoulder_aa)
# 肩2つの角度と肘関節の角度を用いで肘と手首の位置を求める。手首の位置がずれている場合腕回りの回転を求める。
elbow_position2, wrist_position2, wrist_aa2 =\
self.__get_check_wrist_position(shoulder_xangle,
shoulder_yangle,
angle2)
arm_angle = self.get_axis_points_angle(elbow_position2,
wrist_position2,
wrist_position)
arm_angle = self.get_angle_sine(elbow_position2,
arm_angle,
wrist_position2,
wrist_position)
arm_aa = AxisAngle(elbow_position2, arm_angle)
wrist_aa2 += arm_aa
# 肩上下回転, 肩片脇開閉、 腕回転 肘関節 手首回転角
return shoulder_xangle, shoulder_yangle, arm_angle, angle2, wrist_aa2
def seven_link_ik(self, position, quat):
"""
7リンク問題を解く
"""
# 位置や角度をキャリブレーション
if isinstance(quat, quaternion.quaternion):
quat = quat
else:
quat = quaternion.quaternion(quat[0], quat[1], quat[2], quat[3])
goal_vec = self.get_calibrated_position(position)
goal_quat = self.get_calibrated_quaternion(quat)
#print("goal_vec", goal_vec)
# 手首までの4リンク問題を解く
shoulder_up_down, shoulder_open_close, arm_roteta, elbow, wristaa =\
self.four_link_ik(goal_vec)
if shoulder_up_down is None:
# arccoss over 1 の時
result = np.array([np.NaN,
np.NaN,
np.NaN,
np.NaN,
np.NaN,
np.NaN,
np.NaN])
return result
goalaa = AxisAngle(goal_quat)
wrist_angles = self._get_wrist_angles(wristaa, goalaa)
result = np.array([shoulder_up_down,
shoulder_open_close,
arm_roteta,
elbow])
result = np.hstack([result, wrist_angles])
return result
def get_shoulder_angles(self, elbow_position, shoulder_aa):
"""
肩の軸角度と肘の位置から、x,y軸周りの回転角を得る
"""
# 肩の回転角と同じ姿勢になる角度を得る。
xa = np.array([1, 0, 0])
ya = np.array([0, 1, 0])
za = np.array([0, 0, -1])
yaa = AxisAngle(ya, 0)
yaa += shoulder_aa
xaa = AxisAngle(xa, 0)
xaa += shoulder_aa
# 現在のAxixsAngleで回転させる
elbow_position2 = copy.deepcopy(elbow_position)
# x軸周りの回転を調べその後 x軸を回転させた後のz軸での回転角を調べる
elbow_position2[0] = 0
xangle = self.get_axis_points_angle([1, 0, 0],
[0, 0, -1],
elbow_position2)
# x回転後の軸周りの回転をさせる
xaa = AxisAngle([1, 0, 0], xangle)
elbow_position2 = copy.deepcopy(elbow_position)
ya2 = xaa.get_rotate_vector(ya)
za2 = xaa.get_rotate_vector(za)
yangle = self.get_axis_points_angle(ya2, za2, elbow_position2)
# 以下デバッグ処理
# xaa = AxisAngle([1, 0, 0], xangle)
yaa = AxisAngle(ya2, yangle)
elbow_position2 = [0, 0, -self.arm_length1]
elbow_position2 = xaa.get_rotate_vector(elbow_position2)
elbow_position2 = yaa.get_rotate_vector(elbow_position2)
# -------------------------------------------
# 出力は準運動学
return xangle, yangle
def get_calibrated_quaternion(self, quat):
"""
座標系変換されたコントローラーのクォータニオンを入れるとキャリブレーションされたクォータニオンを得る
"""
return self.calibratoin_quaternion_aa.get_rotate_quaternion(quat)
def get_calibrated_position(self, position):
"""
座標系変換されたコントローラーの位置情報からキャリブレーションされた位置情報を得る
"""
position_buf = position - self.__get_shoulder_position()
rpos = self.handposition_calibrate_aa.get_rotate_vector(position_buf)
rpos2 = self.calibratoin_quaternion_aa.get_rotate_vector(position_buf)
rpos *= self.magnification
rpos2 *= self.magnification
# 腕の長さを超える位置にならないようにする
if (self.arm_length1 + self.arm_length2 < np.sqrt(np.dot(rpos, rpos))):
rpos *= ((self.arm_length1 + self.arm_length2) /
np.sqrt(np.dot(rpos, rpos))) - 0.00001
return rpos
def get_calibration_head_euler(self, euler):
return [euler[0], euler[1] - self.calibration_head_euler[1], euler[0]]
def calibrate_head_euler(self, euler):
self.calibration_head_euler = euler
def calibrate_hand_quaternion(self, quat):
"""
キャリブレーション用のクォータニオンを設定
"""
self.calibratoin_quaternion_aa =\
AxisAngle([1.0, 0, 0, 0]) - AxisAngle(quat)
def calibrate_hand_positon(self, head_position, pos1, pos2):
"""
位置をキャリブレーションする情報を設定
"""
if 0 == self.calibration_stage:
if pos1 is not None:
self.cal_pos1 = pos1
self.calibration_stage += 1
if 1 == self.calibration_stage:
if pos2 is not None:
self.cal_pos2 = pos2
self.calibration_head_positoin = head_position
if head_position is not None:
self.__calibrate_hand_positon_main(head_position,
self.cal_pos1,
self.cal_pos2)
self.calibration_stage += 1
def get_calibration_stage(self):
"""
現在キャリブレーションのどの段階にあるのかを得る
"""
return self.calibration_stage
def set_calibration_stage(self, stage):
"""
現在キャリブレーションのどの段階にあるのかを設定する
"""
self.calibration_stage = stage
def __get_shoulder_position(self):
"""
すでに計算した肩の位置を取得
"""
return self.shoulder_pos
def __calibrate_hand_positon_main(self, head_position, pos1, pos2):
"""
位置のキャリブレーション情報を作成するためのメイン関数:calibrate_hand_positonから呼び出している
"""
# pos1手を前に伸ばした時の位置
# pos2肘を90度まげて脇につけた時の位置
# 腕の長さを求める
# len1 = pos1[1] - pos2[1] #上腕
# len2 = np.sqrt( (pos2[0] - head_position[0]) *
# (pos2[0] - head_position[0]) \
# + (pos2[2] - head_position[2]) *
# (pos2[2] - head_position[2]))
# arm_len = (len1 + len2)
# うでのラインと頭の位置の垂線の交点から、肩の位置を求める
arm_line = self.__get_2points_line([pos1[0], pos1[2]],
[pos2[0], pos2[2]])
self.shoulder_pos = self.__get_2dline_crros_point(arm_line,
[head_position[0],
head_position[2]],
[pos1[0], pos1[2]])
self.shoulder_pos.append(self.shoulder_pos[1])
self.shoulder_pos[1] = pos1[1]
self.shoulder_pos = np.array(self.shoulder_pos)
pos1 = np.array(pos1)
pos2 = np.array(pos2)
# 肩の位置を中心にpos1がz-1に重なる回転角を得る。
pos1_buf1 = pos1 - self.shoulder_pos
angle1 = self.get_axis_points_angle([0, 1, 0], pos1_buf1, [0, 0, -1])
rotateaa1 = AxisAngle([0, 1, 0], angle1)
pos1_buf2 = rotateaa1.get_rotate_vector(pos1_buf1)
angle2 = self.get_axis_points_angle([1, 0, 0], pos1_buf2, [0, 0, -1])
rotateaa2 = AxisAngle([1, 0, 0], angle2)
self.handposition_calibrate_aa = rotateaa1 + rotateaa2
self.magnification = np.fabs(self.arm_length1 + self.arm_length2) /\
np.sqrt(np.dot(pos1-self.shoulder_pos, pos1-self.shoulder_pos))
# 肩を原点としたときpos1が z軸と重なればよい
# x,z軸周りについては考える必要なし
def __get_2points_line(self, point1, point2):
"""
2Dの2点からなる線の式のパラメータを取得
"""
if self.check_float((point2[0] - point1[0]), 0):
return [None, None]
else:
a = (point2[1] - point1[1])/(point2[0] - point1[0])
b = -a * point1[0] + point1[1]
return [a, b]
def __get_2dline_crros_point(self, line, point, linepoint):
"""
2Dの2つの線分の交点を求める
"""
if line[0] is None:
return [linepoint[0], point[1]]
else:
x = (line[0]*(point[1] - line[1]) + point[0]) /\
(line[0]*line[0] + 1)
y = line[0]*x + line[1]
return [x, y]
def set_hand_pos(self, pos):
"""
肩の位置を原点とする手の位置を求める
"""
self.hand_pos = [pos[0] - self.shoulder_pos[0],
pos[1] - self.shoulder_pos[0],
pos[2] - self.shoulder_pos[2]]
yaa = AxisAngle([0, 1, 0], self.hand_calibration_y_angle)
self.hand_pos = yaa.get_rotate_vector(self.hand_pos)
def __get_xy_elbow_wrist_position(self, angle1, angle2):
"""
2リンク問題で求めた肩と肘の角度を使って、xy平面上の肘の位置と肩の位置を求める。またその時の手首の軸角度も求める
"""
# クウォータニオン
wristaa = AxisAngle(np.quaternion(1, 0, 0, 0))
elbow_position = [self.arm_length1, 0, 0]
wrist_position = [self.arm_length2, 0, 0]
saa = AxisAngle([0, 0, 1], angle1)
wristaa += saa
# 仮想手首の位置、肘周りの回転軸を得るのに使う
elbow_position = saa.get_rotate_vector(elbow_position)
wrist_position = saa.get_rotate_vector(wrist_position)
# 肘周りの回転
elbowaa = AxisAngle([0, 0, 1], angle2)
wristaa += elbowaa
wrist_position = elbowaa.get_rotate_vector(wrist_position)
wrist_position += elbow_position
return elbow_position, wrist_position, wristaa
def __get_check_wrist_position(self, shoulder_updown_angle,
shoulder_openclose_angle, elbow_angle):
"""
肩2つの角度と肘関節の角度を用いで肘と手首の位置を求める。
"""
elbow_position = [0, 0, -self.arm_length1]
wrist_position = [0, 0, -self.arm_length2]
exbow_axis = [1, 0, 0]
ya = [0, 1, 0]
wristaa = AxisAngle(np.quaternion(1, 0, 0, 0))
shoulder_ud_aa = AxisAngle([1, 0, 0], shoulder_updown_angle)
# 以下位置と回転オブジェクトを連鎖的に計算
elbow_position = shoulder_ud_aa.get_rotate_vector(elbow_position)
wrist_position = shoulder_ud_aa.get_rotate_vector(wrist_position)
exbow_axis = shoulder_ud_aa.get_rotate_vector(exbow_axis)
ya = shoulder_ud_aa.get_rotate_vector(ya)
wristaa += shoulder_ud_aa
shoulder_oc_aa = AxisAngle(ya, shoulder_openclose_angle)
elbow_position = shoulder_oc_aa.get_rotate_vector(elbow_position)
wrist_position = shoulder_oc_aa.get_rotate_vector(wrist_position)
exbow_axis = shoulder_oc_aa.get_rotate_vector(exbow_axis)
wristaa += shoulder_oc_aa
elbow_aa = AxisAngle(exbow_axis, elbow_angle)
wrist_position = elbow_aa.get_rotate_vector(wrist_position)
wrist_position += elbow_position
wristaa += elbow_aa
return elbow_position, wrist_position, wristaa
def _get_wrist_angles(self, wrist_axis_angle, goal_axis_angles):
"""
手首の3軸の回転角を得る
axis_angleを
a1,a2,a3,の軸（ベクトル）回りの回転で表すときの各軸周りの角度を求める。
"""
xa = np.array([1, 0, 0])
ya = np.array([0, 1, 0])
za = np.array([0, 0, -1])
iza = np.array([0, 0, -1])
postures = [np.array([1, 0, 0]),
np.array([0, 1, 0]),
np.array([0, 0, -1])]
axis_list = [xa, ya, iza]
oder_axis_index_list = [0, 0, 0]
# 現在のクウォータニオンで回転させる
goal_xa_vec = goal_axis_angles.get_rotate_vector(xa)
goal_ya_vec = goal_axis_angles.get_rotate_vector(ya)
goal_za_vec = goal_axis_angles.get_rotate_vector(za)
goal_vec_list = [goal_xa_vec, goal_ya_vec, goal_za_vec]
result_angle_list = [0, 0, 0]
for i in range(3):
axis_list[i] = wrist_axis_angle.get_rotate_vector(axis_list[i])
postures[i] = wrist_axis_angle.get_rotate_vector(postures[i])
# 回転軸順番決定手順
# y軸を合わせるためにx軸z軸周りの回転を行う。
# 前回の回転角から変位量の少ないほうを優先して最初の回転軸とする。
# 2回目の回転は、操作対象べく鳥でないほうを軸ベクトルにして操作対象ベクトルを合わせる。
# 3回目の回転は。残りの軸を回転軸にする。最初の軸を操作対象ベクトルにする。
# ------------------------------------------------------------
# y軸を合わせるためにx軸z軸周りの回転を行う。
# 前回の回転角から変位量の少ないほうを優先して最初の回転軸とする。
angle_y_1 = self.get_axis_points_angle(axis_list[1],
postures[2],
goal_vec_list[2])
angle_y_2 = self.get_three_points_angle([0, 0, 0],
postures[1],
goal_vec_list[1])
angle_x_1 = self.get_axis_points_angle(axis_list[0],
postures[2],
goal_vec_list[2])
angle_x_2 = self.get_three_points_angle([0, 0, 0],
postures[0],
goal_vec_list[0])
if (np.fabs(angle_x_1) + np.fabs(angle_x_2) <
np.fabs(angle_y_1) + np.fabs(angle_y_2)):
rotation_axis_index = 0
sub_vec_index = 1
goal_index = 2
oder_axis_index_list[0] = rotation_axis_index
self.debug_on = True
(result_angle_list[rotation_axis_index],
axis_list[goal_index],
axis_list[sub_vec_index],
postures) = self.__get_rotate_axis_and_angle(
axis_list[rotation_axis_index],
postures[goal_index],
goal_vec_list[goal_index],
axis_list[goal_index],
axis_list[sub_vec_index],
postures,
np.pi * 0.5)
self.debug_on = False
rotation_axis_index = 1
goal_index = 2
sub_vec_index = oder_axis_index_list[0]
oder_axis_index_list[1] = rotation_axis_index
(result_angle_list[rotation_axis_index],
axis_list[goal_index],
axis_list[sub_vec_index],
postures) = self.__get_rotate_axis_and_angle(
axis_list[rotation_axis_index],
postures[goal_index],
goal_vec_list[goal_index],
axis_list[goal_index],
None,
postures,
np.pi)
oder_axis_index_list[1] = rotation_axis_index
else:
rotation_axis_index = 1
goal_index = 2
sub_vec_index = 0
oder_axis_index_list[0] = rotation_axis_index
# axis_list[sub_vec_index] にNoneが渡されるので一時退避
sub_vec = axis_list[sub_vec_index]
(result_angle_list[rotation_axis_index],
axis_list[goal_index],
axis_list[sub_vec_index],
postures) = self.__get_rotate_axis_and_angle(
axis_list[rotation_axis_index],
postures[goal_index],
goal_vec_list[goal_index],
axis_list[goal_index],
None,
postures,
np.pi)
oder_axis_index_list[0] = rotation_axis_index
axis_list[sub_vec_index] = sub_vec
rotation_axis_index = 0
goal_index = 2
sub_vec_index = oder_axis_index_list[0]
oder_axis_index_list[1] = rotation_axis_index
(result_angle_list[rotation_axis_index],
axis_list[goal_index],
axis_list[sub_vec_index],
postures) = self.__get_rotate_axis_and_angle(
axis_list[rotation_axis_index],
postures[goal_index],
goal_vec_list[goal_index],
axis_list[goal_index],
axis_list[sub_vec_index],
postures,
np.pi * 0.5)
# ここまで2回転目
# 3回目の回転は。残りの軸を回転軸にする。最初の軸を操作対象ベクトルにする。
rotation_axis_index = 2
sub_vec_index = 0
goal_index = 1
if (1 == rotation_axis_index):
# y軸の時180まで許す
(result_angle_list[rotation_axis_index],
axis_list[goal_index],
axis_list[sub_vec_index],
postures) = self.__get_rotate_axis_and_angle(
axis_list[rotation_axis_index],
postures[goal_index],
goal_vec_list[goal_index],
axis_list[goal_index],
None, postures,
np.pi)
oder_axis_index_list[2] = rotation_axis_index
else:
(result_angle_list[rotation_axis_index],
axis_list[goal_index],
axis_list[sub_vec_index],
postures) = self.__get_rotate_axis_and_angle(
axis_list[rotation_axis_index],
postures[goal_index],
goal_vec_list[goal_index],
None,
None,
postures,
np.pi * 0.5)
oder_axis_index_list[2] = rotation_axis_index
self.__pre_wrist_axis_angles = copy.deepcopy(result_angle_list)
return result_angle_list
def __get_rotate_axis_and_angle(self,
rotate_axis,
start_posture,
goal_posture,
a1,
a2,
postures,
limit_angle):
"""
任意軸周りの回転を複数のベクトルに対して行う
回転軸、回転の起点となるベクトル、回転させる残りのベクトル。回転目標のベクトル、回転角の上限の絶対値
角度と、回転軸以外が回転した後の軸のベクトルを返す
"""
angle = self.get_axis_points_angle(rotate_axis,
start_posture,
goal_posture)
if (limit_angle < np.fabs(angle)):
if 0 < angle:
angle = limit_angle
else:
angle = - limit_angle
a1aa = AxisAngle(rotate_axis, angle)
# x軸を回転させたときの y, z軸のベクトルを取得する
ra1 = a1
ra2 = a2
if not isinstance(a1, type(None)):
ra1 = a1aa.get_rotate_vector(a1)
if not isinstance(a2, type(None)):
ra2 = a1aa.get_rotate_vector(a2)
for i in range(3):
postures[i] = a1aa.get_rotate_vector(postures[i])
# za_buf,ya_bufはデバッグ用終わったら消すこと
return angle, ra1, ra2, postures
def __check_vector_length(self, base, check1, check2):
"""
基準ベクトルに近いベクトルがどちらかを判断
base がcheck1とcheck2のどちらのベクトルに近いか。
1と遠い場合True
2と遠い場合False
"""
deff1 = np.array(check1) - np.array(base)
deff2 = np.array(check2) - np.array(base)
if np.dot(deff2, deff2) < np.dot(deff1, deff1):
return True
else:
return False
def get_axis_points_angle(self, axis, start_point, end_point):
"""
任意軸に対してどれだけ回転したら始点を終点に最も近づけるかを返す。
"""
# 軸周りの2点の角度
# 軸を z軸に合わせ2店の角度を返す。
if (self.check_float_axis(axis, start_point) or
self.check_float_axis(axis, end_point)):
# 回転軸と開始地点または終了地点が同じとき0
return 0
base_axis = [0, 0, 1]
normal = self.get_normal([0, 0, 0], axis, base_axis)
angle = self.get_three_points_angle([0, 0, 0], axis, base_axis)
if None is normal:
pass
else:
aa = AxisAngle(normal, angle)
if None is normal or np.isnan(normal[0]):
start_point_rotated = copy.deepcopy(start_point)
end_point_rotated = copy.deepcopy(end_point)
else:
start_point_rotated = aa.get_rotate_vector(start_point)
end_point_rotated = aa.get_rotate_vector(end_point)
# 基準軸によって0にする軸を変える
# ジンバルロック状態の時は0度とする。
if (np.fabs(self.get_three_points_angle(
[0, 0, 0],
[0, 0, 1],
start_point_rotated) * 180 / np.pi) < 10 or
170 < np.fabs(
self.get_three_points_angle([0, 0, 0],
[0, 0, 1],
start_point_rotated) *
180 / np.pi)):
return 0
if (np.fabs(self.get_three_points_angle(
[0, 0, 0],
[0, 0, 1],
end_point_rotated) * 180 / np.pi) < 10 or
170 < np.fabs(self.get_three_points_angle(
[0, 0, 0],
[0, 0, 1],
end_point_rotated) * 180/np.pi)):
return 0
start_point_rotated[2] = 0
end_point_rotated[2] = 0
angle = self.get_three_points_angle(
[0, 0, 0],
start_point_rotated,
end_point_rotated)
return self.get_angle_sine(axis, angle, start_point, end_point)
def get_three_points_angle(self, base_point, start_point, end_point):
"""
基準軸を頂点に始点から終点までの角度を得る
"""
normal = self.get_normal(base_point, start_point, end_point)
# 法線は最も軸成分が大きい軸が正になるように整える。
angle = self.get_angle(base_point, start_point, end_point)
if None is normal:
angle = 0
else:
angle = self.get_angle_sine(normal, angle, start_point, end_point)
return angle
def get_angle_sine(self, normal, angle, start_point, end_point):
"""
指定のベクトル回りの回転に対して、始点から終点まで指定の角度で回転させたとき角度の向きを整える。arccosでは角度も向きを得られないため
"""
# 回転角の向きを決定
aa = AxisAngle(normal, angle)
end_buf1 = aa.get_rotate_vector(start_point) - np.array(end_point)
aa = AxisAngle(normal, -angle)
end_buf2 = aa.get_rotate_vector(start_point) - np.array(end_point)
if (np.dot(end_buf2, end_buf2) < np.dot(end_buf1, end_buf1)):
angle *= -1
return angle
# 機材固有の計算-----------------------------------------
# 機材固有の計算Eulerからサーボの角度に変更
def euler_to_servo(self, euler, ezero, pm):
if pm: # 向きの指定
return self.__euler_to_servo_main(euler, ezero)
else:
return 221 - self.__euler_to_servo_main(euler, ezero)
# 主要角度計算110が中心なのでそれを中心になるように取得
def __euler_to_servo_main(self, euler, ezero):
# 110を正面0とする。1-220になるように設定
ebuf = self.__get_euler_offset(euler, ezero)
if 0 <= ebuf:
if 110 < ebuf:
return 220
else:
return ebuf + 110
else:
if ebuf <= -110:
return 1
else:
return ebuf + 110
# オフセット設定。（ゼロ点設定）
def __get_euler_offset(self, euler, ezero):
# ezeroの位置を正面0とする。
if (euler < ezero - 180):
euler += 360
return euler-ezero
if (ezero + 180 < euler):
euler -= 360
return euler - ezero
return euler - ezero
if __name__ == '__main__':
rsr = RobotSupport()
rsl = RobotSupport()
# rsr.set_arm_no_entry_area(RIGHT_ARM, -0.05, [[0.04, -0.05, 0.1], [-0.19, -0.15, 0.0]])
# rsl.set_arm_no_entry_area(LEFT_ARM, -0.05, [[-0.04, -0.05, 0.1], [0.19, -0.15, 0.0]])
rsr.set_arm_no_entry_area(RIGHT_ARM, -50, [[60, -50, 100], [-190, -200, 0.0]])
rsl.set_arm_no_entry_area(LEFT_ARM, -50, [[-60, -50, 100], [190, -200, 0.0]])
st_time = time.time()
# rsr.calibrate_hand_positon(
# [0.03157314285635948, 0.046851739287376404, -0.11910277605056763],
# [0.18736883997917175, -0.259101003408432, -0.49662041664123535], None)
#
#
# rsr.calibrate_hand_positon(
# [0.03633175045251846, 0.02545427717268467, -0.19819536805152893], None,
# [0.16725659370422363, -0.5226390361785889, -0.22827120125293732])
#
# rsr.calibrate_hand_quaternion(
# [0.9562617540359497, -0.2867755591869354,
# 0.019782420247793198, -0.054146625101566315])
#
# rsl.calibrate_hand_positon(
# [0.03045990690588951, 0.04664797708392143, -0.11895237863063812],
# [-0.19343185424804688, -0.25430455803871155, -0.5014460682868958],
# None)
#
# rsl.calibrate_hand_positon(
# [0.03633175045251846, 0.02545427717268467, -0.19819536805152893], None,
# [-0.18954160809516907, -0.5021775364875793, -0.2474662810564041])
# rsl.calibrate_hand_quaternion(
# [0.9795217514038086, -0.19008632004261017,
# 0.06494633853435516, -0.013647915795445442])
#
rsr.calibrate_hand_positon(
[-0.004383253864943981, 0.03164731711149216, 0.0985642522573471],
[0.21642494201660156, -0.25676634907722473, -0.26720133423805237], None)
rsr.calibrate_hand_positon(
[0.012829231098294258, 0.04661335051059723, -0.035292066633701324], None,
[0.1920902281999588, -0.4742579460144043, -0.058142099529504776])
rsr.calibrate_hand_quaternion([0.9790235757827759, -0.19800397753715515,
0.01447303220629692, -0.04580163210630417])
rsl.calibrate_hand_positon(
[-0.004383253864943981, 0.03164731711149216, 0.0985642522573471],
[-0.22284720838069916, -0.25386136770248413, -0.2778210937976837], None)
rsl.calibrate_hand_positon(
[0.012698828242719173, 0.04840812087059021, -0.03380025923252106], None,
[-0.20376218855381012, -0.4570033848285675, -0.08551811426877975])
rsl.calibrate_hand_quaternion(
[0.9946711659431458, -0.08958355337381363,
0.04874080419540405, -0.015112766996026039])
serial_support = SerialSuppot("com3", 115200)
print("1------")
print("r------")
# r_nparray = rsr.seven_link_ik(
# [0.1623062938451767, -0.5227496027946472, -0.22464650869369507],
# [0.971195638179779, -0.23032459616661072, 0.06030957028269768, -0.009611304849386215])
## r_nparray = rsr.seven_link_ik(
## [0.18736883997917175, -0.259101003408432, -0.49662041664123535],
## [0.971195638179779, -0.23032459616661072, 0.06030957028269768, -0.009611304849386215])
# l_nparray = rsl.seven_link_ik(
# [-0.1870788037776947, -0.49989694356918335, -0.24374061822891235],
# [0.9929907917976379, -0.10897733271121979, 0.03247931972146034, -0.0322236604988575])
## l_nparray = rsl.seven_link_ik(
## [0.18736883997917175, -0.259101003408432, -0.49662041664123535] ,
## [0.9929907917976379, -0.10897733271121979, 0.03247931972146034, -0.0322236604988575])
#########################################
r_nparray = rsr.seven_link_ik(
[0.21642494201660156, -0.25676634907722473, -0.26720133423805237],
[0.992861807346344, -0.11833740770816803, 0.004744814243167639, -0.014111470431089401])
l_nparray = rsl.seven_link_ik(
[-0.22284720838069916, -0.25386136770248413, -0.2778210937976837],
[0.9986308813095093, -0.046200644224882126, 0.020560894161462784, -0.013382543809711933])
# 90度曲げ
# r_nparray = rsr.seven_link_ik(
# [0.1920902281999588, -0.4742579460144043, -0.058142099529504776],
# [0.992861807346344, -0.11833740770816803, 0.004744814243167639, -0.014111470431089401])
# l_nparray = rsl.seven_link_ik(
# [-0.20376218855381012, -0.4570033848285675, -0.08551811426877975],
# [0.9986308813095093, -0.046200644224882126, 0.020560894161462784, -0.013382543809711933])
# r_nparray = rsr.seven_link_ik(
# [0.18956126272678375, -0.4682967960834503, -0.05679388344287872],
# [0.992861807346344, -0.11833740770816803, 0.004744814243167639, -0.014111470431089401])
# l_nparray = rsl.seven_link_ik(
# [-0.20408451557159424, -0.4562219977378845, -0.0868697464466095],
# [0.9986308813095093, -0.046200644224882126, 0.020560894161462784, -0.013382543809711933])
# r_nparray = rsr.seven_link_ik(
# [0.1729862242937088, -0.3501454293727875, -0.07237232476472855],
# [0.9767186641693115, 0.16919629275798798, 0.04107457026839256, -0.12532415986061096])
# l_nparray = rsl.seven_link_ik(
# [-0.17199943959712982, -0.345160573720932, -0.09736013412475586],
# [0.9651334285736084, 0.22544346749782562, -0.07838959991931915, 0.1074604019522667])
print("l------")
print(
"r_nparray, l_nparray",
r_nparray * 180 / np.pi,
l_nparray * 180 / np.pi)
#neck_list = [90 * np.pi / 180, 90 * np.pi / 180]
#wheel_list = [1,-1]
neck_list = [92.47125244140625, 89.66866683959961]
wheel_list = [-1.0, 1.0]
serial_support.debug_on = False
while True:
print("step 0")
neck_list = [90, 90]
wheel_list = [-1.0, 1.0]
serial_support.move_servo(
l_nparray,
(1 * 120.0 + 100.0) * np.pi / 180,
r_nparray,
(0.3 * 120.0 + 100.0) * np.pi / 180,
neck_list, wheel_list)
time.sleep(1)
print("step 1")
neck_list = [90, 120]
wheel_list = [1.0, -1.0]
serial_support.move_servo(
l_nparray,
(1 * 120.0 + 100.0) * np.pi / 180,
r_nparray,
(0.3 * 120.0 + 100.0) * np.pi / 180,
neck_list, wheel_list)
time.sleep(1)
print("step 2")
neck_list = [120, 90]
wheel_list = [0.0, 0.0]
serial_support.move_servo(
l_nparray,
(1 * 120.0 + 100.0) * np.pi / 180,
r_nparray,
(0.3 * 120.0 + 100.0) * np.pi / 180,
neck_list, wheel_list)
time.sleep(1)
#serial_support.debug_on = True
