PYTHON   67
CozmoKinematics
Guest on 1st February 2023 01:37:11 AM


  1. from math import pi
  2.  
  3. import cozmo
  4.  
  5. from .kine import *
  6. from cozmo_fsm import geometry
  7. from .geometry import tprint
  8. from .rrt_shapes import *
  9.  
  10. # ================ Constants ================
  11.  
  12. wheelbase = 45 # millimeters
  13. center_of_rotation_offset = -19.7 # millimeters
  14.  
  15. # ================================================================
  16.  
  17. class CozmoKinematics(Kinematics):
  18.     def __init__(self,robot):
  19.         base_frame = Joint('base',
  20.                            description='Base frame: the root of the kinematic tree')
  21.  
  22.         # cor is center of rotation
  23.         cor_frame = Joint('cor', parent=base_frame,
  24.                           description='Center of rotation',
  25.                           r=-19.,
  26.                           collision_model=Rectangle(geometry.point(),
  27.                                                     dimensions=(95,60)))
  28.  
  29.         # Use link instead of joint for world_frame
  30.         world_frame = Joint('world', parent=base_frame, type='world', getter=self.get_world,
  31.                             description='World origin in base frame coordinates',
  32.                             qmin=None, qmax=None)
  33.  
  34.         front_axle_frame = Joint('front_axle', parent=base_frame,
  35.                                  description='Center of the front axle',
  36.                                  alpha=pi/2)
  37.        
  38.         left_front_wheel_frame = Joint('left_front_wheel', parent = front_axle_frame,
  39.                                       description = 'Axis of left wheel rotation - \
  40.                                      z points to the right', d = -27.5)
  41.  
  42.         right_front_wheel_frame = Joint('right_front_wheel', parent = front_axle_frame,
  43.                                       description = 'Axis of right wheel rotation - \
  44.                                      z points to the right', d = 27.5)
  45.  
  46.         back_axle_frame = Joint('back_axle', parent=base_frame, r=-46., alpha=pi/2)
  47.  
  48.         # This frame is on the midline.  Could add separate left and right shoulders.
  49.         # Positive angle is up, so z must point to the right.
  50.         # x is forward, y points up.
  51.         shoulder_frame = Joint('shoulder', parent=base_frame,
  52.                                type='revolute', getter=self.get_shoulder,
  53.                                description='Rotation axis of the lift; z points to the right',
  54.                                qmin=cozmo.robot.MIN_LIFT_ANGLE.radians,
  55.                                qmax=cozmo.robot.MAX_LIFT_ANGLE.radians,
  56.                                d=21., r=-39., alpha=pi/2)
  57.  
  58.         lift_attach_frame = \
  59.             Joint('lift_attach', parent=shoulder_frame, type='revolute',
  60.                   description='Tip of the lift, where cubes attach; distal end of four-bar linkage',
  61.                   getter=self.get_lift_attach, r=66.,
  62.                   qmax = - cozmo.robot.MIN_LIFT_ANGLE.radians,
  63.                   qmin = - cozmo.robot.MAX_LIFT_ANGLE.radians,
  64.                   #collision_model=Circle(geometry.point(), radius=10))
  65.             )
  66.  
  67.         #Go get exact measurements
  68.  
  69.         left_hook_dummy = Joint('left_hook_dummy', parent= lift_attach_frame,
  70.                             description = 'Dummy joint at horizontal position of the hooks - \
  71.                                z points up', d = -17.5, alpha = -pi/2)
  72.  
  73.         left_hook_frame = Joint('left_hook', parent = left_hook_dummy,
  74.                                 description = 'frame with origin in the center of the left hooks \
  75.                                     attachment to the robot - z points upwards', d = -10)
  76.  
  77.         right_hook_dummy = Joint('left_hook_dummy', parent= lift_attach_frame,
  78.                             description = 'Dummy joint at horizontal position of the hooks - \
  79.                                z points up', d = 17.5, alpha = -pi/2)
  80.  
  81.         right_hook_frame = Joint('left_hook', parent = right_hook_dummy,
  82.                                 description = 'frame with origin in the center of the right hooks \
  83.                                     attachment to the robot - z points upwards', d = -10)
  84.  
  85.         # Positive head angle is up, so z must point to the right.
  86.         # With x pointing forward, y must point up.
  87.         head_frame = Joint('head', parent=base_frame, type='revolute',
  88.                            getter=self.get_head,
  89.                            description='Axis of head rotation; z points to the right',
  90.                            qmin=cozmo.robot.MIN_HEAD_ANGLE.radians,
  91.                            qmax=cozmo.robot.MAX_HEAD_ANGLE.radians,
  92.                            d=35., r=-10., alpha=pi/2)
  93.  
  94.         # Dummy joint located below head joint at level of the camera frame,
  95.         # and x axis points down, z points forward, y points left
  96.         camera_dummy = Joint('camera_dummy', parent=head_frame,
  97.                              description='Dummy joint below the head, at the level of the camera frame',
  98.                              theta=-pi/2, r=7.5, alpha=-pi/2)
  99.         # x axis points right, y points down, z points forward
  100.         camera_frame = Joint('camera', parent=camera_dummy,
  101.                              description='Camera reference frame; y is down and z is outward',
  102.                              d=15., theta=-pi/2)
  103.  
  104.         joints = [base_frame, world_frame, cor_frame,
  105.                   front_axle_frame, back_axle_frame,
  106.                   shoulder_frame, lift_attach_frame,
  107.                   head_frame, camera_dummy, camera_frame,
  108.                   left_front_wheel_frame, right_front_wheel_frame,
  109.                   left_hook_dummy, right_hook_dummy,
  110.                   left_hook_frame, right_hook_frame]
  111.  
  112.         super().__init__(joints,robot)
  113.  
  114.     def get_head(self):
  115.         return self.robot.head_angle.radians
  116.  
  117.     def get_shoulder(self):
  118.         # Formula supplied by Mark Wesley at Anki
  119.         # Check SDK documentation for new lift-related calls that might replace this
  120.         return math.asin( (self.robot.lift_height.distance_mm-45.0) / 66.0)
  121.  
  122.     def get_lift_attach(self):
  123.         return -self.get_shoulder()
  124.  
  125.     def get_world(self):
  126.         return self.robot.world.particle_filter.pose_estimate()

Raw Paste

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