def done(self):
"""
目前定義為，step()過step_count次數，就算done
這很有問題XD
由於有速度限制，step過後很可能沒有移動到想要的角度
TODO!!
"""
if (type(None) != type(self.direction_list)):
# 這是在腳本裡面
#print("In script")
current_position = PSFunc.getValue(self.psList)
diff = np.array(self.goal_position) - np.array(current_position)
#print("starting position ", self.starting_position)
#print("current position ", psValue)
#print("goal position ", goal_position)
#print("diff ", diff)
print(np.abs(diff) < self.tolerance)
if ((np.abs(diff) < self.tolerance).all()):
return True
else:
return False
# 這是一次一次執行
return self.step_count <= 0
def done(self):
psValue = PSFunc.getInitialPosition(self._psList)
fk = self.armChain.forward_kinematics(psValue)
fingertipPosition = fk[:3, 3]
distance = np.linalg.norm(fingertipPosition -
np.array(self.target_position))
if (self.AXIS == "X"):
fingerOrientation = fk[0:3, 0]
elif (self.AXIS == "Y"):
fingerOrientation = fk[0:3, 1]
elif (self.AXIS == "Z"):
fingerOrientation = fk[0:3, 2]
if (self.orientation != None):
cos = np.dot(fingerOrientation, self.orientation)
else:
cos = 1
#print(fk)
#print(distance, 1-abs(cos), fingerOrientation, self.orientation)
#print(distance, cos)
print(distance <= self.tolerance, 1 - abs(cos) < self.o_tolerance,
self.o_tolerance)
if (distance <= self.tolerance and 1 - abs(cos) < self.o_tolerance):
return True
return False
def _initialize(self, psList):
self.psList = psList
self.starting_position = np.array(PSFunc.getValue(self.psList))
self.goal_position = []
if (len(self.step_distance_list) == len(self.direction_list)):
for i in range(len(self.direction_list)):
# There's probably a better way for this...
raw_val = self.starting_position[
i] + self.step_distance_list[i] * self.direction_list[i]
if (raw_val > max(self.distance_limit_list[i])):
raw_val = max(self.distance_limit_list[i])
if (raw_val < min(self.distance_limit_list[i])):
raw_val = min(self.distance_limit_list[i])
self.goal_position.append(raw_val)
def _initialize_action(self):
current_action = self.actions[self.current_action_id]
if (IKReachTarget == type(current_action)):
initPos = PSFunc.getInitialPosition(self._psList)
current_action._initialize(initPos, self._armChain, self._psList)
elif (Actuate == type(current_action)):
current_action._initialize(self._psList)
elif (Pause == type(current_action)):
pass
elif (Grab == type(current_action)):
current_action._initialize(self._psList)
elif (Turn == type(current_action)):
current_action._initialize(self._psList)
elif (Pour == type(current_action)):
current_action._initialize(self._psList)
else:
print()
raise TypeError("Trying to initialize unknown action in script")
def _initialize(self, psList):
self.psList = PSFunc.getValue(psList)
if (self.isGrab):
self.psList.extend([0.00, 0.00])
else:
self.psList.extend([0.04, 0.04])