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.
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',
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.,
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.
89.                            description='Axis of head rotation; z points to the right',
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
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,
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.
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

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