├── README.md ├── level ├── README.md └── level.py ├── self_docking ├── README.md ├── self_docking.py └── self_docking_v2.py └── guess_number ├── README.md ├── guess_number.py └── guess_number_anim.py /README.md: -------------------------------------------------------------------------------- 1 | # Cozmo - Software 2 | Repository about software controlling the robot Cozmo from the Anki company 3 | 4 | - [guess_number](https://github.com/LucasWaelti/Cozmo/tree/master/guess_number) 5 | This is a little game where Cozmo guesses a secret number that the user chooses. 6 | - [self_docking](https://github.com/LucasWaelti/Cozmo/tree/master/self_docking) 7 | This program allows Cozmo to self dock onto its charger after collecting its cubes, without any further help or special marker. 8 | - [level](https://github.com/LucasWaelti/Cozmo/tree/master/level) 9 | This little tool allows Cozmo to help you build your piece of furniture by acting like a level, using its backpack lights. 10 | -------------------------------------------------------------------------------- /level/README.md: -------------------------------------------------------------------------------- 1 | # Cozmo acts like a level to help you build things 2 | Cozmo will detect a variation of angular position (pitch) and set its backpack colors accordingly. The lights behave in the same way a bubble would, plus some changing colors making it more fun. 3 | 4 | ## Set it up 5 | Lay Cozmo on a flat surface (the ground for instance) and then start the script. Cozmo will then raise its lift and its backpack LEDs will go on. Put it on what you want to adjust and make the corrections! If all three LEDs are green, then Cozmo considers it's laying flat which is good! Otherwise only two LEDs will turn on, turning more and more to red depending on the angle that is detected. 6 | 7 | ## When you're done 8 | When your surface is well positioned, tell Cozmo he helped you succeed by giving him a small "check" to let him know his job here is done! 9 | -------------------------------------------------------------------------------- /self_docking/README.md: -------------------------------------------------------------------------------- 1 | # Cozmo - Collecting Cubes and Self Docking on Charger 2 | This program enables Cozmo to drive off its charger, collect its cubes by placing them near it and then climbing back onto its charger when it is done. Cozmo will directly try to climb back onto its charger if its battery is low and will ignore its cubes. No further help or markers are required. It is possible that Cozmo does not find a cube or its charger, so it will ask for help if needed. It will either say "Cube?" or "Charge?" dependending on what it is looking for. Just place the required object or Cozmo in a position where Cozmo can keep working and let it finish its job! If finding any cube takes too long, Cozmo will give up and go back to its charger. 3 | 4 | You can basically launch the whole procedure from any state. It means you can play a few games with the app and then switch to sdk mode and run the script. Just make sure the setting is not too different from what is described in the following section. 5 | 6 | > **Note**: Use the updated version of the controller (v2) for stacked cubes! 7 | 8 | ## Optimal Setup When Starting 9 | Cozmo should be on its charger when starting (but it will work otherwise too). The cubes can now be stacked (2 or 3!) or rolled and should be located somewhere in front of the charger. 10 | 11 | This video demonstrates how things work: 12 | 13 | https://youtu.be/xfAxPHdetH0 14 | 15 | The video is sped up 4 times and there is therefore no sound. 16 | 17 | ## Error Detection 18 | Different strategies were implemented to detect if Cozmo is succeeding in its task or if a problem occured while running the program. This task is not easy and is prone to errors, that can occur at different levels. The error handling is based on timeouts and unexpected 19 | positions of the robot as well as action failures to determine if the current process has to be aborted/restarted or not. 20 | 21 | ## Improvements (TODO) 22 | - ~~Support already stacked or rolled cubes when cleaning up.~~ -> **Solved with *self_docking_v2*** 23 | - Stack cubes as a pyramid instead of current configuration. 24 | - ~~Detect if backup_onto_charger() has succeeded (not possible?).~~ -> **Solved** 25 | - Speed up some processes. 26 | -------------------------------------------------------------------------------- /guess_number/README.md: -------------------------------------------------------------------------------- 1 | # What it is about 2 | With this program, Cozmo will try to guess the number you are thinking about (by default the search range is [0,60]). Without further ado, here is a video showing how it works (**please enable the subtitles in the video for more info**): 3 | https://www.youtube.com/watch?v=Oj2uBXrqvrE 4 | In the video, Cozmo makes guesses in French but the program should work fine for any language implemented in your Cozmo. 5 | 6 | This idea came to me after seeing [this post](https://forums.anki.com/t/number-guessing-game/10846), so thank you @Cadwallader01 for the idea! 7 | In the post above, the user is supposed to guess a number picked by Cozmo. But I think the other way around is also pretty interesting. That's why I imagined this simple algorithm to implement this functionality. 8 | 9 | # How it works 10 | When starting the program, you will be asked to sequentially tap on each cube when their respective lights go up (it's required for the event handling, at least in the way I implemented it). You can then specify a new range for the search by typing the 2 limit values (`lower higher`) of the new range, separated by a space. It will be checked wether the input is valid or not and the range will be asked again if an error is detected. Any range is possible, you could select `-3000 5000` as your custom range if you want! 11 | On the other hand, if you want to use the default range, just press `Enter` without specifying anything and the program will further execute. 12 | 13 | To make Cozmo guess what your secret number is, you can only tell him if your number is greater or smaller than its guess. To do so, you can tap on each cube with the according color: 14 | 15 | - blue: your number is smaller 16 | - green: that's it! Cozmo guessed right 17 | - red: your number is greater 18 | 19 | When Cozmo finally finds your number, he plays a few animations celebrating and the game is over. 20 | 21 | While playing, Cozmo will look for faces and will turn towards one if a face appears. If no face is detected, Cozmo will make a guess anyway. 22 | 23 | If you mess things up and give weird answers, Cozmo will eventually be aware of it and will quit the game, kind of frustrated. 24 | 25 | If you don't give any answer quickly enough (after 30 seconds), Cozmo will show that it got bored and will exit the game. 26 | 27 | At the end of each game, you can press `Enter` to restart a new game or leave by pressing any key followed by `Enter`. 28 | 29 | # The code! 30 | [guess_number.py](https://github.com/LucasWaelti/Cozmo/blob/master/guess_number/guess_number.py) and 31 | [guess_number_anim.py](https://github.com/LucasWaelti/Cozmo/blob/master/guess_number/guess_number_anim.py) 32 | 33 | ## Remarks 34 | I have tested the code a few times now and it should be robust enough. So if you decide to try it out, hopefully it will work just fine! 35 | 36 | ### Requirements 37 | All you need is to download or copy both files `guess_number_anim.py` and `guess_number.py`, run this last one and start playing. You will need the `numpy` module to be installed. 38 | 39 | ### Advice 40 | Simply be careful when starting the program, information will be displayed in the command prompt to help you setup the game. So if nothing is happening with Cozmo and its cubes, you might be expected to give an input from the keyboard to continue. Once the game is launched, you do not need to care about the command prompt while playing, although the guesses that Cozmo makes are also displayed on the screen in case the audio wasn't too good. 41 | 42 | ### Feedback 43 | I would really appreciate hearing back from those who try this little game. Any remark, suggestion or bug report would be appreciated! 44 | -------------------------------------------------------------------------------- /level/level.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Let Cozmo help you while building some stuff! 3 | With this program, Cozmo will play the role of a level. 4 | Just put Cozmo on the surface you would like to adjust 5 | and give Cozmo a check when the job is done! 6 | ''' 7 | import time 8 | import math 9 | 10 | import cozmo 11 | from cozmo.util import degrees, radians, distance_mm, speed_mmps 12 | from cozmo.objects import LightCube1Id, LightCube2Id, LightCube3Id 13 | from cozmo.lights import Light, Color 14 | 15 | global robot 16 | 17 | global delta 18 | 19 | global MAX_ANGLE 20 | global LIFT_POSITION 21 | 22 | def play_animation(robot: cozmo.robot.Robot, anim_trig, body = False, lift = False, parallel = False): 23 | # anim_trig = cozmo.anim.Triggers."Name of trigger" (this is an object) 24 | # Refer to "http://cozmosdk.anki.com/docs/generated/cozmo.anim.html#cozmo.anim.Triggers" for animations' triggers 25 | # Refer to "http://cozmosdk.anki.com/docs/generated/cozmo.robot.html#cozmo.robot.Robot.play_anim_trigger" for playing the animation 26 | 27 | robot.play_anim_trigger(anim_trig,loop_count = 1, in_parallel = parallel, 28 | num_retries = 0, use_lift_safe = False, 29 | ignore_body_track = body, ignore_head_track=False, 30 | ignore_lift_track = lift).wait_for_completed() 31 | 32 | def celebrate(robot: cozmo.robot.Robot): 33 | trigger = cozmo.anim.Triggers.CodeLabCelebrate 34 | play_animation(robot,trigger,body=True,lift=True,parallel=True) 35 | 36 | 37 | 38 | 39 | def setMaxAngle(max_ang=20): 40 | global MAX_ANGLE 41 | MAX_ANGLE = max_ang 42 | 43 | def calculateColor(abs_angle=0): 44 | ''' 45 | This function will attribute a color corresponding to the error abs_angle. 46 | The maximun error is set at MAX_ANGLE 47 | ''' 48 | # RGB tuple (0-255) 49 | global MAX_ANGLE 50 | slope = 255/MAX_ANGLE 51 | 52 | if(abs_angle > 20): 53 | abs_angle = 20 54 | 55 | G = 255 - slope*abs_angle 56 | G = int(G) 57 | R = slope*abs_angle 58 | R = int(R) 59 | if(abs_angle <= MAX_ANGLE/2): 60 | B = slope*abs_angle 61 | else: 62 | B = 255 - slope*abs_angle 63 | B = int(B) 64 | 65 | color = Color(rgb = (R,G,B)) 66 | return color 67 | 68 | def getColor(pitch_angle=0): 69 | abs_angle = math.fabs(pitch_angle) 70 | color = calculateColor(abs_angle) 71 | return color 72 | 73 | def setBackpackColors(): 74 | global robot,delta,LIFT_POSITION 75 | 76 | print("Robot\'s pitch in radians: ") 77 | while(True): 78 | pitch = robot.pose_pitch.degrees 79 | pitch -= delta 80 | #print(pitch) 81 | 82 | backLights = Light(getColor(pitch)) 83 | lightOff = Light(Color(rgb=(0,0,0))) 84 | if(pitch > 0.04):#Forward 85 | robot.set_backpack_lights(lightOff,backLights,backLights,lightOff,lightOff) 86 | elif(pitch < -0.04): 87 | robot.set_backpack_lights(lightOff,lightOff,backLights,backLights,lightOff) 88 | else: 89 | robot.set_center_backpack_lights(backLights) 90 | 91 | if(math.fabs(robot._lift_position.ratio - LIFT_POSITION) > 0.05): 92 | print('Check detected!') 93 | break 94 | time.sleep(.1) 95 | robot.set_backpack_lights_off() 96 | return 97 | 98 | def init(r: cozmo.robot.Robot): 99 | global robot,delta 100 | robot = r 101 | 102 | setMaxAngle() 103 | 104 | robot.set_head_angle(cozmo.util.Angle(degrees=0), accel=10.0, max_speed=10.0, 105 | duration=0.0, warn_on_clamp=True, in_parallel=False, num_retries=0).wait_for_completed() 106 | robot.pose.invalidate() 107 | delta = robot.pose_pitch.degrees 108 | return 109 | 110 | def level(robot: cozmo.robot.Robot): 111 | global LIFT_POSITION 112 | init(robot) 113 | robot.set_lift_height(0.8).wait_for_completed() 114 | LIFT_POSITION = robot._lift_position.ratio 115 | setBackpackColors() 116 | robot.set_lift_height(0.0).wait_for_completed() 117 | celebrate(robot) 118 | return 119 | 120 | if __name__ == '__main__': 121 | cozmo.run_program(level,use_viewer=False,use_3d_viewer=False) -------------------------------------------------------------------------------- /guess_number/guess_number.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Written by: @Luc (https://forums.anki.com/u/Luc/summary) 3 | 4 | This little program makes Cozmo guess a number between 0 and 60. 5 | After initialising the cubes, you can specify a new range by typing 6 | the 2 limit values of the new range, separated by a space. It will 7 | be checked wether the input is valid or not and the range will be 8 | asked again if an error is detected. If you want to use the default 9 | range, just press Enter without specifying anything. 10 | 11 | To make Cozmo guess what is your secret number, you can only tell 12 | him if your number is greater or smaller than his guess. To do so, 13 | you can tap on each cube with the right color: 14 | 15 | - blue: your number is smaller 16 | - green: that's it! Cozmo guessed right 17 | - red: your number is greater 18 | 19 | When Cozmo finally finds your number, he plays a few animations 20 | celebrating and the program is then over, ready to restart. 21 | 22 | If you mess things up and give weird answers, Cozmo will eventually 23 | be aware of it and will quit the program. 24 | 25 | If you don't give any answer quickly enough, Cozmo will get bored 26 | and exit the program. 27 | 28 | You need to have the file "guess_number_anim.py" in order to use 29 | this file. It contains the animation functions for Cozmo and the cubes. 30 | ''' 31 | 32 | import cozmo 33 | 34 | import numpy as np 35 | import time 36 | 37 | import guess_number_anim as anim 38 | 39 | robot = None 40 | 41 | def init_robot(cozmo_robot: cozmo.robot.Robot): 42 | global robot 43 | robot = cozmo_robot 44 | 45 | ''' ----------------------Search logic------------------------- ''' 46 | guess_memory = [] 47 | x = 0 48 | y = 0 49 | error_detected = False 50 | 51 | def reset_memory(): 52 | global guess_memory 53 | guess_memory = [] 54 | 55 | def check_memory(guess): 56 | # Verify if the guess was already made before 57 | global guess_memory 58 | 59 | for i in range(0,len(guess_memory)): 60 | if(i == len(guess_memory)-1): 61 | # No check, this is the last guess! 62 | continue 63 | elif(guess == guess_memory[i]): 64 | print('Cozmo is confused... Something went wrong!') 65 | error_detected = True 66 | return True 67 | return False 68 | 69 | def make_guess(): 70 | # Produce a guess given a certain range. 71 | # The guess is patially random to avoid that Cozmo always uses 72 | # the same values while searching. 73 | global x,y,guess_memory 74 | 75 | diff = y-x 76 | if(diff == 2): 77 | guess = x + 1 78 | elif(diff == 1): 79 | guess = x 80 | elif(np.random.random() > 0.5): 81 | guess = x + diff/2 + np.random.random()*diff*0.1 82 | else: 83 | guess = x + diff/2 - np.random.random()*diff*0.1 84 | guess = int(guess) 85 | guess_memory.append(guess) 86 | return guess 87 | 88 | def propose_guess(): 89 | global robot 90 | guess = make_guess() 91 | string_guess = str(guess) + '?' 92 | anim.hesitate(robot) 93 | anim.find_face(robot) 94 | anim.propose_guess(robot, string_guess) 95 | print('Guess: ',guess) 96 | return guess 97 | 98 | def search(): 99 | # Main algorithm, used to narrow the search range at each try 100 | # and detect eventual errors from user 101 | global x,y,robot 102 | 103 | while True: 104 | guess = propose_guess() 105 | 106 | if(check_memory(guess)): # Check if something's wrong 107 | anim.frustrated(robot) 108 | anim.sad(robot) 109 | break 110 | #answer = input('Chosen value is smaller "s", greater "g", correct "c"? ') 111 | answer = anim.get_answer_from_cubes(robot) 112 | 113 | diff = y-x 114 | if(answer == 'c'): 115 | anim.success(robot) 116 | break 117 | elif(answer == 's'): 118 | y = guess 119 | if(diff == 1): 120 | y = y+1 121 | elif(answer == 'g'): 122 | x = guess 123 | if(diff == 1): 124 | x = x+1 125 | elif(answer == 'timeOut'): 126 | print('Timeout: no input from user') 127 | anim.bored(robot) 128 | anim.switch_cubes_off() 129 | break 130 | 131 | def get_range(): 132 | # Get the range of search 133 | global x,y 134 | input_range = input("Range of search (press Enter for default): ") 135 | if(input_range == ''): 136 | x = 0 137 | y = 60 138 | print('Using default range: ','[',x,',',y,']') 139 | y = y + 1 # Small hack because of upper limit 140 | else: 141 | # Use special range 142 | x,y = input_range.split(' ') 143 | x = int(x) 144 | y = int(y) 145 | if(x >= y): 146 | print('The range specified is invalid') 147 | get_range() 148 | return 149 | print('Using custom range: ','[',x,',',y,']') 150 | y = y + 1 # Small hack because of upper limit 151 | 152 | ''' ----------------------Search logic: End------------------------- ''' 153 | 154 | def cozmo_program(c_robot: cozmo.robot.Robot): 155 | global robot 156 | init_robot(c_robot) 157 | 158 | anim.init_cubes(robot) 159 | print('Initialisation successful') 160 | anim.wiggle(robot) 161 | 162 | while(True): 163 | get_range() 164 | 165 | anim.find_face(robot) 166 | 167 | print('Color code:\n- Red: number is greater\n- Green: guess is correct') 168 | print('- Blue: number is smaller') 169 | 170 | search() 171 | 172 | reset_memory() 173 | error_detected = False 174 | 175 | again = input('Press Enter to restart a game or any key followed by Enter to exit: ') 176 | if(again != ''): 177 | break 178 | 179 | 180 | cozmo.robot.Robot.drive_off_charger_on_connect = True 181 | cozmo.run_program(cozmo_program,use_viewer=False,use_3d_viewer=False) 182 | -------------------------------------------------------------------------------- /guess_number/guess_number_anim.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Written by: @Luc (https://forums.anki.com/u/Luc/summary) 3 | ''' 4 | 5 | import cozmo 6 | from cozmo.objects import LightCube1Id, LightCube2Id, LightCube3Id 7 | from cozmo.util import degrees 8 | 9 | import time 10 | import numpy as np 11 | 12 | def find_face(robot: cozmo.robot.Robot): 13 | # Look around for a face 14 | behavior = robot.start_behavior(cozmo.behavior.BehaviorTypes.FindFaces) 15 | try: 16 | face_to_follow = robot.world.wait_for_observed_face(timeout=5) 17 | except: 18 | face_to_follow = None 19 | # Stop looking around 20 | behavior.stop() 21 | if(face_to_follow): 22 | turn_action = robot.turn_towards_face(face_to_follow).wait_for_completed() 23 | 24 | ''' -------------------------------------------------------------- ''' 25 | 26 | def play_animation(robot: cozmo.robot.Robot, anim_trig, body = False, para = False): 27 | # anim_trig = cozmo.anim.Triggers."Name of trigger" (this is an object) 28 | # Refer to "http://cozmosdk.anki.com/docs/generated/cozmo.anim.html#cozmo.anim.Triggers" for animations' triggers 29 | # Refer to "http://cozmosdk.anki.com/docs/generated/cozmo.robot.html#cozmo.robot.Robot.play_anim_trigger" for playing the animation 30 | 31 | robot.play_anim_trigger(anim_trig,loop_count = 1, in_parallel = para, 32 | num_retries = 0, use_lift_safe = False, 33 | ignore_body_track = body, ignore_head_track=False, 34 | ignore_lift_track=False).wait_for_completed() 35 | 36 | def hesitate_long(robot: cozmo.robot.Robot): 37 | trigger = cozmo.anim.Triggers.MeetCozmoFirstEnrollmentSayName #PatternGuessThinking #CodeLabThinking 38 | play_animation(robot,trigger) 39 | robot.turn_in_place(degrees(10)).wait_for_completed() 40 | 41 | def hesitate_short(robot: cozmo.robot.Robot): 42 | trigger = cozmo.anim.Triggers.PatternGuessThinking #CodeLabThinking 43 | play_animation(robot,trigger,True) 44 | 45 | def hesitate(robot: cozmo.robot.Robot): 46 | decide = np.random.random() 47 | if(decide > 0.5): 48 | hesitate_long(robot) 49 | else: 50 | hesitate_short(robot) 51 | transition(robot) 52 | 53 | def transition(robot: cozmo.robot.Robot): 54 | trigger = cozmo.anim.Triggers.MajorWin # mainly used to reset arms 55 | play_animation(robot,trigger, True) 56 | 57 | def sad(robot: cozmo.robot.Robot): 58 | trigger = cozmo.anim.Triggers.MajorFail 59 | play_animation(robot,trigger) 60 | 61 | def excited(robot: cozmo.robot.Robot): 62 | trigger = cozmo.anim.Triggers.CodeLabExcited 63 | play_animation(robot,trigger) 64 | robot.turn_in_place(degrees(-130)).wait_for_completed() 65 | 66 | def frustrated(robot: cozmo.robot.Robot): 67 | trigger = cozmo.anim.Triggers.FrustratedByFailureMajor 68 | play_animation(robot,trigger) 69 | 70 | def hiccup(robot: cozmo.robot.Robot): 71 | trigger = cozmo.anim.Triggers.Hiccup 72 | play_animation(robot,trigger) 73 | 74 | def wiggle(robot: cozmo.robot.Robot): 75 | trigger = cozmo.anim.Triggers.OnWiggle 76 | play_animation(robot,trigger) 77 | 78 | def success(robot: cozmo.robot.Robot): 79 | decide = np.random.random() 80 | if(decide > 0.4): 81 | excited(robot) 82 | else: 83 | wiggle(robot) 84 | 85 | def bored(robot: cozmo.robot.Robot): 86 | trigger1 = cozmo.anim.Triggers.NothingToDoBoredIntro 87 | trigger2 = cozmo.anim.Triggers.NothingToDoBoredEvent 88 | trigger3 = cozmo.anim.Triggers.NothingToDoBoredOutro 89 | play_animation(robot,trigger1) 90 | play_animation(robot,trigger2) 91 | play_animation(robot,trigger3) 92 | 93 | ''' -------------------------------------------------------------- ''' 94 | 95 | def propose_guess(robot: cozmo.robot.Robot, guess = ''): 96 | robot.say_text(guess, voice_pitch = 0.5,play_excited_animation=False).wait_for_completed() 97 | 98 | ''' -------------------------------------------------------------- ''' 99 | 100 | # The event handler is non-deterministic regarding the order of events 101 | ''' cube1 - red 102 | cube2 - green 103 | cube3 - blue''' 104 | cube1 = None 105 | cube2 = None 106 | cube3 = None 107 | # Mapping of events to id, cubes and commands. 108 | # Give ID returned by handler at event as parameter 109 | # to the following dictionnaries to retreive value 110 | ev_cube = {1:None,2:None,3:None} 111 | ev_command = {1:'',2:'',3:''} 112 | ev_light = {1:None,2:None,3:None} 113 | ev_id = 0 114 | 115 | def switch_cube_on(cube,light): 116 | cube.set_lights(light) 117 | 118 | def switch_cubes_on(): 119 | global cube1,cube2,cube3 120 | 121 | cube1.set_lights(cozmo.lights.red_light) 122 | cube2.set_lights(cozmo.lights.green_light) 123 | cube3.set_lights(cozmo.lights.blue_light) 124 | 125 | def switch_cubes_off(): 126 | global cube1,cube2,cube3 127 | 128 | cube1.set_lights_off() 129 | cube2.set_lights_off() 130 | cube3.set_lights_off() 131 | 132 | def handle_tap_init(evt, **kw): 133 | global ev_id 134 | ev_id = evt.obj.object_id 135 | 136 | 137 | def make_selected_cube_blink(cube,blink_light): 138 | # Animate tapped cube 139 | cube.set_light_corners(blink_light,blink_light,blink_light,blink_light) 140 | time.sleep(0.1) 141 | cube.set_light_corners(cozmo.lights.white_light,blink_light,blink_light,blink_light) 142 | time.sleep(0.1) 143 | cube.set_light_corners(blink_light,cozmo.lights.white_light,blink_light,blink_light) 144 | time.sleep(0.1) 145 | cube.set_light_corners(blink_light,blink_light,cozmo.lights.white_light,blink_light) 146 | time.sleep(0.1) 147 | cube.set_light_corners(blink_light,blink_light,blink_light,cozmo.lights.white_light) 148 | time.sleep(0.1) 149 | cube.set_lights(blink_light) 150 | time.sleep(0.1) 151 | switch_cubes_off() 152 | 153 | def init_cubes(robot: cozmo.robot.Robot): 154 | global cube1,cube2,cube3,ev_cube,ev_id,ev_command,ev_light 155 | cube1 = robot.world.get_light_cube(LightCube1Id) # looks like a paperclip 156 | cube2 = robot.world.get_light_cube(LightCube2Id) # looks like a lamp / heart 157 | cube3 = robot.world.get_light_cube(LightCube3Id) # looks like the letters 'ab' over 'T' 158 | 159 | # Make sure each cube is off 160 | switch_cubes_off() 161 | # Add listener to control initialisation 162 | handler = robot.add_event_handler(cozmo.objects.EvtObjectTapped, handle_tap_init) 163 | 164 | seq = ['red','green','blue'] 165 | light_seq = [cozmo.lights.red_light,cozmo.lights.green_light,cozmo.lights.blue_light] 166 | cube_seq = [cube1,cube2,cube3] 167 | comm_seq = ['g','c','s'] # greater,correct,smaller 168 | for i in range(0,3): 169 | print('Tap the ',seq[i],' cube') 170 | switch_cube_on(cube_seq[i],light_seq[i]) 171 | while(True): 172 | if(ev_id): 173 | # Map event to cube 174 | ev_cube[ev_id] = cube_seq[i] 175 | ev_command[ev_id] = comm_seq[i] 176 | ev_light[ev_id] = light_seq[i] 177 | make_selected_cube_blink(ev_cube[ev_id],light_seq[i]) 178 | switch_cubes_off() 179 | ev_id = 0 180 | break 181 | handler.disable() 182 | 183 | ''' -------------------------------------------------------------- ''' 184 | 185 | def handle_tapped(evt, **kw): 186 | global ev_id 187 | ev_id = evt.obj.object_id 188 | 189 | def get_answer_from_cubes(robot: cozmo.robot.Robot): 190 | global cube1,cube2,cube3,ev_cube,ev_id,ev_command,ev_light 191 | timeOut = 30 192 | timer = 0 193 | 194 | switch_cubes_on() 195 | handler = robot.add_event_handler(cozmo.objects.EvtObjectTapped, handle_tapped) 196 | 197 | while(True): 198 | time.sleep(0.5) 199 | timer += 0.5 200 | if(ev_id): 201 | switch_cubes_off() 202 | make_selected_cube_blink(ev_cube[ev_id],ev_light[ev_id]) 203 | local_id = ev_id 204 | ev_id = 0 # Deactivate selection 205 | handler.disable() 206 | return ev_command[local_id] 207 | elif(timer > timeOut): 208 | handler.disable() 209 | return 'timeOut' 210 | -------------------------------------------------------------------------------- /self_docking/self_docking.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Written by @Luc, https://forums.anki.com/u/Luc/activity 3 | 4 | This program enables Cozmo to drive off its charger, collect its cubes 5 | by placing them near its charger and then climbing back onto its 6 | charger when it is done. Cozmo will directly try to climb back onto its charger 7 | if the battery is low. No further help or markers are required. 8 | It is possible that Cozmo does not find a cube or its charger, 9 | so it will ask for help if needed. Just place the required object 10 | or Cozmo in a position where Cozmo can keep working and let it finish its job! 11 | 12 | TODO (improvements): 13 | - Support already stacked or rolled cubes when cleaning up. 14 | - Stack cubes as a pyramid instead of current configuration. 15 | - Speed up some processes. 16 | ''' 17 | 18 | import cozmo 19 | from cozmo.util import degrees, radians, distance_mm, speed_mmps 20 | from cozmo.objects import LightCube1Id, LightCube2Id, LightCube3Id 21 | 22 | import time 23 | import math 24 | 25 | # Choose on which side of the charger (when facing it), 26 | # Cozmo should put its cubes. 27 | # SIDE = 1 (left), SIDE = -1 (right) 28 | SIDE = -1 29 | 30 | # Pitch value when head is horizonal, calculated later. 31 | pitch_threshold = 0 32 | 33 | # Define how many search cycles must be engaged (5 sec each) while looking for cubes. 34 | # When this number is reached, Cozmo will stop looking for its cubes and will drive 35 | # back onto its charger. 36 | retries = 5 37 | 38 | ########################## Animations ########################## 39 | 40 | def play_animation(robot: cozmo.robot.Robot, anim_trig, body = False, lift = False, para = False): 41 | # anim_trig = cozmo.anim.Triggers."Name of trigger" (this is an object) 42 | # Refer to "http://cozmosdk.anki.com/docs/generated/cozmo.anim.html#cozmo.anim.Triggers" for animations' triggers 43 | # Refer to "http://cozmosdk.anki.com/docs/generated/cozmo.robot.html#cozmo.robot.Robot.play_anim_trigger" for playing the animation 44 | 45 | robot.play_anim_trigger(anim_trig,loop_count = 1, in_parallel = para, 46 | num_retries = 0, use_lift_safe = False, 47 | ignore_body_track = body, ignore_head_track=False, 48 | ignore_lift_track = lift).wait_for_completed() 49 | 50 | def frustrated(robot: cozmo.robot.Robot): 51 | trigger = cozmo.anim.Triggers.FrustratedByFailureMajor 52 | play_animation(robot,trigger) 53 | 54 | def celebrate(robot: cozmo.robot.Robot): 55 | trigger = cozmo.anim.Triggers.CodeLabCelebrate 56 | play_animation(robot,trigger,body=True,lift=True,para=True) 57 | 58 | ########################## Cube lights related code ########################## 59 | 60 | def switch_cube_on(cube: cozmo.objects.LightCube): 61 | cube.set_lights(cozmo.lights.green_light) 62 | return 63 | 64 | def switch_cube_off(cube: cozmo.objects.LightCube): 65 | cube.set_lights_off() 66 | return 67 | 68 | ########################## Cubes related code ########################## 69 | 70 | # Cozmo's memory to know which cube still 71 | # needs to be put next to the charger 72 | cubeIDs = [] 73 | cubes = [] 74 | 75 | def look_for_next_cube(): 76 | # look for a cube that was not put next to the charger yet 77 | global robot, cubeIDs, cubes, retries 78 | 79 | i = 0 80 | valid = True 81 | while(True): 82 | i += 1 83 | behavior = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) 84 | try: 85 | seen_cube = robot.world.wait_for_observed_light_cube(timeout=5,include_existing=True) 86 | except: 87 | seen_cube = None 88 | behavior.stop() 89 | 90 | if(seen_cube == None): 91 | frustrated(robot) 92 | robot.say_text('Cube?',duration_scalar=0.5).wait_for_completed() 93 | else: 94 | # A cube was seen but we have to know if it has to be moved 95 | if(len(cubeIDs) == 0): 96 | cubeIDs.append(seen_cube.object_id) 97 | cubes.append(seen_cube) 98 | return seen_cube # communicate that this cube has to be moved 99 | else: 100 | for i in range(0,len(cubeIDs)): 101 | if(seen_cube.object_id == cubeIDs[i]): 102 | valid = False # that cube was already taken care of 103 | if(valid): 104 | cubeIDs.append(seen_cube.object_id) 105 | cubes.append(seen_cube) 106 | return seen_cube 107 | else: 108 | valid = True 109 | 110 | if(i >= retries): 111 | print('ERROR: Aborting clean up, Cozmo cannot find all its cubes.') 112 | break 113 | return 0 114 | 115 | def pickUp_cube(cube: cozmo.objects.LightCube): 116 | global robot 117 | action = robot.pickup_object(cube,use_pre_dock_pose=True, in_parallel=False, num_retries=5) 118 | action.wait_for_completed() 119 | if(action.has_failed): 120 | print('ERROR: Picking cube ' + str(cube.cube_id) + ' has failed.') 121 | print('MESSAGE: Aborting clean up, engaging docking procedure.') 122 | return 0 123 | return 1 124 | def putDown_cube(cube: cozmo.objects.LightCube): 125 | global robot 126 | action = robot.place_object_on_ground_here(cube, in_parallel=False, num_retries=2) 127 | action.wait_for_completed() 128 | return 1 129 | def stack_cube(target: cozmo.objects.LightCube): 130 | global robot 131 | action = robot.place_on_object(target, in_parallel=False, num_retries=2) 132 | action.wait_for_completed() 133 | return 1 134 | 135 | def clean_up_cubes(): 136 | global cubeIDs,SIDE 137 | 138 | cubeIDs = [] 139 | switch_cube_off(robot.world.get_light_cube(LightCube1Id)) 140 | switch_cube_off(robot.world.get_light_cube(LightCube2Id)) 141 | switch_cube_off(robot.world.get_light_cube(LightCube3Id)) 142 | 143 | go_to_charger() 144 | turn_around() 145 | 146 | # I. Find first cube and put it next to charger 147 | cube = look_for_next_cube() 148 | if not cube: 149 | return 150 | switch_cube_on(cube) 151 | print(cubeIDs) 152 | success = pickUp_cube(cube) 153 | if not success: 154 | switch_cube_off(cube) 155 | return 156 | charger = go_to_charger() 157 | final_adjust(charger,60) 158 | robot.turn_in_place(degrees(SIDE*60)).wait_for_completed() 159 | robot.drive_straight(distance_mm(100),speed_mmps(40)).wait_for_completed() 160 | putDown_cube(cube) 161 | switch_cube_off(cube) 162 | # Get back in front of charger 163 | robot.drive_straight(distance_mm(-70),speed_mmps(40)).wait_for_completed() 164 | robot.turn_in_place(degrees(SIDE*180-SIDE*60)).wait_for_completed() 165 | robot.drive_straight(distance_mm(40),speed_mmps(40)).wait_for_completed() 166 | 167 | # II. Find next cube and stack on first placed cube 168 | cube = look_for_next_cube() 169 | if(cube == None): 170 | return 171 | switch_cube_on(cube) 172 | print(cubeIDs) 173 | success = pickUp_cube(cube) 174 | if not success: 175 | switch_cube_off(cube) 176 | return 177 | charger = go_to_charger() 178 | robot.turn_in_place(degrees(SIDE*70)).wait_for_completed() #SIDE*180 179 | stack_cube(cubes[0]) 180 | switch_cube_off(cube) 181 | # Get back in front of charger 182 | #robot.drive_straight(distance_mm(-70),speed_mmps(40)).wait_for_completed() 183 | charger = go_to_charger() 184 | final_adjust(charger,100,80) 185 | robot.turn_in_place(degrees(SIDE*180)).wait_for_completed() #-SIDE*70 186 | robot.set_lift_height(height=0,max_speed=10,in_parallel=False).wait_for_completed() 187 | #robot.drive_straight(distance_mm(40),speed_mmps(40)).wait_for_completed() 188 | 189 | # III. Go get the last cube and lay it in front of the others 190 | cube = look_for_next_cube() 191 | if(cube == None): 192 | return 193 | switch_cube_on(cube) 194 | print(cubeIDs) 195 | success = pickUp_cube(cube) 196 | if not success: 197 | switch_cube_off(cube) 198 | return 199 | turn_around() 200 | charger = go_to_charger() 201 | final_adjust(charger,80) 202 | robot.turn_in_place(degrees(SIDE*60)).wait_for_completed() 203 | robot.go_to_object(cubes[0], distance_mm(120), in_parallel=False, num_retries=1).wait_for_completed() 204 | putDown_cube(cube) 205 | switch_cube_off(cube) 206 | # Get back in front of charger 207 | robot.drive_straight(distance_mm(-40),speed_mmps(40)).wait_for_completed() 208 | robot.turn_in_place(degrees(SIDE*180-SIDE*60)).wait_for_completed() 209 | robot.drive_straight(distance_mm(50),speed_mmps(40)).wait_for_completed() 210 | turn_around() 211 | return 212 | 213 | ########################## Charger related code ########################## 214 | 215 | def find_charger(): 216 | global robot 217 | 218 | while(True): 219 | 220 | behavior = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) 221 | try: 222 | seen_charger = robot.world.wait_for_observed_charger(timeout=10,include_existing=True) 223 | except: 224 | seen_charger = None 225 | behavior.stop() 226 | if(seen_charger != None): 227 | #print(seen_charger) 228 | return seen_charger 229 | frustrated(robot) 230 | robot.say_text('Charge?',duration_scalar=0.5).wait_for_completed() 231 | return None 232 | 233 | def go_to_charger(): 234 | # Driving towards charger without much precision 235 | global robot 236 | 237 | charger = None 238 | ''' cf. 08_drive_to_charger_test.py ''' 239 | # see if Cozmo already knows where the charger is 240 | if robot.world.charger: 241 | # make sure Cozmo was not delocalised after observing the charger 242 | if robot.world.charger.pose.is_comparable(robot.pose): 243 | print("Cozmo already knows where the charger is!") 244 | charger = robot.world.charger 245 | else: 246 | # Cozmo knows about the charger, but the pose is not based on the 247 | # same origin as the robot (e.g. the robot was moved since seeing 248 | # the charger) so try to look for the charger first 249 | pass 250 | if not charger: 251 | charger = find_charger() 252 | 253 | action = robot.go_to_object(charger,distance_from_object=distance_mm(80), in_parallel=False, num_retries=5) 254 | #action = robot.go_to_pose(charger.pose) 255 | action.wait_for_completed() 256 | return charger 257 | 258 | def disp_coord(charger: cozmo.objects.Charger): 259 | # Debugging function used to diplay coordinates of objects 260 | # (Not currently used) 261 | global robot 262 | 263 | r_coord = robot.pose.position #.x .y .z, .rotation otherwise 264 | r_zRot = robot.pose_angle.degrees # or .radians 265 | c_coord = charger.pose.position 266 | c_zRot = charger.pose.rotation.angle_z.degrees 267 | 268 | print('Recorded coordinates of the robot and charger:') 269 | print('Robot: ',end=' ') 270 | print(r_coord) 271 | print(r_zRot) 272 | print('Charger: ',end=' ') 273 | print(c_coord) 274 | print(c_zRot) 275 | print('\n') 276 | 277 | PI = 3.14159265359 278 | def clip_angle(angle=3.1415): 279 | # Allow Cozmo to turn the least possible. Without it, Cozmo could 280 | # spin on itself several times or turn for instance -350 degrees 281 | # instead of 10 degrees. 282 | global PI 283 | 284 | # Retreive supplementary turns (in radians) 285 | while(angle >= 2*PI): 286 | angle -= 2*PI 287 | while(angle <= -2*PI): 288 | angle += 2*PI 289 | # Select shortest rotation to reach the target 290 | if(angle > PI): 291 | angle -= 2*PI 292 | elif(angle < -PI): 293 | angle += 2*PI 294 | return angle 295 | 296 | def check_tol(charger: cozmo.objects.Charger,dist_charger=40): 297 | # Check if the position tolerance in front of the charger is respected 298 | global robot,PI 299 | 300 | distance_tol = 5 # mm, tolerance for placement error 301 | angle_tol = 5*PI/180 # rad, tolerance for orientation error 302 | 303 | try: 304 | charger = robot.world.wait_for_observed_charger(timeout=2,include_existing=True) 305 | except: 306 | print('WARNING: Cannot see the charger to verify the position.') 307 | 308 | # Calculate positions 309 | r_coord = [0,0,0] 310 | c_coord = [0,0,0] 311 | # Coordonates of robot and charger 312 | r_coord[0] = robot.pose.position.x #.x .y .z, .rotation otherwise 313 | r_coord[1] = robot.pose.position.y 314 | r_coord[2] = robot.pose.position.z 315 | r_zRot = robot.pose_angle.radians # .degrees or .radians 316 | c_coord[0] = charger.pose.position.x 317 | c_coord[1] = charger.pose.position.y 318 | c_coord[2] = charger.pose.position.z 319 | c_zRot = charger.pose.rotation.angle_z.radians 320 | 321 | # Create target position 322 | # dist_charger in mm, distance if front of charger 323 | c_coord[0] -= dist_charger*math.cos(c_zRot) 324 | c_coord[1] -= dist_charger*math.sin(c_zRot) 325 | 326 | # Direction and distance to target position (in front of charger) 327 | distance = math.sqrt((c_coord[0]-r_coord[0])**2 + (c_coord[1]-r_coord[1])**2 + (c_coord[2]-r_coord[2])**2) 328 | 329 | if(distance < distance_tol and math.fabs(r_zRot-c_zRot) < angle_tol): 330 | return 1 331 | else: 332 | return 0 333 | 334 | def final_adjust(charger: cozmo.objects.Charger,dist_charger=40,speed=20,critical=False): 335 | # Final adjustement to properly face the charger. 336 | # The position can be adjusted several times if 337 | # the precision is critical, i.e. when climbing 338 | # back onto the charger. 339 | global robot,PI 340 | 341 | while(True): 342 | # Calculate positions 343 | r_coord = [0,0,0] 344 | c_coord = [0,0,0] 345 | # Coordonates of robot and charger 346 | r_coord[0] = robot.pose.position.x #.x .y .z, .rotation otherwise 347 | r_coord[1] = robot.pose.position.y 348 | r_coord[2] = robot.pose.position.z 349 | r_zRot = robot.pose_angle.radians # .degrees or .radians 350 | c_coord[0] = charger.pose.position.x 351 | c_coord[1] = charger.pose.position.y 352 | c_coord[2] = charger.pose.position.z 353 | c_zRot = charger.pose.rotation.angle_z.radians 354 | 355 | # Create target position 356 | # dist_charger in mm, distance if front of charger 357 | c_coord[0] -= dist_charger*math.cos(c_zRot) 358 | c_coord[1] -= dist_charger*math.sin(c_zRot) 359 | 360 | # Direction and distance to target position (in front of charger) 361 | distance = math.sqrt((c_coord[0]-r_coord[0])**2 + (c_coord[1]-r_coord[1])**2 + (c_coord[2]-r_coord[2])**2) 362 | vect = [c_coord[0]-r_coord[0],c_coord[1]-r_coord[1],c_coord[2]-r_coord[2]] 363 | # Angle of vector going from robot's origin to target's position 364 | theta_t = math.atan2(vect[1],vect[0]) 365 | 366 | print('CHECK: Adjusting position') 367 | # Face the target position 368 | angle = clip_angle((theta_t-r_zRot)) 369 | robot.turn_in_place(radians(angle)).wait_for_completed() 370 | # Drive toward the target position 371 | robot.drive_straight(distance_mm(distance),speed_mmps(speed)).wait_for_completed() 372 | # Face the charger 373 | angle = clip_angle((c_zRot-theta_t)) 374 | robot.turn_in_place(radians(angle)).wait_for_completed() 375 | 376 | # In case the robot does not need to climb onto the charger 377 | if not critical: 378 | break 379 | elif(check_tol(charger,dist_charger)): 380 | print('CHECK: Robot aligned relativ to the charger.') 381 | break 382 | return 383 | 384 | def restart_procedure(charger: cozmo.objects.Charger): 385 | global robot 386 | 387 | robot.stop_all_motors() 388 | robot.set_lift_height(height=0.5,max_speed=10,in_parallel=True).wait_for_completed() 389 | robot.pose.invalidate() 390 | charger.pose.invalidate() 391 | print('ABORT: Driving away') 392 | #robot.drive_straight(distance_mm(150),speed_mmps(80),in_parallel=False).wait_for_completed() 393 | robot.drive_wheels(80,80,duration=2) 394 | turn_around() 395 | robot.set_lift_height(height=0,max_speed=10,in_parallel=True).wait_for_completed() 396 | # Restart procedure 397 | get_on_charger() 398 | return 399 | 400 | def get_on_charger(): 401 | global robot,pitch_threshold 402 | 403 | robot.set_head_angle(degrees(0),in_parallel=False).wait_for_completed() 404 | pitch_threshold = math.fabs(robot.pose_pitch.degrees) 405 | pitch_threshold += 1 # Add 1 degree to threshold 406 | print('Pitch threshold: ' + str(pitch_threshold)) 407 | 408 | # Drive towards charger 409 | go_to_charger() 410 | 411 | # Let Cozmo first look for the charger once again. The coordinates 412 | # tend to be too unprecise if an old coordinate system is kept. 413 | if robot.world.charger is not None and robot.world.charger.pose.is_comparable(robot.pose): 414 | robot.world.charger.pose.invalidate() 415 | charger = find_charger() 416 | 417 | # Adjust position in front of the charger 418 | final_adjust(charger,critical=True) 419 | 420 | # Turn around and start going backward 421 | turn_around() 422 | robot.drive_wheel_motors(-120,-120) 423 | robot.set_lift_height(height=0.5,max_speed=10,in_parallel=True).wait_for_completed() 424 | robot.set_head_angle(degrees(0),in_parallel=True).wait_for_completed() 425 | 426 | # This section allow to wait for Cozmo to arrive on its charger 427 | # and detect eventual errors. The whole procedure will be restarted 428 | # in case something goes wrong. 429 | timeout = 1 # seconds before timeout 430 | t = 0 431 | # Wait for back wheels to climb on charger 432 | while(True): 433 | time.sleep(.1) 434 | t += 0.1 435 | if(t >= timeout): 436 | print('ERROR: robot timed out before climbing on charger.') 437 | restart_procedure(charger) 438 | return 439 | elif(math.fabs(robot.pose_pitch.degrees) >= pitch_threshold): 440 | print('CHECK: backwheels on charger.') 441 | break 442 | # Wait for front wheels to climb on charger 443 | timeout = 2 444 | t = 0 445 | while(True): 446 | time.sleep(.1) 447 | t += 0.1 448 | if(math.fabs(robot.pose_pitch.degrees) > 20 or t >= timeout): 449 | # The robot is climbing on charger's wall -> restart 450 | print('ERROR: robot climbed on charger\'s wall or timed out.') 451 | restart_procedure(charger) 452 | return 453 | elif(math.fabs(robot.pose_pitch.degrees) < pitch_threshold): 454 | print('CHECK: robot on charger, backing up on pins.') 455 | robot.stop_all_motors() 456 | break 457 | 458 | # Final backup onto charger's contacts 459 | robot.set_lift_height(height=0,max_speed=10,in_parallel=True).wait_for_completed() 460 | robot.backup_onto_charger(max_drive_time=3) 461 | if(robot.is_on_charger): 462 | print('PROCEDURE SUCCEEDED') 463 | else: 464 | restart_procedure(charger) 465 | return 466 | 467 | # Celebrate success 468 | robot.drive_off_charger_contacts().wait_for_completed() 469 | celebrate(robot) # A small celebration where only the head moves 470 | robot.backup_onto_charger(max_drive_time=3) 471 | return 472 | 473 | ########################## Battery related code ########################## 474 | 475 | def check_battery(): 476 | global robot 477 | 478 | if(robot.battery_voltage < 3.5): 479 | print('Battery is low, directly docking to charger without cleaning up.') 480 | return 1 481 | else: 482 | return 0 483 | 484 | ########################## General code ########################## 485 | 486 | def turn_around(): 487 | global robot 488 | robot.turn_in_place(degrees(-178)).wait_for_completed() 489 | return 490 | 491 | ########################## Main ########################## 492 | 493 | robot = None 494 | 495 | def init_robot(cozmo_robot: cozmo.robot.Robot): 496 | global robot 497 | robot = cozmo_robot 498 | 499 | def execute_procedure(): 500 | low_battery = check_battery() 501 | if not low_battery: 502 | clean_up_cubes() 503 | get_on_charger() 504 | return 505 | 506 | def cozmo_program(cozmo_robot: cozmo.robot.Robot): 507 | global robot 508 | init_robot(cozmo_robot) 509 | 510 | # Get off charger if on it 511 | if(robot.is_on_charger): 512 | robot.drive_off_charger_contacts().wait_for_completed() 513 | robot.drive_straight(distance_mm(110),speed_mmps(80)).wait_for_completed() 514 | 515 | execute_procedure() 516 | return 517 | 518 | if __name__ == "__main__": 519 | cozmo.robot.Robot.drive_off_charger_on_connect = False 520 | cozmo.run_program(cozmo_program,use_viewer=True,use_3d_viewer=False) 521 | -------------------------------------------------------------------------------- /self_docking/self_docking_v2.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Written by @Luc, https://forums.anki.com/u/Luc/activity 3 | 4 | This program enables Cozmo to drive off its charger, collect its cubes 5 | by placing them near its charger and then climbing back onto its 6 | charger when it is done. Cozmo will directly try to climb back onto its charger 7 | if the battery is low. No further help or markers are required. 8 | It is possible that Cozmo does not find a cube or its charger, 9 | so it will ask for help if needed. Just place the required object 10 | or Cozmo in a position where Cozmo can keep working and let it finish its job! 11 | 12 | This version supports stacked cubes (2 or even 3). 13 | 14 | TODO (improvements): 15 | - Stack cubes as a pyramid instead of current configuration. 16 | ''' 17 | 18 | import cozmo 19 | from cozmo.util import degrees, radians, distance_mm, speed_mmps 20 | from cozmo.objects import LightCube1Id, LightCube2Id, LightCube3Id 21 | 22 | import asyncio 23 | import time 24 | import math 25 | 26 | # Choose on which side of the charger (when facing it), Cozmo should put its cubes. 27 | # SIDE = 1 (left), SIDE = -1 (right) 28 | SIDE = -1 29 | 30 | # Pitch value when head is horizonal, calculated later. 31 | pitch_threshold = 0 32 | 33 | # Define how many search cycles must be engaged (5 sec each) while looking for cubes. 34 | # When this number is reached, Cozmo will stop looking for its cubes and will drive 35 | # back onto its charger. 36 | retries = 5 37 | 38 | ########################## Animations ########################## 39 | 40 | def play_animation(robot: cozmo.robot.Robot, anim_trig, body = False, lift = False, parallel = False): 41 | # anim_trig = cozmo.anim.Triggers."Name of trigger" (this is an object) 42 | # Refer to "http://cozmosdk.anki.com/docs/generated/cozmo.anim.html#cozmo.anim.Triggers" for animations' triggers 43 | # Refer to "http://cozmosdk.anki.com/docs/generated/cozmo.robot.html#cozmo.robot.Robot.play_anim_trigger" for playing the animation 44 | 45 | robot.play_anim_trigger(anim_trig,loop_count = 1, in_parallel = parallel, 46 | num_retries = 0, use_lift_safe = False, 47 | ignore_body_track = body, ignore_head_track=False, 48 | ignore_lift_track = lift).wait_for_completed() 49 | 50 | def frustrated(robot: cozmo.robot.Robot): 51 | trigger = cozmo.anim.Triggers.FrustratedByFailureMajor 52 | play_animation(robot,trigger) 53 | 54 | def celebrate(robot: cozmo.robot.Robot): 55 | trigger = cozmo.anim.Triggers.CodeLabCelebrate 56 | play_animation(robot,trigger,body=True,lift=True,parallel=True) 57 | 58 | ########################## Cube lights related code ########################## 59 | 60 | def switch_cube_on(cube: cozmo.objects.LightCube): 61 | cube.set_lights(cozmo.lights.green_light) 62 | return 63 | 64 | def switch_cube_off(cube: cozmo.objects.LightCube): 65 | cube.set_lights_off() 66 | return 67 | 68 | ########################## Cubes related code ########################## 69 | 70 | # Cozmo's memory to know which cubes are already placed near the charger 71 | cubeIDs = [] 72 | cubes = [] 73 | 74 | def putDown_cube(cube: cozmo.objects.LightCube): 75 | global robot 76 | action = robot.place_object_on_ground_here(cube, in_parallel=False, num_retries=2) 77 | action.wait_for_completed() 78 | if(action.has_failed): 79 | return 0 80 | return 1 81 | def stack_cube(target: cozmo.objects.LightCube): 82 | global robot 83 | action = robot.place_on_object(target, in_parallel=False, num_retries=2) 84 | action.wait_for_completed() 85 | if(action.has_failed): 86 | return 0 87 | return 1 88 | 89 | def look_for_next_cube(): 90 | # look for any cube that was not put next to the charger yet 91 | global robot, cubeIDs, cubes, retries 92 | 93 | i = 0 94 | valid = True 95 | while(True): 96 | i += 1 97 | # Make time to see a possible stacked cube 98 | try: 99 | seen_cube = robot.world.wait_for_observed_light_cube(timeout=.5,include_existing=True) 100 | except: 101 | seen_cube = None 102 | if(seen_cube is None): 103 | # There was no stacked cube 104 | behavior = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) 105 | try: 106 | seen_cube = robot.world.wait_for_observed_light_cube(timeout=5,include_existing=True) 107 | except: 108 | seen_cube = None 109 | behavior.stop() 110 | 111 | if(seen_cube == None): 112 | frustrated(robot) 113 | robot.say_text('Cube?',duration_scalar=0.5).wait_for_completed() 114 | else: 115 | # A cube was seen but we have to know if it has to be moved 116 | if(len(cubeIDs) == 0): 117 | cubeIDs.append(seen_cube.object_id) 118 | cubes.append(seen_cube) 119 | return seen_cube # communicate that this cube has to be moved 120 | else: 121 | for i in range(0,len(cubeIDs)): 122 | if(seen_cube.object_id == cubeIDs[i]): 123 | valid = False # that cube was already taken care of 124 | if(valid): 125 | cubeIDs.append(seen_cube.object_id) 126 | cubes.append(seen_cube) 127 | return seen_cube 128 | else: 129 | valid = True 130 | 131 | if(i >= retries): 132 | print('ERROR: Aborting clean up, Cozmo cannot find all its cubes.') 133 | break 134 | return 0 135 | 136 | def try_picking_up_cube(): # cube: cozmo.objects.LightCube 137 | # Look for a cube and try picking it up. If another cube is located 138 | # on the one Cozmo is trying to pick up, Cozmo will switch targets 139 | # and pick the upper cube. 140 | # 0: action failed 141 | # 1: action succeeded 142 | global robot, cubeIDs, cubes 143 | 144 | num_fail = 5 145 | for failures in range(1,num_fail+1): 146 | cube = look_for_next_cube() 147 | switch_cube_on(cube) 148 | action = robot.pickup_object(cube,use_pre_dock_pose=True, in_parallel=False, num_retries=1) 149 | action.wait_for_completed() 150 | if(action.has_failed): 151 | print('The cube cannot be picked, checking for stacked cube.') 152 | switch_cube_off(cube) 153 | # Look up to try and find a stacked cube 154 | robot.set_head_angle(degrees(20),in_parallel=False).wait_for_completed() 155 | # Pop last cube from memory that raised an exception 156 | cubes.pop() 157 | cubeIDs.pop() 158 | # If no exception is raised, check for action failure 159 | else: 160 | print(cubeIDs) 161 | break 162 | if(failures >= num_fail): 163 | print('ERROR: Picking cube ' + str(cube.cube_id) + ' has failed.') 164 | print('MESSAGE: Aborting clean up, engaging docking procedure.') 165 | return 0, None 166 | else: 167 | return 1, cube 168 | 169 | def transport_cube_I(cube: cozmo.objects.LightCube): 170 | global robot,SIDE 171 | 172 | # First cube was picked, place it next to the charger 173 | charger = go_to_charger() 174 | final_adjust(charger,60) 175 | robot.turn_in_place(degrees(SIDE*60)).wait_for_completed() 176 | robot.drive_straight(distance_mm(100),speed_mmps(40)).wait_for_completed() 177 | putDown_cube(cube) 178 | switch_cube_off(cube) 179 | # Get back in front of charger 180 | robot.drive_straight(distance_mm(-70),speed_mmps(40)).wait_for_completed() 181 | robot.turn_in_place(degrees(SIDE*180-SIDE*60)).wait_for_completed() 182 | robot.drive_straight(distance_mm(40),speed_mmps(40)).wait_for_completed() 183 | return 184 | 185 | def transport_cube_II(cube: cozmo.objects.LightCube): 186 | global robot,SIDE 187 | 188 | # Second cube was picked, place it on the first cube 189 | charger = go_to_charger() 190 | robot.turn_in_place(degrees(SIDE*70)).wait_for_completed() #SIDE*180 191 | stack_cube(cubes[0]) 192 | switch_cube_off(cube) 193 | # Get back in front of charger 194 | charger = go_to_charger() 195 | final_adjust(charger,100,80) 196 | robot.turn_in_place(degrees(SIDE*180)).wait_for_completed() #-SIDE*70 197 | robot.set_lift_height(height=0,max_speed=10,in_parallel=False).wait_for_completed() 198 | return 199 | 200 | def transport_cube_III(cube: cozmo.objects.LightCube): 201 | global robot,SIDE 202 | 203 | # Third cube was picked, go place it next to the others 204 | charger = go_to_charger() 205 | final_adjust(charger,80) 206 | robot.turn_in_place(degrees(SIDE*60)).wait_for_completed() 207 | robot.go_to_object(cubes[0], distance_mm(120), in_parallel=False, num_retries=1).wait_for_completed() 208 | putDown_cube(cube) 209 | switch_cube_off(cube) 210 | # Get back in front of charger 211 | robot.drive_straight(distance_mm(-40),speed_mmps(40)).wait_for_completed() 212 | robot.turn_in_place(degrees(-SIDE*60)).wait_for_completed() 213 | return 214 | 215 | def knock_cubes_over(): 216 | # This function allows to detect if the three cubes are stacked on each others. 217 | # If it is the case, Cozmo will try to make them fall. 218 | global robot 219 | 220 | cube1 = robot.world.get_light_cube(LightCube1Id) 221 | cube2 = robot.world.get_light_cube(LightCube2Id) 222 | cube3 = robot.world.get_light_cube(LightCube3Id) 223 | 224 | retries = 5 225 | i = 0 226 | 227 | for i in range(0,5): 228 | # Try to see at least 1 cube 229 | behavior = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) 230 | try: 231 | seen_cube = robot.world.wait_for_observed_light_cube(timeout=10,include_existing=True) 232 | except: 233 | seen_cube = None 234 | behavior.stop() 235 | # Try to observe possible stacked cubes 236 | robot.set_head_angle(degrees(10)).wait_for_completed() 237 | time.sleep(.5) 238 | robot.set_head_angle(degrees(30)).wait_for_completed() 239 | time.sleep(.5) 240 | robot.set_head_angle(degrees(0)).wait_for_completed() 241 | 242 | num_observed_cubes = 0 243 | if(cube1.pose.is_comparable(robot.pose)): 244 | num_observed_cubes += 1 245 | if(cube2.pose.is_comparable(robot.pose)): 246 | num_observed_cubes += 1 247 | if(cube3.pose.is_comparable(robot.pose)): 248 | num_observed_cubes += 1 249 | 250 | if(num_observed_cubes == 3): 251 | # All cubes were observed, check if stacked 252 | ref = [] 253 | ref.append(cube1.pose.position.x) 254 | ref.append(cube1.pose.position.y) 255 | tol = 20 # Less than 20 mm means that the cubes are stacked 256 | delta2 = math.sqrt((ref[0]-cube2.pose.position.x)**2 + (ref[1]-cube2.pose.position.y)**2) 257 | delta3 = math.sqrt((ref[0]-cube3.pose.position.x)**2 + (ref[1]-cube3.pose.position.y)**2) 258 | 259 | if(delta2 < tol and delta3 < tol): 260 | try: 261 | behavior = robot.start_behavior(cozmo.behavior.BehaviorTypes.KnockOverCubes) 262 | behavior.wait_for_started(timeout=10) 263 | behavior.wait_for_completed(timeout=None) 264 | except asyncio.TimeoutError: 265 | print('WARNING: Timeout exception raised from behavior type KnockOverCubes.') 266 | except: 267 | print('WARNING: An exception was raised from behavior type KnockOverCubes.') 268 | else: 269 | robot.drive_straight(distance_mm(-50),speed_mmps(80)).wait_for_completed() 270 | return 1 271 | else: 272 | robot.drive_straight(distance_mm(-50),speed_mmps(80)).wait_for_completed() 273 | return 1 274 | if(i >= 4): 275 | return 0 276 | 277 | def clean_up_cubes(): 278 | switch_cube_off(robot.world.get_light_cube(LightCube1Id)) 279 | switch_cube_off(robot.world.get_light_cube(LightCube2Id)) 280 | switch_cube_off(robot.world.get_light_cube(LightCube3Id)) 281 | 282 | go_to_charger() 283 | turn_around() 284 | 285 | if(knock_cubes_over() == 0): 286 | return 287 | 288 | step = 1 289 | while(True): 290 | action_result, cube = try_picking_up_cube() 291 | 292 | if(action_result == 0): 293 | # Cozmo failed to pick a cube, abort clean up. 294 | break 295 | 296 | if(step == 1): 297 | transport_cube_I(cube) 298 | step += 1 299 | elif(step == 2): 300 | transport_cube_II(cube) 301 | step += 1 302 | elif(step == 3): 303 | transport_cube_III(cube) 304 | break 305 | return 306 | 307 | ########################## Charger related code ########################## 308 | 309 | def find_charger(): 310 | global robot 311 | 312 | while(True): 313 | 314 | behavior = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) 315 | try: 316 | seen_charger = robot.world.wait_for_observed_charger(timeout=10,include_existing=True) 317 | except: 318 | seen_charger = None 319 | behavior.stop() 320 | if(seen_charger != None): 321 | #print(seen_charger) 322 | return seen_charger 323 | frustrated(robot) 324 | robot.say_text('Charge?',duration_scalar=0.5).wait_for_completed() 325 | return None 326 | 327 | def go_to_charger(): 328 | # Driving towards charger without much precision 329 | global robot 330 | 331 | charger = None 332 | ''' cf. 08_drive_to_charger_test.py ''' 333 | # see if Cozmo already knows where the charger is 334 | if robot.world.charger: 335 | # make sure Cozmo was not delocalised after observing the charger 336 | if robot.world.charger.pose.is_comparable(robot.pose): 337 | print("Cozmo already knows where the charger is!") 338 | charger = robot.world.charger 339 | else: 340 | # Cozmo knows about the charger, but the pose is not based on the 341 | # same origin as the robot (e.g. the robot was moved since seeing 342 | # the charger) so try to look for the charger first 343 | pass 344 | if not charger: 345 | charger = find_charger() 346 | 347 | action = robot.go_to_object(charger,distance_from_object=distance_mm(80), in_parallel=False, num_retries=5) 348 | #action = robot.go_to_pose(charger.pose) 349 | action.wait_for_completed() 350 | return charger 351 | 352 | def disp_coord(charger: cozmo.objects.Charger): 353 | # Debugging function used to diplay coordinates of objects 354 | # (Not currently used) 355 | global robot 356 | 357 | r_coord = robot.pose.position #.x .y .z, .rotation otherwise 358 | r_zRot = robot.pose_angle.degrees # or .radians 359 | c_coord = charger.pose.position 360 | c_zRot = charger.pose.rotation.angle_z.degrees 361 | 362 | print('Recorded coordinates of the robot and charger:') 363 | print('Robot:',end=' ') 364 | print(r_coord) 365 | print(r_zRot) 366 | print('Charger:',end=' ') 367 | print(c_coord) 368 | print(c_zRot) 369 | print('\n') 370 | 371 | PI = 3.14159265359 372 | def clip_angle(angle=3.1415): 373 | # Allow Cozmo to turn the least possible. Without it, Cozmo could 374 | # spin on itself several times or turn for instance -350 degrees 375 | # instead of 10 degrees. 376 | global PI 377 | 378 | # Retreive supplementary turns (in radians) 379 | while(angle >= 2*PI): 380 | angle -= 2*PI 381 | while(angle <= -2*PI): 382 | angle += 2*PI 383 | # Select shortest rotation to reach the target 384 | if(angle > PI): 385 | angle -= 2*PI 386 | elif(angle < -PI): 387 | angle += 2*PI 388 | return angle 389 | 390 | def check_tol(charger: cozmo.objects.Charger,dist_charger=40): 391 | # Check if the position tolerance in front of the charger is respected 392 | global robot,PI 393 | 394 | distance_tol = 5 # mm, tolerance for placement error 395 | angle_tol = 5*PI/180 # rad, tolerance for orientation error 396 | 397 | try: 398 | charger = robot.world.wait_for_observed_charger(timeout=2,include_existing=True) 399 | except: 400 | print('WARNING: Cannot see the charger to verify the position.') 401 | 402 | # Calculate positions 403 | r_coord = [0,0,0] 404 | c_coord = [0,0,0] 405 | # Coordonates of robot and charger 406 | r_coord[0] = robot.pose.position.x #.x .y .z, .rotation otherwise 407 | r_coord[1] = robot.pose.position.y 408 | r_coord[2] = robot.pose.position.z 409 | r_zRot = robot.pose_angle.radians # .degrees or .radians 410 | c_coord[0] = charger.pose.position.x 411 | c_coord[1] = charger.pose.position.y 412 | c_coord[2] = charger.pose.position.z 413 | c_zRot = charger.pose.rotation.angle_z.radians 414 | 415 | # Create target position 416 | # dist_charger in mm, distance if front of charger 417 | c_coord[0] -= dist_charger*math.cos(c_zRot) 418 | c_coord[1] -= dist_charger*math.sin(c_zRot) 419 | 420 | # Direction and distance to target position (in front of charger) 421 | distance = math.sqrt((c_coord[0]-r_coord[0])**2 + (c_coord[1]-r_coord[1])**2 + (c_coord[2]-r_coord[2])**2) 422 | 423 | if(distance < distance_tol and math.fabs(r_zRot-c_zRot) < angle_tol): 424 | return 1 425 | else: 426 | return 0 427 | 428 | def final_adjust(charger: cozmo.objects.Charger,dist_charger=40,speed=40,critical=False): 429 | # Final adjustement to properly face the charger. 430 | # The position can be adjusted several times if 431 | # the precision is critical, i.e. when climbing 432 | # back onto the charger. 433 | global robot,PI 434 | 435 | while(True): 436 | # Calculate positions 437 | r_coord = [0,0,0] 438 | c_coord = [0,0,0] 439 | # Coordonates of robot and charger 440 | r_coord[0] = robot.pose.position.x #.x .y .z, .rotation otherwise 441 | r_coord[1] = robot.pose.position.y 442 | r_coord[2] = robot.pose.position.z 443 | r_zRot = robot.pose_angle.radians # .degrees or .radians 444 | c_coord[0] = charger.pose.position.x 445 | c_coord[1] = charger.pose.position.y 446 | c_coord[2] = charger.pose.position.z 447 | c_zRot = charger.pose.rotation.angle_z.radians 448 | 449 | # Create target position 450 | # dist_charger in mm, distance if front of charger 451 | c_coord[0] -= dist_charger*math.cos(c_zRot) 452 | c_coord[1] -= dist_charger*math.sin(c_zRot) 453 | 454 | # Direction and distance to target position (in front of charger) 455 | distance = math.sqrt((c_coord[0]-r_coord[0])**2 + (c_coord[1]-r_coord[1])**2 + (c_coord[2]-r_coord[2])**2) 456 | vect = [c_coord[0]-r_coord[0],c_coord[1]-r_coord[1],c_coord[2]-r_coord[2]] 457 | # Angle of vector going from robot's origin to target's position 458 | theta_t = math.atan2(vect[1],vect[0]) 459 | 460 | print('CHECK: Adjusting position') 461 | # Face the target position 462 | angle = clip_angle((theta_t-r_zRot)) 463 | robot.turn_in_place(radians(angle)).wait_for_completed() 464 | # Drive toward the target position 465 | robot.drive_straight(distance_mm(distance),speed_mmps(speed)).wait_for_completed() 466 | # Face the charger 467 | angle = clip_angle((c_zRot-theta_t)) 468 | robot.turn_in_place(radians(angle)).wait_for_completed() 469 | 470 | # In case the robot does not need to climb onto the charger 471 | if not critical: 472 | break 473 | elif(check_tol(charger,dist_charger)): 474 | print('CHECK: Robot aligned relativ to the charger.') 475 | break 476 | return 477 | 478 | def restart_procedure(charger: cozmo.objects.Charger): 479 | global robot 480 | 481 | robot.stop_all_motors() 482 | robot.set_lift_height(height=0.5,max_speed=10,in_parallel=True).wait_for_completed() 483 | robot.pose.invalidate() 484 | charger.pose.invalidate() 485 | print('ABORT: Driving away') 486 | #robot.drive_straight(distance_mm(150),speed_mmps(80),in_parallel=False).wait_for_completed() 487 | robot.drive_wheels(80,80,duration=2) 488 | turn_around() 489 | robot.set_lift_height(height=0,max_speed=10,in_parallel=True).wait_for_completed() 490 | # Restart procedure 491 | get_on_charger() 492 | return 493 | 494 | def get_on_charger(): 495 | global robot,pitch_threshold 496 | 497 | robot.set_head_angle(degrees(0),in_parallel=False).wait_for_completed() 498 | pitch_threshold = math.fabs(robot.pose_pitch.degrees) 499 | pitch_threshold += 1 # Add 1 degree to threshold 500 | print('Pitch threshold: ' + str(pitch_threshold)) 501 | 502 | # Drive towards charger 503 | go_to_charger() 504 | 505 | # Let Cozmo first look for the charger once again. The coordinates 506 | # tend to be too unprecise if an old coordinate system is kept. 507 | if robot.world.charger is not None and robot.world.charger.pose.is_comparable(robot.pose): 508 | robot.world.charger.pose.invalidate() 509 | charger = find_charger() 510 | 511 | # Adjust position in front of the charger 512 | final_adjust(charger,critical=True) 513 | 514 | # Turn around and start going backward 515 | turn_around() 516 | robot.drive_wheel_motors(-120,-120) 517 | robot.set_lift_height(height=0.5,max_speed=10,in_parallel=True).wait_for_completed() 518 | robot.set_head_angle(degrees(0),in_parallel=True).wait_for_completed() 519 | 520 | # This section allow to wait for Cozmo to arrive on its charger 521 | # and detect eventual errors. The whole procedure will be restarted 522 | # in case something goes wrong. 523 | timeout = 1 # seconds before timeout 524 | t = 0 525 | # Wait for back wheels to climb on charger 526 | while(True): 527 | time.sleep(.1) 528 | t += 0.1 529 | if(t >= timeout): 530 | print('ERROR: robot timed out before climbing on charger.') 531 | restart_procedure(charger) 532 | return 533 | elif(math.fabs(robot.pose_pitch.degrees) >= pitch_threshold): 534 | print('CHECK: backwheels on charger.') 535 | break 536 | # Wait for front wheels to climb on charger 537 | timeout = 2 538 | t = 0 539 | while(True): 540 | time.sleep(.1) 541 | t += 0.1 542 | if(math.fabs(robot.pose_pitch.degrees) > 20 or t >= timeout): 543 | # The robot is climbing on charger's wall -> restart 544 | print('ERROR: robot climbed on charger\'s wall or timed out.') 545 | restart_procedure(charger) 546 | return 547 | elif(math.fabs(robot.pose_pitch.degrees) < pitch_threshold): 548 | print('CHECK: robot on charger, backing up on pins.') 549 | robot.stop_all_motors() 550 | break 551 | 552 | # Final backup onto charger's contacts 553 | robot.set_lift_height(height=0,max_speed=10,in_parallel=True).wait_for_completed() 554 | robot.backup_onto_charger(max_drive_time=3) 555 | if(robot.is_on_charger): 556 | print('PROCEDURE SUCCEEDED') 557 | else: 558 | restart_procedure(charger) 559 | return 560 | 561 | # Celebrate success 562 | robot.drive_off_charger_contacts().wait_for_completed() 563 | celebrate(robot) # A small celebration where only the head moves 564 | robot.backup_onto_charger(max_drive_time=3) 565 | return 566 | 567 | ########################## Battery related code ########################## 568 | 569 | def check_battery(): 570 | global robot 571 | 572 | if(robot.battery_voltage < 3.5): 573 | print('Battery is low, directly docking to charger without cleaning up.') 574 | return 1 575 | else: 576 | return 0 577 | 578 | ########################## General code ########################## 579 | 580 | def turn_around(): 581 | global robot 582 | robot.turn_in_place(degrees(-180)).wait_for_completed() 583 | return 584 | 585 | ########################## Main ########################## 586 | 587 | robot = None 588 | 589 | def init_robot(cozmo_robot: cozmo.robot.Robot): 590 | global robot 591 | robot = cozmo_robot 592 | 593 | def execute_procedure(): 594 | low_battery = check_battery() 595 | if not low_battery: 596 | clean_up_cubes() 597 | get_on_charger() 598 | return 599 | 600 | def cozmo_program(cozmo_robot: cozmo.robot.Robot): 601 | global robot 602 | init_robot(cozmo_robot) 603 | 604 | # Get off charger if on it 605 | if(robot.is_on_charger): 606 | robot.drive_off_charger_contacts().wait_for_completed() 607 | robot.drive_straight(distance_mm(110),speed_mmps(80)).wait_for_completed() 608 | 609 | execute_procedure() 610 | return 611 | 612 | if __name__ == "__main__": 613 | cozmo.robot.Robot.drive_off_charger_on_connect = False 614 | cozmo.run_program(cozmo_program,use_viewer=True,use_3d_viewer=False) 615 | --------------------------------------------------------------------------------