キャリブレーションの際に求める方の位置の計算方法です。
１、①②とキャリブレーションした点を取ります。
２、1の2点を通る線分を引きます。
３，２に頭の位置から垂線を引きます。
４，３の交点を肩の位置とします。
高さは1の時の高さを用います。
robot_support.pyの以下の部分が該当します。
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)