def BUOY_FOUND(self,event): if(event.color == vision.Color.RED): #get rotation matrix from robot frame to intertial frame, then apply it to the x,y measurements #this rotation matrix must exclude yaw, this ends up looking a bit silly because of quaternion math q1 = self.stateEstimator.getEstimatedOrientation() v1 = math.Vector3(event.x,event.y,0) #now get the quaternion to unrotate our yaw, by unrotating by our level quaternion qp = control.holdCurrentHeadingHelper(q1) #by rotating to the inertial frame, then rotating to the body yaw frame we acheive our goal #rotate coordinates from body frame to inertial frame v1p = qp*(q1.UnitInverse()*v1) event.x = v1p.x event.y = v1p.y self.run(event)
def _angleyaw(self, event): """ When we want to yaw the robot, send a command to the controller to rotate the robot around the body z axis proportional to the event command. When we receive a zero rate command, we should only pass along the command to the controller ONCE. """ change = (event.number / 100.0) * self._angleYawGain if change == 0 and self._lastYawCommand == 0: # dont sent ZERO command twice # note that we do want to send commands repeatedly if # the speed is not zero pass else: ori = self._estimator.getEstimatedOrientation() qHeading = control.holdCurrentHeadingHelper(ori) self._controller.rotate(qHeading * math.Quaternion( math.Degree(change), math.Vector3.UNIT_Z)) self._lastYawCommand = change