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])