PYTHON   43

FindDistance

Guest on 2nd August 2022 12:47:53 AM

  1. from cozmo_fsm import *
  2. import math
  3. import numpy as np
  4.  
  5. class FindDistance(StateNode):
  6.     def start(self, event=None):
  7.         if self.running:return
  8.         super().start(event)
  9.  
  10.         #Translation matrix between the camera and base frame
  11.         #Note - the base frame lies on the front axel
  12.         cam_to_base_matrix = robot.kine.joint_to_base('camera')
  13.        
  14.         #Initiates the location of the object as 100mm in front of the camera
  15.         #In the camera frame, z points forward, so z = 100
  16.         object_location = np.array([[0],[0],[100],[1]])
  17.  
  18.         #Finds the location of the object relative to the base frame(front axel)
  19.         base_location = np.matmul(cam_to_base_matrix, object_location)
  20.        
  21.         #Gets the x location from the matrix since x points forward in the base frame
  22.         dist = base_location[0][0]
  23.         print(dist)
  24.         self.post_completion()
  25.  
  26. #Runs through the FSM to find the values
  27. class KineCalc(StateMachineProgram):
  28.     $setup {
  29.         StateNode() =N=> SetHeadAngle(-10) =C=> FindDistance() =C=>
  30.         SetHeadAngle(0) =C=> FindDistance() =C=>
  31.         SetHeadAngle(10) =C=> FindDistance() =C=>
  32.         SetHeadAngle(20) =C=> FindDistance() =C=>
  33.         SetHeadAngle(30) =C=> FindDistance() =C=>
  34.         SetHeadAngle(40) =C=> FindDistance()
  35.     }

Raw Paste

Login or Register to edit or fork this paste. It's free.