├── LICENSE ├── README.md ├── assets └── grid_interface.png ├── configs ├── airgen │ ├── drone_basic.json │ ├── drone_googlemaps.json │ ├── forklift_warehouse.json │ ├── multi_drone.json │ ├── wheeled_basic.json │ └── wheeled_robot_multisensor.json └── isaac │ ├── anymalc_office.json │ ├── g1_fullwarehouse.json │ ├── go2_warehouse.json │ ├── h1_hospital.json │ ├── ur5e_api_tabletop.json │ └── ur5e_tabletop.json └── notebooks ├── Multi.ipynb ├── ai_models_car.ipynb ├── ai_models_drone.ipynb ├── car_control.ipynb ├── car_data_capture.ipynb ├── car_lidar.ipynb ├── car_visual_odometry.ipynb ├── data_capture.ipynb ├── data_generation.ipynb ├── depth_point_cloud.ipynb ├── domain_randomization.ipynb ├── drone_control.ipynb ├── drone_data_capture.ipynb ├── drone_gps_nav.ipynb ├── drone_path_planning.ipynb ├── drone_thermal_camera.ipynb ├── grid-isaac ├── arm_api_control.ipynb ├── arm_reach_object_UR.ipynb ├── arm_reach_object_franka.ipynb ├── humanoid_ai_models.ipynb ├── humanoid_detect_navigate.ipynb ├── locomotion_data_capture.ipynb └── locomotion_safexploration.ipynb ├── hello_grid.ipynb ├── metadata.json ├── scalable_data_generation.ipynb ├── simulation_control.ipynb └── vehicle_control.ipynb /LICENSE: -------------------------------------------------------------------------------- 1 | SCALED FOUNDATIONS RESPONSIBLE AI SOURCE CODE LICENSE (RAIL-S) 2 | ============================================================== 3 | 4 | TERMS AND CONDITIONS. 5 | The Responsible Artificial Intelligence Source Code License (“License”) governs the use of the accompanying software. If you access or use the software, you accept the License. If you do not accept the License, do not access or use the software. 6 | 1. Definitions. 7 | As used in this License, the following capitalized terms have the following meanings: 8 | (i) "License" means the terms and conditions for use, reproduction, and distribution as defined by Sections one (1) through eight (8) of this document. 9 | (ii) "Licensor" means the copyright owner or legal entity authorized by the copyright owner that is granting the License. 10 | (iii) "You" (or "Your") means an individual or legal entity exercising permissions granted by this License. 11 | (iv) The terms “reproduce”, “reproduction”, “derivative works”, and “distribution” have the same meaning here as under U.S. Copyright Law. 12 | (v) “Contribution” means the original software, additions to the original software, modifications to the original software, or derivative works of the original software. 13 | (vi) "Contributor" means any person or Licensor who provides a Contribution. 14 | 15 | 2. Grant of Rights. 16 | Subject to this License, each Contributor grants You a non-exclusive, worldwide, royalty-free copyright license to reproduce its Contribution, prepare derivative works of its Contribution, and distribute its Contribution or any derivative works of its Contribution that You create, for non-commercial purposes including academic, research, and experimentation. 17 | 18 | 3. Restrictions. 19 | 1. If You distribute any portion of the Contribution, You must include a complete copy of this License with the distribution; and 20 | 2. You agree that the Contribution, or any derivative work of the Contribution, will not be used by You or any third party subject to Your control, to: 21 | 22 | a. Surveillance 23 | i. Detect or infer any legally protected class or aspect of any person, as defined by U.S. Federal Law; and 24 | ii. Detect or infer aspects and/or features of an identity any person, such as name, family name, address, gender, sexual orientation, race, religion, age, location (at any geographical level), skin color, society or political affiliations, employment status and/or employment history, and health and medical conditions. Age and medical conditions may be inferred solely for the purpose of improving software/hardware accessibility and such data should not be cached or stored without the explicit and time limited permission of Licensor 25 | 26 | b. Computer Generated Media 27 | i. Synthesize and/or modify audio-realistic and/or video-realistic representations (indistinguishable from photo/video recordings) of people and events, without including a caption, watermark, and/or metadata file indicating that the audio-realistic and/or video-realistic representations were generated using the Contribution. 28 | 29 | c. Health Care 30 | i. Predict the likelihood that any person will request to file an insurance claim; 31 | ii. Determine an insurance premium or deny insurance applications or claims; 32 | iii. Predict the likelihood that any person request to file an insurance claim based on determining a lifestyle of a person, medical-test reports, demographic details of a person and/or online activity of a person; 33 | iv. Determine an insurance premium or deny insurance applications or claims based on data determining a lifestyle of a person, medical-test reports, demographic details of a person, and/or online activity of a person; 34 | v. Deny an insurance claim based on any predicted likelihood of the possibility of insurance fraud; and 35 | vi. Diagnose a medical condition without human oversight. 36 | 37 | d. Criminal 38 | i. Predict the likelihood that a crime will be committed by any person; 39 | ii. Predict the likelihood, of any person, being a criminal or having committed a crime; 40 | iii. Predict the likelihood, of any person, being a criminal, based on the person’s facial attributes or another person’s facial attributes; 41 | iv. Predict the likelihood, of any person, having committed a crime, based on the person’s facial attributes or another person’s facial attributes; 42 | v. Predict the likelihood that a crime will be committed by any person, based on the person’s facial attributes or another person’s facial attributes; 43 | vi. Predict a likelihood of a crime being committed by any person, based on evidence collected, facial and emotion analysis, or other such features 44 | vii. Use personal data and/or personal characteristics or features such as: name, family name, address, gender, sexual orientation, race, religion, age, location (at any geographical level), skin color, society or political affiliations, employment status and/or history, health and medical conditions (including physical, mental), family history, social media and publicly available data, image or video analysis of an individual or a group(s) of individuals, heart-rate, perspiration, breathing, and brain imaging and other metabolic data to predict the likelihood a person will engage in criminal behavior; and 45 | viii. Predict the likelihood of a person being a criminal based on the person or other User’s facial attributes. 46 | 47 | e. Defense 48 | i. Conduct military or defense missions in active deployment with the stated purpose of bringing physical harm to humans, environment (including land and animals), and public and private infrastructure. 49 | 50 | 3. Restrictions referenced in Section 3.2 MUST be included as an enforceable provision by You in any type of legal agreement governing the use and/or distribution of the Work or any Derivative Works, and You shall give notice to subsequent users You Distribute to, that the Work or any Derivative Works are subject to Section 3.2. You shall require all of Your users who use the Work or any Derivative Works to comply with the terms of Section 3.2. 51 | 52 | 4. Termination 53 | Upon the occurrence of any of the restricted uses listed above in “3. Restrictions”, Licensor shall have the right to: 54 | (i) terminate this License Agreement and disable any Contribution either by pre-installed or then installed disabling instructions, and to take immediate possession of the Contribution and all copies wherever located, without demand or notice; 55 | (ii) require You to immediately return to Licensor all copies of the Contribution, or upon request by Licensor destroy the Contribution and all copies and certify in writing that they have been destroyed; 56 | (iii) for a period of 1 year, provide a prominent notice on the Licensor’s website indicating that this License was violated by the Licensor; 57 | (iv) release/delete any and all data collected through use of the Contribution; and 58 | (v) notify all parties affected by use of the Contribution. 59 | Termination of this License Agreement shall be in addition to and not in lieu of any other remedies available to Licensor. Licensor expressly reserves the right to pursue all legal and equitable remedies available under the law. 60 | 61 | 5. Disclaimer of Warranty. 62 | Unless required by applicable law or agreed to in writing, Licensor provides any Contribution (and each Contributor provides its Contributions) on an "As-Is" basis, without WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing a Contribution and assume any risks associated with Your exercise of permissions under this License. 63 | 64 | 6. Limitation of Liability. 65 | In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use any Contribution (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 66 | 7. Accepting Warranty or Additional Liability. 67 | While redistributing the Contribution, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. 68 | END OF TERMS AND CONDITIONS 69 | 70 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # General Robot Intelligence Development (GRID) Platform 2 | 3 | GRID is a platform built by General Robotics to function as every robot's AI team: enabling the creation and seamless infusion of safe intelligence into your robots. 4 | 5 | ![every-robots-ai-team](https://github.com/user-attachments/assets/a2906d8f-b1fe-4d13-aba5-e1fc17b7757f) 6 | 7 | GRID is composed of high-fidelity simulation, state of the art robot AI skills, and development and deployment pipelines. GRID represents several robotic form factors, and can be used for building several robotics skills across perception, planning, and control; or to capture massive amounts of synthetic data to train intelligent robots. To know more about our vision and technology, please visit our [website](https://generalrobotics.company), check out our [blogs](https://www.generalrobotics.company/blogs), or read our [technical report](https://arxiv.org/abs/2310.00887). 8 | 9 | GRID is available in two flavors: 10 | 11 | - Open GRID - A fully web-based platform to develop, train, validate and deploy intelligent robots faster, with no installation required. Visit https://grid.generalrobotics.dev to start building now! 12 | - GRID Enterprise - A packaged enterprise version for a scalable, customizable and private robot training experience. See https://www.generalrobotics.company/product/grid-enterprise for more details. 13 | 14 | This repository contains several sample configurations and Python notebooks intended for use with the GRID platform. 15 | 16 | If you find our work useful in your academic research, please cite us as: 17 | 18 | ``` 19 | @techreport{vemprala2023grid, 20 | title={GRID: A Platform for General Robot Intelligence Development}, 21 | author={Sai Vemprala and Shuhang Chen and Abhinav Shukla and Dinesh Narayanan and Ashish Kapoor}, 22 | year={2023}, 23 | eprint={2310.00887}, 24 | archivePrefix={arXiv}, 25 | primaryClass={cs.RO} 26 | } 27 | ``` 28 | 29 | -------------------------------------------------------------------------------- /assets/grid_interface.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GenRobo/GRID-playground/7a14d33274934c6e05ff37946d8b60b26c150b86/assets/grid_interface.png -------------------------------------------------------------------------------- /configs/airgen/drone_basic.json: -------------------------------------------------------------------------------- 1 | { 2 | "sim": { 3 | "sim_type": "airgen", 4 | "scene_name": "blocks", 5 | "kwargs": { 6 | "geo": false 7 | }, 8 | "settings": { 9 | "SimMode": "Multirotor", 10 | "Vehicles": { 11 | "Drone": { 12 | "VehicleType": "SimpleFlight", 13 | "VehicleModel": "Default" 14 | } 15 | }, 16 | "OriginGeopoint": { 17 | "Latitude": 47.62094998919241, 18 | "Longitude": -122.35554810901883, 19 | "Altitude": 100 20 | } 21 | } 22 | }, 23 | "grid": { 24 | "entities": { 25 | "robot": [ 26 | { 27 | "name": "airgen-drone", 28 | "kwargs": {} 29 | } 30 | ], 31 | "model": [] 32 | } 33 | } 34 | } -------------------------------------------------------------------------------- /configs/airgen/drone_googlemaps.json: -------------------------------------------------------------------------------- 1 | { 2 | "sim": { 3 | "sim_type": "airgen", 4 | "scene_name": "google_maps", 5 | "kwargs": { 6 | "geo": false 7 | }, 8 | "settings": { 9 | "SimMode": "Multirotor", 10 | "Vehicles": { 11 | "Drone": { 12 | "VehicleType": "SimpleFlight", 13 | "VehicleModel": "Astro" 14 | } 15 | }, 16 | "OriginGeopoint": { 17 | "Latitude": 47.62094998919241, 18 | "Longitude": -122.35554810901883, 19 | "Altitude": 100 20 | } 21 | } 22 | }, 23 | "grid": { 24 | "entities": { 25 | "robot": [ 26 | { 27 | "name": "airgen-drone", 28 | "kwargs": {} 29 | } 30 | ], 31 | "model": [] 32 | } 33 | } 34 | } -------------------------------------------------------------------------------- /configs/airgen/forklift_warehouse.json: -------------------------------------------------------------------------------- 1 | { 2 | "sim": { 3 | "sim_type": "airgen", 4 | "scene_name": "warehouse_big", 5 | "kwargs": { 6 | "geo": false 7 | }, 8 | "settings": { 9 | "SimMode": "Car", 10 | "Vehicles": { 11 | "Drone": { 12 | "VehicleType": "Chaos", 13 | "VehicleModel": "Forklift" 14 | } 15 | } 16 | } 17 | }, 18 | "grid": { 19 | "entities": { 20 | "robot": [ 21 | { 22 | "name": "airgen-car", 23 | "kwargs": {} 24 | } 25 | ], 26 | "model": [] 27 | } 28 | } 29 | } -------------------------------------------------------------------------------- /configs/airgen/multi_drone.json: -------------------------------------------------------------------------------- 1 | { 2 | "sim": { 3 | "sim_type": "airgen", 4 | "scene_name": "electric_central", 5 | "kwargs": { 6 | "geo": false 7 | }, 8 | "settings": { 9 | "SimMode": "Multirotor", 10 | "Vehicles": { 11 | "Drone": { 12 | "VehicleType": "SimpleFlight", 13 | "VehicleModel": "Astro", 14 | "X": 0, "Y": -5, "Z": 0 15 | }, 16 | "Drone1": { 17 | "VehicleType": "SimpleFlight", 18 | "VehicleModel": "Astro", 19 | "X": 0, "Y": -2.5, "Z": 0 20 | }, 21 | "Drone2": { 22 | "VehicleType": "SimpleFlight", 23 | "VehicleModel": "Astro", 24 | "X": 0, "Y": 0, "Z": 0 25 | }, 26 | "Drone3": { 27 | "VehicleType": "SimpleFlight", 28 | "VehicleModel": "Astro", 29 | "X": 0, "Y": 2.5, "Z": 0 30 | }, 31 | "Drone4": { 32 | "VehicleType": "SimpleFlight", 33 | "VehicleModel": "Astro", 34 | "X": 0, "Y": 5, "Z": 0 35 | } 36 | }, 37 | "OriginGeopoint": { 38 | "Latitude": 47.62094998919241, 39 | "Longitude": -122.35554810901883, 40 | "Altitude": 100 41 | }, 42 | "DefaultSensors": { 43 | "Gps": { 44 | "SensorType": 3, 45 | "Enabled": true, 46 | "EphTimeConstant": 0.9, 47 | "EpvTimeConstant": 0.9, 48 | "EphInitial": 25, 49 | "EpvInitial": 25, 50 | "EphFinal": 0.1, 51 | "EpvFinal": 0.1, 52 | "EphMin3d": 3, 53 | "EphMin2d": 4, 54 | "UpdateLatency": 0.2, 55 | "UpdateFrequency": 50, 56 | "StartupDelay": 1 57 | }, 58 | "Imu": { 59 | "SensorType": 2, 60 | "Enabled": true, 61 | "AngularRandomWalk": 0.3, 62 | "GyroBiasStabilityTau": 500, 63 | "GyroBiasStability": 4.6, 64 | "VelocityRandomWalk": 0.24, 65 | "AccelBiasStabilityTau": 800, 66 | "AccelBiasStability": 36 67 | } 68 | } 69 | } 70 | }, 71 | "grid": { 72 | "entities": { 73 | "robot": [ 74 | { 75 | "name": "airgen-drone", 76 | "kwargs": {} 77 | } 78 | ], 79 | "model": [] 80 | } 81 | } 82 | } -------------------------------------------------------------------------------- /configs/airgen/wheeled_basic.json: -------------------------------------------------------------------------------- 1 | { 2 | "sim": { 3 | "sim_type": "airgen", 4 | "scene_name": "blocks", 5 | "kwargs": { 6 | "geo": false 7 | }, 8 | "settings": { 9 | "SimMode": "Car", 10 | "Vehicles": { 11 | "Drone": { 12 | "VehicleType": "Chaos", 13 | "VehicleModel": "MCR" 14 | } 15 | }, 16 | "OriginGeopoint": { 17 | "Latitude": 47.62094998919241, 18 | "Longitude": -122.35554810901883, 19 | "Altitude": 100 20 | } 21 | } 22 | }, 23 | "grid": { 24 | "entities": { 25 | "robot": [ 26 | { 27 | "name": "airgen-drone", 28 | "kwargs": {} 29 | } 30 | ], 31 | "model": [] 32 | } 33 | } 34 | } -------------------------------------------------------------------------------- /configs/airgen/wheeled_robot_multisensor.json: -------------------------------------------------------------------------------- 1 | { 2 | "sim": { 3 | "sim_type": "airgen", 4 | "scene_name": "neighborhood", 5 | "kwargs": { 6 | "geo": false 7 | }, 8 | "settings": { 9 | "SimMode": "Car", 10 | "Vehicles": { 11 | "Drone": { 12 | "VehicleType": "Chaos", 13 | "VehicleModel": "MCR", 14 | "X": 0, 15 | "Y": -5, 16 | "Z": 0 17 | } 18 | }, 19 | "OriginGeopoint": { 20 | "Latitude": 47.62094998919241, 21 | "Longitude": -122.35554810901883, 22 | "Altitude": 100 23 | }, 24 | "DefaultSensors": { 25 | "Gps": { 26 | "SensorType": 3, 27 | "Enabled": true, 28 | "EphTimeConstant": 0.9, 29 | "EpvTimeConstant": 0.9, 30 | "EphInitial": 25, 31 | "EpvInitial": 25, 32 | "EphFinal": 0.1, 33 | "EpvFinal": 0.1, 34 | "EphMin3d": 3, 35 | "EphMin2d": 4, 36 | "UpdateLatency": 0.2, 37 | "UpdateFrequency": 50, 38 | "StartupDelay": 1 39 | }, 40 | "Imu": { 41 | "SensorType": 2, 42 | "Enabled": true, 43 | "AngularRandomWalk": 0.3, 44 | "GyroBiasStabilityTau": 500, 45 | "GyroBiasStability": 4.6, 46 | "VelocityRandomWalk": 0.24, 47 | "AccelBiasStabilityTau": 800, 48 | "AccelBiasStability": 36 49 | }, 50 | "Lidar": { 51 | "SensorType": 6, 52 | "Enabled": true, 53 | "NumberOfChannels": 16, 54 | "RotationsPerSecond": 10, 55 | "PointsPerSecond": 100000, 56 | "X": 0, 57 | "Y": 0, 58 | "Z": -1, 59 | "Roll": 0, 60 | "Pitch": 0, 61 | "Yaw": 0, 62 | "VerticalFOVUpper": 45, 63 | "VerticalFOVLower": -45, 64 | "HorizontalFOVStart": -135, 65 | "HorizontalFOVEnd": 135, 66 | "DrawDebugPoints": true, 67 | "DataFrame": "SensorLocalFrame" 68 | }, 69 | "Barometer": { 70 | "SensorType": 1, 71 | "Enabled": true, 72 | "PressureFactorSigma": 0.001825, 73 | "PressureFactorTau": 3600, 74 | "UncorrelatedNoiseSigma": 2.7, 75 | "UpdateLatency": 0, 76 | "UpdateFrequency": 50, 77 | "StartupDelay": 0 78 | }, 79 | "Magnetometer": { 80 | "SensorType": 4, 81 | "Enabled": true, 82 | "NoiseSigma": 0.005, 83 | "ScaleFactor": 1, 84 | "NoiseBias": 0, 85 | "UpdateLatency": 0, 86 | "UpdateFrequency": 50, 87 | "StartupDelay": 0 88 | } 89 | } 90 | } 91 | }, 92 | "grid": { 93 | "entities": { 94 | "robot": [ 95 | { 96 | "name": "airgen-car", 97 | "kwargs": {} 98 | } 99 | ], 100 | "model": [] 101 | } 102 | } 103 | } -------------------------------------------------------------------------------- /configs/isaac/anymalc_office.json: -------------------------------------------------------------------------------- 1 | { 2 | "sim": { 3 | "sim_type": "isaac", 4 | "scene_name": "isaac_office", 5 | "kwargs": { 6 | "geo": false 7 | }, 8 | "settings": { 9 | "robot_name": "isaac_anymalc" 10 | } 11 | }, 12 | "grid": { 13 | "entities": { 14 | "robot": [ 15 | { 16 | "name": "locomotion:isaac:sim", 17 | "kwargs": {} 18 | } 19 | ], 20 | "model": [] 21 | } 22 | } 23 | } -------------------------------------------------------------------------------- /configs/isaac/g1_fullwarehouse.json: -------------------------------------------------------------------------------- 1 | { 2 | "sim": { 3 | "sim_type": "isaac", 4 | "scene_name": "isaac_g1", 5 | "kwargs": { 6 | "geo": false 7 | }, 8 | "settings": { 9 | "robot_name": "isaac_fullwarehouse" 10 | } 11 | }, 12 | "grid": { 13 | "entities": { 14 | "robot": [ 15 | { 16 | "name": "humanoid:isaac:sim", 17 | "kwargs": {} 18 | } 19 | ], 20 | "model": [] 21 | } 22 | } 23 | } -------------------------------------------------------------------------------- /configs/isaac/go2_warehouse.json: -------------------------------------------------------------------------------- 1 | { 2 | "sim": { 3 | "sim_type": "isaac", 4 | "scene_name": "isaac_warehouse", 5 | "kwargs": { 6 | "geo": false 7 | }, 8 | "settings": { 9 | "robot_name": "isaac_go2" 10 | } 11 | }, 12 | "grid": { 13 | "entities": { 14 | "robot": [ 15 | { 16 | "name": "locomotion:isaac:sim", 17 | "kwargs": {} 18 | } 19 | ], 20 | "model": [] 21 | } 22 | } 23 | } -------------------------------------------------------------------------------- /configs/isaac/h1_hospital.json: -------------------------------------------------------------------------------- 1 | { 2 | "sim": { 3 | "sim_type": "isaac", 4 | "scene_name": "isaac_hospital", 5 | "kwargs": { 6 | "geo": false 7 | }, 8 | "settings": { 9 | "robot_name": "isaac_h1" 10 | } 11 | }, 12 | "grid": { 13 | "entities": { 14 | "robot": [ 15 | { 16 | "name": "humanoid:isaac:sim", 17 | "kwargs": {} 18 | } 19 | ], 20 | "model": [] 21 | } 22 | } 23 | } -------------------------------------------------------------------------------- /configs/isaac/ur5e_api_tabletop.json: -------------------------------------------------------------------------------- 1 | { 2 | "sim": { 3 | "sim_type": "isaac", 4 | "scene_name": "isaac_tabletop", 5 | "kwargs": { 6 | "geo": false 7 | }, 8 | "settings": { 9 | "robot_name": "isaac_ur5e_deltapose" 10 | } 11 | }, 12 | "grid": { 13 | "entities": { 14 | "robot": [ 15 | { 16 | "name": "arm:isaac:sim", 17 | "kwargs": {} 18 | } 19 | ], 20 | "model": [] 21 | } 22 | } 23 | } -------------------------------------------------------------------------------- /configs/isaac/ur5e_tabletop.json: -------------------------------------------------------------------------------- 1 | { 2 | "sim": { 3 | "sim_type": "isaac", 4 | "scene_name": "isaac_tabletop", 5 | "kwargs": { 6 | "geo": false 7 | }, 8 | "settings": { 9 | "robot_name": "isaac_ur5e_kb" 10 | } 11 | }, 12 | "grid": { 13 | "entities": { 14 | "robot": [ 15 | { 16 | "name": "arm:isaac:sim", 17 | "kwargs": {} 18 | } 19 | ], 20 | "model": [] 21 | } 22 | } 23 | } -------------------------------------------------------------------------------- /notebooks/Multi.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 34, 6 | "id": "3f14f4fc-221c-461e-9fe7-c5e9746340ce", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "import airgen\n", 11 | "from airgen.utils.collect import data_collector\n", 12 | "from typing import List, Tuple, Dict, Any, Optional, Callable" 13 | ] 14 | }, 15 | { 16 | "cell_type": "code", 17 | "execution_count": 2, 18 | "id": "83ba0165-9d2b-4ac5-9458-e5e696ab1705", 19 | "metadata": {}, 20 | "outputs": [], 21 | "source": [ 22 | "drones = [airgen.MultirotorClient()] *5" 23 | ] 24 | }, 25 | { 26 | "cell_type": "code", 27 | "execution_count": 3, 28 | "id": "8da5e8cc-d5f4-411e-8c21-04b931aa6bde", 29 | "metadata": {}, 30 | "outputs": [], 31 | "source": [ 32 | "num_drones = 5\n", 33 | "for i in range(num_drones):\n", 34 | " drone_name = f\"Drone{i}\" if i>0 else \"Drone\"\n", 35 | " drones[i].enableApiControl(True, vehicle_name=drone_name)\n", 36 | " drones[i].takeoffAsync(vehicle_name=drone_name)" 37 | ] 38 | }, 39 | { 40 | "cell_type": "code", 41 | "execution_count": 6, 42 | "id": "fa8ed3a8-e963-4603-a088-94e07f0fab0b", 43 | "metadata": {}, 44 | "outputs": [ 45 | { 46 | "data": { 47 | "text/plain": [ 48 | "" 49 | ] 50 | }, 51 | "execution_count": 6, 52 | "metadata": {}, 53 | "output_type": "execute_result" 54 | } 55 | ], 56 | "source": [ 57 | "drones[0].moveToPositionAsync(0, 10, -10, 5)" 58 | ] 59 | }, 60 | { 61 | "cell_type": "code", 62 | "execution_count": 8, 63 | "id": "1f85523f-9246-474e-a4b6-4b5c20e1fd00", 64 | "metadata": {}, 65 | "outputs": [], 66 | "source": [ 67 | "from grid.model.perception.detection.gdino import GroundingDINO" 68 | ] 69 | }, 70 | { 71 | "cell_type": "code", 72 | "execution_count": 17, 73 | "id": "695ac9be-2aa0-4860-bec3-a5c695d4d84f", 74 | "metadata": {}, 75 | "outputs": [ 76 | { 77 | "name": "stdout", 78 | "output_type": "stream", 79 | "text": [ 80 | "[[ 49.85315 63.12695 101.855576 142.61145 ]\n", 81 | " [106.66642 80.96768 144.94434 137.97488 ]\n", 82 | " [185.32486 106.87101 195.88211 139.22798 ]\n", 83 | " [208.94939 106.346375 219.08446 141.56047 ]] ['windmill', 'windmill', 'windmill', 'windmill']\n" 84 | ] 85 | } 86 | ], 87 | "source": [ 88 | "gdinomodel = GroundingDINO()\n", 89 | "img = drones[0].getImages(\"front_center\",[airgen.ImageType.Scene])[0][0]\n", 90 | "boxes, phrases = gdinomodel.detect_object(img, \"windmill .\")\n", 91 | "print(boxes, phrases)" 92 | ] 93 | }, 94 | { 95 | "cell_type": "code", 96 | "execution_count": null, 97 | "id": "889c76f1-fae5-49c9-9fe3-a9bf68e982ac", 98 | "metadata": {}, 99 | "outputs": [], 100 | "source": [ 101 | "from grid.model.perception.vlm.llava import LLaVA" 102 | ] 103 | }, 104 | { 105 | "cell_type": "code", 106 | "execution_count": 19, 107 | "id": "f8d535d2-e5fc-4681-926f-fa4176459c04", 108 | "metadata": {}, 109 | "outputs": [ 110 | { 111 | "name": "stdout", 112 | "output_type": "stream", 113 | "text": [ 114 | " The image features a large, open field with a wind farm in the background. There are several wind turbines scattered throughout the field, with some closer to the foreground and others further away. The wind turbines are of varying sizes, and they are all connected to power lines, indicating that they are generating electricity.\n", 115 | "\n", 116 | "In the foreground, there is a car parked, possibly belonging to someone visiting the wind farm. The overall scene gives a sense of the vastness of the field and the power of the wind turbines.\n" 117 | ] 118 | } 119 | ], 120 | "source": [ 121 | "llava = LLaVA()\n", 122 | "outputs = llava.run(img, \"What do you see?\")\n", 123 | "print(outputs)" 124 | ] 125 | }, 126 | { 127 | "cell_type": "code", 128 | "execution_count": 20, 129 | "id": "ac8c6b1c-3f2a-4b0b-a19b-a3a0e787b88e", 130 | "metadata": {}, 131 | "outputs": [], 132 | "source": [ 133 | "from grid.model.perception.depth.midas import MIDAS" 134 | ] 135 | }, 136 | { 137 | "cell_type": "code", 138 | "execution_count": 24, 139 | "id": "4a052152-50f5-41eb-ad67-9022e1ec402a", 140 | "metadata": {}, 141 | "outputs": [ 142 | { 143 | "name": "stdout", 144 | "output_type": "stream", 145 | "text": [ 146 | "[[ 0. 0. 0. ... 49.099712 51.197495\n", 147 | " 44.142357]\n", 148 | " [ 0. 0. 0. ... 47.814533 50.251526\n", 149 | " 50.553577]\n", 150 | " [ 0. 0. 0. ... 48.603462 47.677307\n", 151 | " 49.153477]\n", 152 | " ...\n", 153 | " [2542.075 2544.3564 2543.6848 ... 2498.5103 2491.9482\n", 154 | " 2489.9546 ]\n", 155 | " [2559.432 2562.4492 2566.4429 ... 2520.8389 2509.9563\n", 156 | " 2505.6707 ]\n", 157 | " [2566.123 2579.5022 2589.9307 ... 2555.2407 2523.0515\n", 158 | " 2509.2966 ]]\n" 159 | ] 160 | } 161 | ], 162 | "source": [ 163 | "midas = MIDAS()\n", 164 | "depth = midas.run(img)\n", 165 | "print(depth)" 166 | ] 167 | }, 168 | { 169 | "cell_type": "code", 170 | "execution_count": 30, 171 | "id": "235b1f84-457d-404a-b23f-69b09d199bff", 172 | "metadata": {}, 173 | "outputs": [], 174 | "source": [ 175 | "num_episodes = 1\n", 176 | "import numpy as np\n", 177 | "\n", 178 | "x_bounds = (20, 50) \n", 179 | "y_bounds = (30, 50) \n", 180 | "num_values = 5 \n", 181 | "\n", 182 | "\n", 183 | "x_values = np.random.uniform(x_bounds[0], x_bounds[1], num_values)\n", 184 | "y_values = np.random.uniform(y_bounds[0], y_bounds[1], num_values)\n", 185 | "\n", 186 | "for i in range(num_episodes):\n", 187 | " for i in range(num_drones):\n", 188 | " drone_name = f\"Drone{i}\" if i>0 else \"Drone\"\n", 189 | " drones[i].moveToPositionAsync(x_values[i], y_values[i], -10, 5, vehicle_name=drone_name)" 190 | ] 191 | }, 192 | { 193 | "cell_type": "code", 194 | "execution_count": 63, 195 | "id": "243afc84-a947-4195-993c-a24905e78184", 196 | "metadata": {}, 197 | "outputs": [], 198 | "source": [ 199 | "def readSensors(client: airgen.MultirotorClient(), drone_name: 'str') -> dict:\n", 200 | " sensor_data = {}\n", 201 | " img = client.getImages(\"front_center\",[airgen.ImageType.Scene], vehicle_name=drone_name)[0]\n", 202 | " sensor_data['rgb'] = img\n", 203 | " # sensor_data['depth'] = midas.run(img[0])\n", 204 | " # sensor_data['annot'] = gdinomodel.detect_object(img[0], \"windmill .\")\n", 205 | " return sensor_data" 206 | ] 207 | }, 208 | { 209 | "cell_type": "code", 210 | "execution_count": 62, 211 | "id": "85699119-cc17-4dd5-a0bf-14501c16612e", 212 | "metadata": {}, 213 | "outputs": [ 214 | { 215 | "name": "stderr", 216 | "output_type": "stream", 217 | "text": [ 218 | "[AirGen][2024-11-08 01:19:43,451][WARNING]-[ collect.py: 69] - error in data collection task: readSensors() missing 1 required positional argument: 'drone_name', return data collection task:readSensors, is not complete\n", 219 | "WARNING:AirGen:error in data collection task: readSensors() missing 1 required positional argument: 'drone_name', return data collection task:readSensors, is not complete\n" 220 | ] 221 | }, 222 | { 223 | "name": "stdout", 224 | "output_type": "stream", 225 | "text": [ 226 | "collected 0 measurements during moving task\n" 227 | ] 228 | } 229 | ], 230 | "source": [ 231 | "@data_collector(readSensors, time_delta=0.1)\n", 232 | "def move_task(\n", 233 | " client: airgen.MultirotorClient, drone_name: 'str', position: Tuple[float], **kwargs\n", 234 | ") -> None | Tuple[None, List[dict]]:\n", 235 | " client.moveToPositionAsync(position[0],position[1], position[2], position[3], vehicle_name=drone_name)\n", 236 | "\n", 237 | "import numpy as np\n", 238 | "\n", 239 | "x_bounds = (100, 50) \n", 240 | "y_bounds = (100, 50) \n", 241 | "num_values = 5 \n", 242 | "\n", 243 | "\n", 244 | "x_values = np.random.uniform(x_bounds[0], x_bounds[1], num_values)\n", 245 | "y_values = np.random.uniform(y_bounds[0], y_bounds[1], num_values)\n", 246 | "\n", 247 | "for i in range(num_drones):\n", 248 | " drone_name = f\"Drone{i}\" if i>0 else \"Drone\"\n", 249 | " _, sensor_data = move_task(drones[i], drone_name, (x_values[i], y_values[i], -10, 5), _collect_data=True)\n", 250 | " \n", 251 | "# for i, data in enumerate(sensor_data):\n", 252 | "# # lidar = data[\"lidar\"]\n", 253 | "# # rgb, _ = data[\"rgb\"]\n", 254 | "# # rr.log(\"grid/imagery\",rr.Image(rgb))\n", 255 | "# # rr.log(\"pointcloud\", rr.Points3D(np.array(lidar.point_cloud).reshape(-1, 3)))\n", 256 | "\n", 257 | "print(f\"collected {len(sensor_data)} measurements during moving task\")" 258 | ] 259 | }, 260 | { 261 | "cell_type": "code", 262 | "execution_count": null, 263 | "id": "74e50de8-ca4d-4c19-a5d6-7c74ba083dd7", 264 | "metadata": {}, 265 | "outputs": [], 266 | "source": [] 267 | } 268 | ], 269 | "metadata": { 270 | "kernelspec": { 271 | "display_name": "Python 3 (ipykernel)", 272 | "language": "python", 273 | "name": "python3" 274 | }, 275 | "language_info": { 276 | "codemirror_mode": { 277 | "name": "ipython", 278 | "version": 3 279 | }, 280 | "file_extension": ".py", 281 | "mimetype": "text/x-python", 282 | "name": "python", 283 | "nbconvert_exporter": "python", 284 | "pygments_lexer": "ipython3", 285 | "version": "3.12.3" 286 | } 287 | }, 288 | "nbformat": 4, 289 | "nbformat_minor": 5 290 | } 291 | -------------------------------------------------------------------------------- /notebooks/ai_models_car.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "d09473d9", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.wheeled.airgen_car import AirGenCar\n", 11 | "# initialise the robot\n", 12 | "import airgen\n", 13 | "from airgen.utils.collect import data_collector\n", 14 | "from airgen import WeatherParameter, Vector3r\n", 15 | "from typing import List, Tuple\n", 16 | "airgen_car_0 = AirGenCar()" 17 | ] 18 | }, 19 | { 20 | "cell_type": "code", 21 | "execution_count": null, 22 | "id": "ed24e3c6", 23 | "metadata": {}, 24 | "outputs": [], 25 | "source": [ 26 | "# set the weather, wind, and timeofday parameters and import AI models\n", 27 | "from grid.model.perception.detection.gdino import GroundingDINO\n", 28 | "from grid.model.perception.segmentation.gsam2 import GSAM2\n", 29 | "\n", 30 | "detection = GroundingDINO()\n", 31 | "segmentation = GSAM2()\n", 32 | "\n", 33 | "client = airgen_car_0.client\n", 34 | "\n", 35 | "client.simEnableWeather(True)\n", 36 | "client.simSetWeatherParameter(WeatherParameter.Fog, 1.0) # adds fog to the scene\n", 37 | "client.simSetTimeOfDay(True, \"2024-07-22 17:00:00\") # sets the time of day to be around sunset" 38 | ] 39 | }, 40 | { 41 | "cell_type": "code", 42 | "execution_count": null, 43 | "id": "a9282dfa", 44 | "metadata": {}, 45 | "outputs": [], 46 | "source": [ 47 | "search_radius = 50 # distance in meters\n", 48 | "path = client.simPlanPathToRandomFreePoint(search_radius, smooth_path=True, draw_path=True) # generates the trajectory of points\n", 49 | "\n", 50 | "points = []\n", 51 | "for point in path:\n", 52 | " points.append(airgen.Vector3r(point['x_val'], point['y_val'], point['z_val']))" 53 | ] 54 | }, 55 | { 56 | "cell_type": "code", 57 | "execution_count": null, 58 | "id": "1f06dd2e", 59 | "metadata": {}, 60 | "outputs": [], 61 | "source": [ 62 | "def runAIModels(client: airgen.VehicleClient) -> dict:\n", 63 | " img, _ = client.getImages(\"front_center\",[airgen.ImageType.Scene])[0]\n", 64 | " boxes, scores, labels = detection.run(img,\"car.\")\n", 65 | " mask = segmentation.run(img,\"road\")\n", 66 | " return img, boxes, scores, labels, mask" 67 | ] 68 | }, 69 | { 70 | "cell_type": "code", 71 | "execution_count": null, 72 | "id": "a4f3372e", 73 | "metadata": {}, 74 | "outputs": [], 75 | "source": [ 76 | "@data_collector(runAIModels, time_delta=0.1)\n", 77 | "def move_task(\n", 78 | " client: airgen.MultirotorClient, position: Tuple[float], **kwargs\n", 79 | ") -> None | Tuple[None, List[dict]]:\n", 80 | " client.moveOnPath(points, velocity=5.0)\n", 81 | "\n", 82 | "img, boxes, scores, labels, mask = move_task(client, (0, 0, -10), _collect_data=True)" 83 | ] 84 | } 85 | ], 86 | "metadata": { 87 | "language_info": { 88 | "name": "python" 89 | } 90 | }, 91 | "nbformat": 4, 92 | "nbformat_minor": 5 93 | } 94 | -------------------------------------------------------------------------------- /notebooks/ai_models_drone.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "d09473d9", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.aerial.airgen_drone import AirGenDrone \n", 11 | "airgen_drone_0 = AirGenDrone()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "ed24e3c6", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "airgen_drone_0.client.takeoffAsync().join()\n", 22 | "airgen_drone_0.client.moveToZAsync(-25, 2).join()" 23 | ] 24 | }, 25 | { 26 | "cell_type": "code", 27 | "execution_count": null, 28 | "id": "a9282dfa", 29 | "metadata": {}, 30 | "outputs": [], 31 | "source": [ 32 | "import airgen\n", 33 | "import numpy as np\n", 34 | "from grid.model.perception.detection.gdino import GroundingDINO\n", 35 | "\n", 36 | "groundingdino = GroundingDINO()\n", 37 | "\n", 38 | "# Define a detect function that captures an RGB image from the front camera and uses an object detector\n", 39 | "# Please seperate the object name with a dot (.)\n", 40 | "\n", 41 | "def detect(drone, object_name=\"fire.\"):\n", 42 | " rgb_image, pose = drone.getImages(\"front_center\", [airgen.ImageType.Scene])[0]\n", 43 | " boxes, scores, labels = groundingdino.run(rgb_image, object_name)\n", 44 | " return boxes, scores, labels\n", 45 | "\n", 46 | "# Let's search for the fire by rotating 360 degrees and trying to detect fire at each step\n", 47 | "yaw_angles = np.linspace(0, 360, 30)\n", 48 | "\n", 49 | "for yaw in yaw_angles:\n", 50 | " airgen_drone_0.client.rotateToYawAsync(yaw).join()\n", 51 | " boxes, scores, labels = detect(airgen_drone_0.client)\n", 52 | " if 'fire' in labels:\n", 53 | " break" 54 | ] 55 | }, 56 | { 57 | "cell_type": "code", 58 | "execution_count": null, 59 | "id": "1f06dd2e", 60 | "metadata": {}, 61 | "outputs": [], 62 | "source": [ 63 | "# Let's try to use a different model and object category\n", 64 | "\n", 65 | "from grid.model.perception.segmentation.clipseg import CLIPSeg\n", 66 | "clipseg = CLIPSeg(use_local = True)\n", 67 | "\n", 68 | "# Use CLIP features to try and segment out 'smoke'\n", 69 | "\n", 70 | "def segment(drone_client, object_name=\"smoke\"):\n", 71 | " rgb_image, pose = drone_client.getImages(\"front_center\", [airgen.ImageType.Scene])[0]\n", 72 | " result = clipseg.run(rgb_image, object_name)\n", 73 | " return result\n", 74 | "\n", 75 | "segment(airgen_drone_0.client)" 76 | ] 77 | }, 78 | { 79 | "cell_type": "code", 80 | "execution_count": null, 81 | "id": "a4f3372e", 82 | "metadata": {}, 83 | "outputs": [], 84 | "source": [ 85 | "# Let's evaluate the vision models with some weather variations to the scene\n", 86 | "import time\n", 87 | "airgen_drone_0.client.simEnableWeather(True)\n", 88 | "\n", 89 | "for i in range(10):\n", 90 | " airgen_drone_0.client.simSetWeatherParameter(airgen.WeatherParameter.Rain, i / 10)\n", 91 | " airgen_drone_0.client.simSetWeatherParameter(airgen.WeatherParameter.Fog, i / 10)\n", 92 | " detect(airgen_drone_0.client)\n", 93 | " segment(airgen_drone_0.client)\n", 94 | " time.sleep(1)" 95 | ] 96 | } 97 | ], 98 | "metadata": { 99 | "language_info": { 100 | "name": "python" 101 | } 102 | }, 103 | "nbformat": 4, 104 | "nbformat_minor": 5 105 | } 106 | -------------------------------------------------------------------------------- /notebooks/car_control.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "f8fd45bc", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.wheeled.airgen_car import AirGenCar\n", 11 | "airgen_car_0 = AirGenCar()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "60860e65", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "import airgen\n", 22 | "import time\n", 23 | "# get car state \n", 24 | "car_state = airgen_car_0.client.getCarState()\n", 25 | "\n", 26 | "airgen_car_0.client.enableCarSpeedControl(False)\n", 27 | "\n", 28 | "# control the car\n", 29 | "car_controls = airgen.CarControls()\n", 30 | "car_controls.throttle=0.5\n", 31 | "car_controls.steering=0.0\n", 32 | "airgen_car_0.client.setCarControls(car_controls)\n", 33 | "print(\"Going forward...\")\n", 34 | "time.sleep(3) # Let car drive a bit\n", 35 | "print(\"Stopping...\")\n", 36 | "airgen_car_0.client.setCarControls(airgen.CarControls())\n" 37 | ] 38 | }, 39 | { 40 | "cell_type": "code", 41 | "execution_count": null, 42 | "id": "f7f4dd0b", 43 | "metadata": {}, 44 | "outputs": [], 45 | "source": [ 46 | "## more controls\n", 47 | "# Go forward + steer right\n", 48 | "car_controls.throttle = 0.5\n", 49 | "car_controls.steering = 1\n", 50 | "airgen_car_0.client.setCarControls(car_controls)\n", 51 | "print(\"Going forward, steering right\")\n", 52 | "time.sleep(3) # Let car drive a bit\n", 53 | "print(\"Stopping...\")\n", 54 | "airgen_car_0.client.setCarControls(airgen.CarControls())" 55 | ] 56 | }, 57 | { 58 | "cell_type": "code", 59 | "execution_count": null, 60 | "id": "dc32c5eb", 61 | "metadata": {}, 62 | "outputs": [], 63 | "source": [ 64 | "# go reverse\n", 65 | "car_controls.throttle = 0.5\n", 66 | "car_controls.is_manual_gear = True\n", 67 | "car_controls.manual_gear = -1\n", 68 | "car_controls.steering = 0\n", 69 | "airgen_car_0.client.setCarControls(car_controls)\n", 70 | "print(\"Going reverse...\")\n", 71 | "time.sleep(3) # Let car drive a bit\n" 72 | ] 73 | }, 74 | { 75 | "cell_type": "code", 76 | "execution_count": null, 77 | "id": "2afe8fe5", 78 | "metadata": {}, 79 | "outputs": [], 80 | "source": [ 81 | "# Apply brakes\n", 82 | "car_controls.is_manual_gear = False # Change back gear to auto\n", 83 | "car_controls.manual_gear = 0\n", 84 | "car_controls.throttle = 0\n", 85 | "car_controls.brake = 1\n", 86 | "airgen_car_0.client.setCarControls(car_controls)\n", 87 | "print(\"Applying brakes...\")\n", 88 | "time.sleep(3) # Let car drive a bit\n", 89 | "car_controls.brake = 0 # Remove brake" 90 | ] 91 | } 92 | ], 93 | "metadata": { 94 | "kernelspec": { 95 | "display_name": "dev", 96 | "language": "python", 97 | "name": "python3" 98 | }, 99 | "language_info": { 100 | "codemirror_mode": { 101 | "name": "ipython", 102 | "version": 3 103 | }, 104 | "file_extension": ".py", 105 | "mimetype": "text/x-python", 106 | "name": "python", 107 | "nbconvert_exporter": "python", 108 | "pygments_lexer": "ipython3", 109 | "version": "3.11.7" 110 | } 111 | }, 112 | "nbformat": 4, 113 | "nbformat_minor": 5 114 | } 115 | -------------------------------------------------------------------------------- /notebooks/car_data_capture.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "4a40871f", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.wheeled.airgen_car import AirGenCar\n", 11 | "airgen_car_0 = AirGenCar()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "802b10c5-ed83-41f9-a67b-5ef1477fc16b", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "import h5py, os\n", 22 | "from typing import List\n", 23 | "from airgen import ImageType, VehicleClient\n", 24 | "from airgen.utils.visualize import rr_log_airgen_image\n", 25 | "from grid import GRID_USER_SESSION_BLOB_DIR\n", 26 | "\n", 27 | "\n", 28 | "def capture_data(car: VehicleClient, sensor_types: List[str], capture_types: List[str], hdf5_file_path: str, idx=0):\n", 29 | " with h5py.File(hdf5_file_path, 'a') as hdf5_file:\n", 30 | " frame_group = hdf5_file.create_group(f\"frame_{idx}\")\n", 31 | " sensor_group = frame_group.create_group(\"sensor_data\")\n", 32 | " image_group = frame_group.create_group(\"image_data\")\n", 33 | "\n", 34 | " capture_name_map = {\n", 35 | " \"rgb\": ImageType.Scene, \n", 36 | " \"depth\": ImageType.DepthPerspective, \n", 37 | " \"segmentation\": ImageType.Segmentation\n", 38 | " }\n", 39 | " \n", 40 | " # Capture IMU data\n", 41 | " if \"imu\" in sensor_types:\n", 42 | " imu_data = car.getImuData()\n", 43 | " imu_group = sensor_group.create_group(\"imu\")\n", 44 | " imu_group.create_dataset(\"time_stamp\", data=imu_data.time_stamp)\n", 45 | " imu_group.create_dataset(\"orientation\", data=[\n", 46 | " imu_data.orientation.w_val, \n", 47 | " imu_data.orientation.x_val, \n", 48 | " imu_data.orientation.y_val, \n", 49 | " imu_data.orientation.z_val\n", 50 | " ])\n", 51 | " imu_group.create_dataset(\"angular_velocity\", data=[\n", 52 | " imu_data.angular_velocity.x_val, \n", 53 | " imu_data.angular_velocity.y_val, \n", 54 | " imu_data.angular_velocity.z_val\n", 55 | " ])\n", 56 | " imu_group.create_dataset(\"linear_acceleration\", data=[\n", 57 | " imu_data.linear_acceleration.x_val, \n", 58 | " imu_data.linear_acceleration.y_val, \n", 59 | " imu_data.linear_acceleration.z_val\n", 60 | " ])\n", 61 | " \n", 62 | " # Capture image data\n", 63 | " image_types = [capture_name_map[capture_type] for capture_type in capture_types]\n", 64 | " images = car.getImages(\"front_center\", image_types)\n", 65 | " \n", 66 | " for capture_type, image in zip(capture_types, images):\n", 67 | " rr_log_airgen_image(\"grid\", capture_name_map[capture_type], image[0])\n", 68 | " image_group.create_dataset(capture_type, data=image[0])\n", 69 | "\n", 70 | " if idx % 10 == 0:\n", 71 | " print(f\"Saved {idx} frames\")" 72 | ] 73 | }, 74 | { 75 | "cell_type": "code", 76 | "execution_count": null, 77 | "id": "61f7fef6-a27a-4129-8d52-d9e8dbadadbb", 78 | "metadata": {}, 79 | "outputs": [], 80 | "source": [ 81 | "import time\n", 82 | "\n", 83 | "# Example usage\n", 84 | "hdf5_file_path = os.path.join(GRID_USER_SESSION_BLOB_DIR, 'car_data.h5')\n", 85 | "start_time = time.time()\n", 86 | "\n", 87 | "# Apply speed commands to car\n", 88 | "airgen_car_0.client.setCarTargetSpeed(1.0)\n", 89 | "\n", 90 | "for i in range(50):\n", 91 | " airgen_car_0.client.simPause(True)\n", 92 | " capture_data(airgen_car_0.client, ['imu'], ['rgb', 'depth', 'segmentation'], hdf5_file_path, idx=i)\n", 93 | " airgen_car_0.client.simPause(False)\n", 94 | " time.sleep(0.1)\n", 95 | "\n", 96 | "airgen_car_0.client.setCarTargetSpeed(0.0)\n", 97 | "\n", 98 | "elapsed_time = time.time() - start_time\n", 99 | "print(f\"Total time taken: {elapsed_time:.2f} seconds\")" 100 | ] 101 | } 102 | ], 103 | "metadata": { 104 | "kernelspec": { 105 | "display_name": "Python 3 (ipykernel)", 106 | "language": "python", 107 | "name": "python3" 108 | }, 109 | "language_info": { 110 | "codemirror_mode": { 111 | "name": "ipython", 112 | "version": 3 113 | }, 114 | "file_extension": ".py", 115 | "mimetype": "text/x-python", 116 | "name": "python", 117 | "nbconvert_exporter": "python", 118 | "pygments_lexer": "ipython3", 119 | "version": "3.11.9" 120 | } 121 | }, 122 | "nbformat": 4, 123 | "nbformat_minor": 5 124 | } 125 | -------------------------------------------------------------------------------- /notebooks/car_lidar.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "b7cc0c0b", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.wheeled.airgen_car import AirGenCar\n", 11 | "airgen_car_0 = AirGenCar()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "1ce1d815", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "import rerun as rr\n", 22 | "import numpy as np\n", 23 | "\n", 24 | "def capture_lidar_and_display():\n", 25 | " lidarData = airgen_car_0.client.getLidarData()\n", 26 | "\n", 27 | " points = np.array(lidarData.point_cloud, dtype=np.dtype('f4'))\n", 28 | " points = np.reshape(points, (int(points.shape[0]/3), 3))\n", 29 | "\n", 30 | " rr.log(\"grid/point_cloud\", rr.Points3D(points))" 31 | ] 32 | }, 33 | { 34 | "cell_type": "code", 35 | "execution_count": null, 36 | "id": "dd079c32", 37 | "metadata": {}, 38 | "outputs": [], 39 | "source": [ 40 | "import airgen, time\n", 41 | "\n", 42 | "car_controls = airgen.CarControls()\n", 43 | "\n", 44 | "car_controls.throttle = 0.5\n", 45 | "car_controls.steering = 0\n", 46 | "airgen_car_0.client.setCarControls(car_controls)\n", 47 | "\n", 48 | "for i in range(30):\n", 49 | " capture_lidar_and_display()\n", 50 | " time.sleep(0.1) \n", 51 | "\n", 52 | "airgen_car_0.client.setCarControls(airgen.CarControls())" 53 | ] 54 | } 55 | ], 56 | "metadata": { 57 | "language_info": { 58 | "name": "python" 59 | } 60 | }, 61 | "nbformat": 4, 62 | "nbformat_minor": 5 63 | } 64 | -------------------------------------------------------------------------------- /notebooks/car_visual_odometry.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "a6d32a86", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.wheeled.airgen_car import AirGenCar\n", 11 | "airgen_car_0 = AirGenCar()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "96368e8f", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "from grid.model.perception.vo.vo_dpvo import DPVO\n", 22 | "\n", 23 | "import numpy as np\n", 24 | "client = airgen_car_0.client\n", 25 | "\n", 26 | "# Get camera parameters\n", 27 | "camera_info = client.simGetCameraInfo(\"0\")\n", 28 | "print(f\"camera into : {camera_info}\")\n", 29 | "\n", 30 | "# Initialize the VO model with calibration parameters\n", 31 | "# match this with the camera settings while creating the session\n", 32 | "calib = np.array([128, 128, 128, 128])\n", 33 | "vo_model = DPVO(calib)" 34 | ] 35 | }, 36 | { 37 | "cell_type": "code", 38 | "execution_count": null, 39 | "id": "ca9edcde", 40 | "metadata": {}, 41 | "outputs": [], 42 | "source": [ 43 | "import rerun as rr\n", 44 | "import airgen\n", 45 | "import time\n", 46 | "\n", 47 | "client.enableApiControl(False) # User controls the vehicle\n", 48 | "\n", 49 | "# Get the initial pose of the vehicle\n", 50 | "initial_pose = client.simGetVehiclePose()\n", 51 | "initial_position = initial_pose.position\n", 52 | "initial_orientation = initial_pose.orientation\n", 53 | "# Initialize current global pose\n", 54 | "current_position = initial_position\n", 55 | "current_orientation = initial_orientation\n", 56 | "\n", 57 | "def apply_relative_pose(current_position, current_orientation, rel_pose):\n", 58 | " # Extract relative translation and rotation from the pose\n", 59 | " rel_east = rel_pose[0] \n", 60 | " rel_down = -rel_pose[1] \n", 61 | " rel_north = rel_pose[2] \n", 62 | " rel_qx = rel_pose[3]\n", 63 | " rel_qy = rel_pose[4]\n", 64 | " rel_qz = rel_pose[5]\n", 65 | " rel_qw = rel_pose[6]\n", 66 | "\n", 67 | " new_position = airgen.Vector3r(x_val = current_position.x_val + rel_north,\n", 68 | " y_val = current_position.y_val + rel_east,\n", 69 | " z_val = current_position.z_val + rel_down)\n", 70 | " # Apply relative translation\n", 71 | " \n", 72 | " # get relative orientation (assuming radians)\n", 73 | " relative_orientation = airgen.Quaternionr(rel_qw, rel_qx , rel_qy, rel_qz)\n", 74 | " \n", 75 | " # Apply relative rotation by quaternion multiplication\n", 76 | " new_orientation = current_orientation * relative_orientation\n", 77 | " \n", 78 | " return new_position, new_orientation\n", 79 | "\n", 80 | "#airgen uses NED coordinate system\n", 81 | "rr.log('vo', rr.ViewCoordinates.RIGHT_HAND_Z_DOWN, static=True)\n", 82 | "try:\n", 83 | " t = 0\n", 84 | " while True:\n", 85 | " # Capture RGB image from Airgen\n", 86 | " t += 1\n", 87 | " rgb_image = client.getImages(\"front_center\", [airgen.ImageType.Scene])[0][0]\n", 88 | " \n", 89 | " # Calculate pose using VO model\n", 90 | " relative_pose = vo_model.run(rgb_image)\n", 91 | " # this pose returns x,y,z,roll,pitch,yaw\n", 92 | " # Apply the relative pose to the current global pose\n", 93 | " current_position, current_orientation = apply_relative_pose(initial_position, initial_orientation, relative_pose)\n", 94 | "\n", 95 | " rr.log(f'vo/position/pos_{t}', rr.Points3D(positions=[current_position.x_val, current_position.y_val, -current_position.z_val]))\n", 96 | " \n", 97 | " euler_angles = current_orientation.to_euler_angles()\n", 98 | " pitch = euler_angles.x_val\n", 99 | " roll = euler_angles.y_val\n", 100 | " yaw = euler_angles.z_val\n", 101 | " time.sleep(0.1)\n", 102 | " \n", 103 | "\n", 104 | "except KeyboardInterrupt:\n", 105 | " pass" 106 | ] 107 | } 108 | ], 109 | "metadata": { 110 | "language_info": { 111 | "name": "python" 112 | } 113 | }, 114 | "nbformat": 4, 115 | "nbformat_minor": 5 116 | } 117 | -------------------------------------------------------------------------------- /notebooks/data_capture.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "3026d8b6", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.aerial.airgen_drone import AirGenDrone \n", 11 | "airgen_drone__0 = AirGenDrone()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "3b4cb271", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "# Take off\n", 22 | "airgen_drone_0.client.takeoffAsync().join()" 23 | ] 24 | }, 25 | { 26 | "cell_type": "code", 27 | "execution_count": null, 28 | "id": "40599a90", 29 | "metadata": {}, 30 | "outputs": [], 31 | "source": [ 32 | "# define sensor and image collection function\n", 33 | "from typing import List, Literal\n", 34 | "from airgen import ImageType, VehicleClient\n", 35 | "from airgen.utils.visualize import rr_log_airgen_image\n", 36 | "\n", 37 | "def data_capture(drone: VehicleClient, sensor_types: List[str], capture_types: Literal['rgb', 'depth', 'segmentation']):\n", 38 | " frame_data = {\"sensor_data\": {}, \"image_data\": {}}\n", 39 | " capture_name_map = {\"rgb\": ImageType.Scene, \"depth\": ImageType.DepthPerspective, \"segmentation\": ImageType.Segmentation}\n", 40 | "\n", 41 | " # get sensor data\n", 42 | " for sensor_name in sensor_types:\n", 43 | " if sensor_name == \"imu\":\n", 44 | " frame_data[\"sensor_data\"][sensor_name] = drone.getImuData()\n", 45 | " elif sensor_name == \"gps\":\n", 46 | " frame_data[\"sensor_data\"][sensor_name] = drone.getGpsData()\n", 47 | " else:\n", 48 | " raise ValueError(f\"Unknown sensor name: {sensor_name}\")\n", 49 | " image_types = [capture_name_map[capture_type] for capture_type in capture_types]\n", 50 | " images = drone.getImages(\"front_center\", image_types)\n", 51 | "\n", 52 | " # Received data is a tuple of the image and the corresponding pose\n", 53 | " for (capture_type, image) in zip(capture_types, images):\n", 54 | " frame_data[\"image_data\"][capture_type] = image[0]\n", 55 | " rr_log_airgen_image(\"grid\", capture_name_map[capture_type], image[0])\n", 56 | " return frame_data\n", 57 | "\n", 58 | "data = data_capture(airgen_drone_0.client, ['imu', 'gps'], ['rgb', 'depth', 'segmentation'])" 59 | ] 60 | }, 61 | { 62 | "cell_type": "code", 63 | "execution_count": null, 64 | "id": "fc89dad2", 65 | "metadata": {}, 66 | "outputs": [], 67 | "source": [ 68 | "# save collected data to blob storage\n", 69 | "import json\n", 70 | "import numpy as np\n", 71 | "def serialize_to_dict(obj):\n", 72 | " if isinstance(obj, np.ndarray):\n", 73 | " return obj.tolist()\n", 74 | " if hasattr(obj, \"__dict__\"):\n", 75 | " return obj.__dict__\n", 76 | " else:\n", 77 | " raise TypeError(f\"Object of type {type(obj)} is not serializable\")\n", 78 | "data_str = json.dumps(data, default=serialize_to_dict, ensure_ascii=False, indent=4)\n", 79 | "\n", 80 | "import os\n", 81 | "from grid import GRID_USER_SESSION_BLOB_DIR\n", 82 | "# any file in the directory of GRID_USER_SESSION_BLOB_DIR will be uploaded to the blob storage\n", 83 | "file_path = os.path.join(GRID_USER_SESSION_BLOB_DIR, 'data_collection_sim.json')\n", 84 | "with open(file_path, 'w') as f:\n", 85 | " f.write(data_str)" 86 | ] 87 | } 88 | ], 89 | "metadata": { 90 | "kernelspec": { 91 | "display_name": "grid", 92 | "language": "python", 93 | "name": "python3" 94 | }, 95 | "language_info": { 96 | "codemirror_mode": { 97 | "name": "ipython", 98 | "version": 3 99 | }, 100 | "file_extension": ".py", 101 | "mimetype": "text/x-python", 102 | "name": "python", 103 | "nbconvert_exporter": "python", 104 | "pygments_lexer": "ipython3", 105 | "version": "3.11.7" 106 | } 107 | }, 108 | "nbformat": 4, 109 | "nbformat_minor": 5 110 | } 111 | -------------------------------------------------------------------------------- /notebooks/data_generation.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "40599a90", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "# define sensor and image collection function\n", 11 | "from typing import List, Literal\n", 12 | "from airgen import ImageType, VehicleClient\n", 13 | "from airgen.utils.visualize import rr_log_airgen_image\n", 14 | "import cv2\n", 15 | "import numpy as np\n", 16 | "from grid import GRID_USER_SESSION_BLOB_DIR\n", 17 | "\n", 18 | "def data_capture(car: VehicleClient, sensor_types: List[str], capture_types: Literal['rgb', 'depth', 'segmentation', 'visdepth'], idx: int = 0):\n", 19 | " frame_data = {\"sensor_data\": {}, \"image_data\": {}}\n", 20 | " capture_name_map = {\"rgb\": ImageType.Scene, \"depth\": ImageType.DepthPerspective, \"segmentation\": ImageType.Segmentation, \"visdepth\": ImageType.DepthVis}\n", 21 | "\n", 22 | " # get sensor data\n", 23 | " for sensor_name in sensor_types:\n", 24 | " if sensor_name == \"imu\":\n", 25 | " frame_data[\"sensor_data\"][sensor_name] = car.getImuData()\n", 26 | " else:\n", 27 | " raise ValueError(f\"Unknown sensor name: {sensor_name}\")\n", 28 | " image_types = [capture_name_map[capture_type] for capture_type in capture_types]\n", 29 | " images = car.getImages(\"front_center\", image_types)\n", 30 | "\n", 31 | " # Received data is a tuple of the image and the corresponding pose\n", 32 | " for (capture_type, image) in zip(capture_types, images):\n", 33 | " #frame_data[\"image_data\"][capture_type] = image[0]\n", 34 | " if capture_type.startswith(\"depth\"):\n", 35 | " img_path = os.path.join(GRID_USER_SESSION_BLOB_DIR, capture_type + \"_\" + str(idx) + \".npy\")\n", 36 | " np.save(img_path, image[0])\n", 37 | " else:\n", 38 | " img_path = os.path.join(GRID_USER_SESSION_BLOB_DIR, capture_type + \"_\" + str(idx) + \".png\")\n", 39 | " cv2.imwrite(img_path, image[0])\n", 40 | " rr_log_airgen_image(\"grid\", capture_name_map[capture_type], image[0])\n", 41 | " return frame_data\n", 42 | "\n", 43 | "data = data_capture(airgen_car_0.client, ['imu'], ['rgb', 'depth', 'segmentation', 'visdepth'])" 44 | ] 45 | }, 46 | { 47 | "cell_type": "code", 48 | "execution_count": null, 49 | "id": "fc89dad2", 50 | "metadata": {}, 51 | "outputs": [], 52 | "source": [ 53 | "from airgen import CarControls\n", 54 | "import time\n", 55 | "\n", 56 | "car_controls = CarControls()\n", 57 | "car_controls.throttle = 0.5\n", 58 | "airgen_car_0.client.setCarControls(car_controls)\n", 59 | "\n", 60 | "for i in range(10):\n", 61 | " data = data_capture(airgen_car_0.client, ['imu'], ['rgb', 'depth', 'segmentation'], i)\n", 62 | " time.sleep(0.5)\n", 63 | "\n", 64 | "airgen_car_0.client.setCarControls(CarControls())" 65 | ] 66 | }, 67 | { 68 | "cell_type": "code", 69 | "execution_count": null, 70 | "id": "31da9526", 71 | "metadata": {}, 72 | "outputs": [], 73 | "source": [ 74 | "# save collected data to blob storage\n", 75 | "import json\n", 76 | "import numpy as np\n", 77 | "def serialize_to_dict(obj):\n", 78 | " if isinstance(obj, np.ndarray):\n", 79 | " return obj.tolist()\n", 80 | " if hasattr(obj, \"__dict__\"):\n", 81 | " return obj.__dict__\n", 82 | " else:\n", 83 | " raise TypeError(f\"Object of type {type(obj)} is not serializable\")\n", 84 | "data_str = json.dumps(data, default=serialize_to_dict, ensure_ascii=False, indent=4)\n", 85 | "\n", 86 | "# any file in the directory of GRID_USER_SESSION_BLOB_DIR will be uploaded to the blob storage\n", 87 | "file_path = os.path.join(GRID_USER_SESSION_BLOB_DIR, 'data_collection_sim.json')\n", 88 | "with open(file_path, 'w') as f:\n", 89 | " f.write(data_str)" 90 | ] 91 | }, 92 | { 93 | "cell_type": "code", 94 | "execution_count": null, 95 | "id": "ec1de0a2", 96 | "metadata": {}, 97 | "outputs": [], 98 | "source": [ 99 | "airgen_car_0.client.simRunConsoleCommand(\"t.maxFPS 15\")\n", 100 | "airgen_car_0.client.enableApiControl(False)" 101 | ] 102 | } 103 | ], 104 | "metadata": { 105 | "language_info": { 106 | "name": "python" 107 | } 108 | }, 109 | "nbformat": 4, 110 | "nbformat_minor": 5 111 | } 112 | -------------------------------------------------------------------------------- /notebooks/depth_point_cloud.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "66816d12", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.model.perception.depth.metric3d import Metric3D\n", 11 | "depth_metric3d_0 = Metric3D()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "822d406b", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "# run this cell everytime you want to compute the point cloud from rgb image\n", 22 | "# We use Metric3d model along with utilities built in airgen to compute and\n", 23 | "# visualise the point cloud in rerun. We also get the depth map here to see\n", 24 | "# how it looks\n", 25 | "\n", 26 | "import numpy as np\n", 27 | "import airgen\n", 28 | "from PIL import Image\n", 29 | "import rerun as rr\n", 30 | "from airgen.utils.vision import depth2pointcloud\n", 31 | "\n", 32 | "client = airgen.VehicleClient()\n", 33 | "\n", 34 | "images = client.getImages(\"front_center\", [airgen.ImageType.Scene, airgen.ImageType.DepthPerspective])\n", 35 | "capture = {\n", 36 | " \"front_rgb\": images[0],\n", 37 | " \"front_depth\": images[1],\n", 38 | " }\n", 39 | "\n", 40 | "for camera_name, camera_data in capture.items():\n", 41 | " if camera_name == \"front_depth\":\n", 42 | " # compute point cloud and visualize it in rerun\n", 43 | " # remove points that are too far away\n", 44 | " mask = np.where(camera_data[0] < 1000.0, 1, 0).astype(np.uint8)\n", 45 | " point_cloud = depth2pointcloud(\n", 46 | " depth=camera_data[0], camera_param=camera_data[1], mask=mask\n", 47 | " )\n", 48 | " print(point_cloud.shape)\n", 49 | " print(camera_data[0].shape)\n", 50 | " rr.log('grid/point_cloud', rr.Points3D(positions = point_cloud))\n", 51 | " rr.log('grid/depth_ground_truth', rr.DepthImage(camera_data[0]))\n", 52 | " else:\n", 53 | " print(camera_data[0].shape)\n", 54 | " rr.log('grid/rgb', rr.Image(camera_data[0]))\n", 55 | " depth_map_pred = depth_metric3d_0.run(camera_data[0])\n", 56 | " print(\"Predicted Depth map shape:\", depth_map_pred.shape)\n", 57 | " # reshape to be compliant to the depth2pointcloud api\n", 58 | " depth_map_3d_reshape = depth_map_pred.reshape((256, 256, 1))\n", 59 | " mask = np.where(depth_map_3d_reshape < 150.0, 1, 0).astype(np.uint8)\n", 60 | " point_cloud = depth2pointcloud(\n", 61 | " depth=depth_map_3d_reshape, camera_param=camera_data[1], mask = mask\n", 62 | " )\n", 63 | " print(\"Predicted Depth map after reshape:\", depth_map_3d_reshape.shape)\n", 64 | " rr.log('grid/point_cloud_predicted', rr.Points3D(positions = point_cloud))\n", 65 | " rr.log('grid/depth_predicted', rr.DepthImage(depth_map_3d_reshape))" 66 | ] 67 | } 68 | ], 69 | "metadata": { 70 | "language_info": { 71 | "name": "python" 72 | } 73 | }, 74 | "nbformat": 4, 75 | "nbformat_minor": 5 76 | } 77 | -------------------------------------------------------------------------------- /notebooks/domain_randomization.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "fd3273cf", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.wheeled.airgen_car import AirGenCar\n", 11 | "airgen_car_0 = AirGenCar()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "be65761b", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "import airgen\n", 22 | "\n", 23 | "airgen_car_0.client.simListAssets()\n", 24 | "\n", 25 | "pose = airgen_car_0.client.simGetVehiclePose()\n", 26 | "pose_obj = pose\n", 27 | "pose_obj.position.x_val -= 10\n", 28 | "\n", 29 | "airgen_car_0.client.simSpawnObject(\"Car_New\", \"Car_01\", pose_obj, airgen.Vector3r(1, 1, 1), True, False)" 30 | ] 31 | }, 32 | { 33 | "cell_type": "code", 34 | "execution_count": null, 35 | "id": "d2d98ac6", 36 | "metadata": {}, 37 | "outputs": [], 38 | "source": [ 39 | "curr_pose = airgen_car_0.client.simGetObjectPose(\"Car_New\")\n", 40 | "new_pose = curr_pose\n", 41 | "new_pose.position.x_val = curr_pose.position.x_val - 20\n", 42 | "airgen_car_0.client.simSetObjectPose(\"Car_New\", new_pose)" 43 | ] 44 | }, 45 | { 46 | "cell_type": "code", 47 | "execution_count": null, 48 | "id": "1895eba0", 49 | "metadata": {}, 50 | "outputs": [], 51 | "source": [ 52 | "airgen_car_0.client.simSetTimeOfDay(True, \"2024-07-11 22:00:00\")" 53 | ] 54 | }, 55 | { 56 | "cell_type": "code", 57 | "execution_count": null, 58 | "id": "cfb64be8", 59 | "metadata": {}, 60 | "outputs": [ 61 | { 62 | "data": { 63 | "text/plain": [ 64 | "True" 65 | ] 66 | }, 67 | "execution_count": 1, 68 | "metadata": {}, 69 | "output_type": "execute_result" 70 | } 71 | ], 72 | "source": [ 73 | "texture_path = \"/mnt/azure_blobfuse_mount/user/sessions/663b8240-f6c5-4e85-95ba-5d7ad5afdadf/sample_texture.jpg\"\n", 74 | "airgen_car_0.client.simSetObjectMaterialFromTexture('Car_56', texture_path)" 75 | ] 76 | }, 77 | { 78 | "cell_type": "code", 79 | "execution_count": null, 80 | "id": "eb6327fc", 81 | "metadata": {}, 82 | "outputs": [], 83 | "source": [ 84 | "from grid.model.perception.detection.gdino import GroundingDINO\n", 85 | "from grid.model.perception.segmentation.clipseg import CLIPSeg\n", 86 | "detection_gdino_0 = GroundingDINO()\n", 87 | "seg_clipseg_0 = CLIPSeg()\n", 88 | "\n", 89 | "# Define a detect function that captures an RGB image from the front camera and uses an object detector\n", 90 | "\n", 91 | "def detect(client, object_name=\"car\"):\n", 92 | " rgb_image, pose = client.getImages(\"front_center\", [airgen.ImageType.Scene])[0]\n", 93 | " boxes, phrases = groundingdino.detect_object(rgb_image, object_name)\n", 94 | " return boxes, phrases\n", 95 | "\n", 96 | "\n", 97 | "def segment(client, object_name=\"car\"):\n", 98 | " rgb_image, pose = client.getImages(\"front_center\", [airgen.ImageType.Scene])[0]\n", 99 | " result = clipseg.segment_image(rgb_image, object_name)\n", 100 | " return result" 101 | ] 102 | }, 103 | { 104 | "cell_type": "code", 105 | "execution_count": null, 106 | "id": "f19795bf", 107 | "metadata": {}, 108 | "outputs": [], 109 | "source": [ 110 | "# Let's evaluate the vision models with some weather variations to the scene\n", 111 | "import time\n", 112 | "airgen_car_0.client.simEnableWeather(True)\n", 113 | "\n", 114 | "for i in range(10):\n", 115 | " airgen_car_0.client.simSetWeatherParameter(airgen.WeatherParameter.Rain, i / 10)\n", 116 | " airgen_car_0.client.simSetWeatherParameter(airgen.WeatherParameter.Fog, i / 10)\n", 117 | " detect(airgen_car_0.client)\n", 118 | " segment(airgen_car_0.client)\n", 119 | " time.sleep(1)" 120 | ] 121 | } 122 | ], 123 | "metadata": { 124 | "language_info": { 125 | "name": "python" 126 | } 127 | }, 128 | "nbformat": 4, 129 | "nbformat_minor": 5 130 | } 131 | -------------------------------------------------------------------------------- /notebooks/drone_control.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "ce6436d0", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.aerial.airgen_drone import AirGenDrone \n", 11 | "airgen_drone__0 = AirGenDrone()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "60860e65", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "# Take off\n", 22 | "airgen_drone_0.client.takeoffAsync().join()\n", 23 | "\n", 24 | "# Move to an altitude of 25 meters\n", 25 | "airgen_drone_0.client.moveToZAsync(-25, 5).join()" 26 | ] 27 | }, 28 | { 29 | "cell_type": "code", 30 | "execution_count": null, 31 | "id": "6a5615cd", 32 | "metadata": {}, 33 | "outputs": [], 34 | "source": [ 35 | "# Fly in a square path using velocity control\n", 36 | "\n", 37 | "airgen_drone_0.client.moveByVelocityAsync(5, 0, 0, 5).join()\n", 38 | "airgen_drone_0.client.moveByVelocityAsync(0, 5, 0, 5).join()\n", 39 | "airgen_drone_0.client.moveByVelocityAsync(-5, 0, 0, 5).join()\n", 40 | "airgen_drone_0.client.moveByVelocityAsync(0, -5, 0, 5).join()" 41 | ] 42 | }, 43 | { 44 | "cell_type": "code", 45 | "execution_count": null, 46 | "id": "bdd52cc0", 47 | "metadata": {}, 48 | "outputs": [], 49 | "source": [ 50 | "# Fly in a square path using position control\n", 51 | "\n", 52 | "airgen_drone_0.client.moveToPositionAsync(25, 0, -25, 5).join()\n", 53 | "airgen_drone_0.client.moveToPositionAsync(25, 25, -25, 5).join()\n", 54 | "airgen_drone_0.client.moveToPositionAsync(0, 25, -25, 5).join()\n", 55 | "airgen_drone_0.client.moveToPositionAsync(0, 0, -25, 5).join()" 56 | ] 57 | }, 58 | { 59 | "cell_type": "code", 60 | "execution_count": null, 61 | "metadata": {}, 62 | "outputs": [], 63 | "source": [ 64 | "# Fly in a square path using path control\n", 65 | "import airgen\n", 66 | "waypoints = [\n", 67 | " airgen.Vector3r(25, 0, -25),\n", 68 | " airgen.Vector3r(25, 25, -25),\n", 69 | " airgen.Vector3r(0, 25, -25),\n", 70 | " airgen.Vector3r(0, 0, -25),\n", 71 | "]\n", 72 | "\n", 73 | "airgen_drone_0.client.moveOnPathAsync(\n", 74 | " waypoints, 5, 60, airgen.DrivetrainType.MaxDegreeOfFreedom, airgen.YawMode(False, 0), -1, 1,\n", 75 | ").join()" 76 | ] 77 | }, 78 | { 79 | "cell_type": "code", 80 | "execution_count": null, 81 | "id": "6a166670", 82 | "metadata": {}, 83 | "outputs": [], 84 | "source": [ 85 | "# Fly in a square path with yaw angle relative to the direction of travel\n", 86 | "airgen_drone_0.client.moveOnPathAsync(\n", 87 | " waypoints, 5, 60, airgen.DrivetrainType.ForwardOnly, airgen.YawMode(False, 0), -1, 1\n", 88 | ").join()" 89 | ] 90 | }, 91 | { 92 | "cell_type": "code", 93 | "execution_count": null, 94 | "metadata": {}, 95 | "outputs": [], 96 | "source": [ 97 | "# Land\n", 98 | "airgen_drone_0.client.moveToZAsync(-2, 5).join()\n", 99 | "airgen_drone_0.client.landAsync().join()" 100 | ] 101 | }, 102 | { 103 | "cell_type": "code", 104 | "execution_count": null, 105 | "id": "f593b301", 106 | "metadata": {}, 107 | "outputs": [], 108 | "source": [ 109 | "# plan a path in geo-typical environments\n", 110 | "import numpy as np\n", 111 | "nav_mesh_info = airgen_drone_0.client.getNavMeshInfo()\n", 112 | "# Get current pose of the drone in 6-DOF\n", 113 | "start_pose = airgen_drone_0.client.simGetVehiclePose()\n", 114 | "\n", 115 | "# Sample a random valid pose in the environment\n", 116 | "amplitude = (\n", 117 | " np.absolute(\n", 118 | " np.array(\n", 119 | " [\n", 120 | " nav_mesh_info[2][\"x_val\"] - nav_mesh_info[1][\"x_val\"],\n", 121 | " nav_mesh_info[2][\"y_val\"] - nav_mesh_info[1][\"y_val\"],\n", 122 | " nav_mesh_info[2][\"z_val\"] - nav_mesh_info[1][\"z_val\"],\n", 123 | " ]\n", 124 | " )\n", 125 | " )\n", 126 | " / 2.0\n", 127 | ")\n", 128 | "# sample a random point on the nav mesh\n", 129 | "random_point = airgen.Vector3r(\n", 130 | " np.random.uniform(\n", 131 | " nav_mesh_info[0][\"x_val\"] - amplitude[0],\n", 132 | " nav_mesh_info[0][\"x_val\"] + amplitude[0],\n", 133 | " ),\n", 134 | " np.random.uniform(\n", 135 | " nav_mesh_info[0][\"y_val\"] - amplitude[1],\n", 136 | " nav_mesh_info[0][\"y_val\"] + amplitude[1],\n", 137 | " ),\n", 138 | " np.random.uniform(\n", 139 | " nav_mesh_info[0][\"z_val\"] - amplitude[2],\n", 140 | " nav_mesh_info[0][\"z_val\"] + amplitude[2],\n", 141 | " ),\n", 142 | ")\n", 143 | "# sample a random yaw angle\n", 144 | "random_yaw = np.random.uniform(-np.pi, np.pi)\n", 145 | "goal_pose= airgen.Pose(\n", 146 | " random_point, airgen.Quaternionr(airgen.Vector3r(0, 0, random_yaw))\n", 147 | " )\n", 148 | "\n", 149 | "# Compute a collision-free path between start and goal\n", 150 | "trajectory = airgen_drone_0.client.simPlanPath(\n", 151 | " start_pose.position, goal_pose.position, True, True\n", 152 | ")\n", 153 | "\n", 154 | "points = []\n", 155 | "for waypoint in trajectory:\n", 156 | " points.append(\n", 157 | " airgen.Vector3r(waypoint[\"x_val\"], waypoint[\"y_val\"], waypoint[\"z_val\"])\n", 158 | " )\n", 159 | "\n", 160 | "# Move the drone along the planned path at a velocity of 5 m/s\n", 161 | "velocity = 5.0\n", 162 | "airgen_drone_0.client.moveOnPathAsync(\n", 163 | " points,\n", 164 | " velocity,\n", 165 | " 120,\n", 166 | " airgen.DrivetrainType.ForwardOnly,\n", 167 | " airgen.YawMode(False, 0),\n", 168 | " -1,\n", 169 | " 0,\n", 170 | ").join()" 171 | ] 172 | } 173 | ], 174 | "metadata": { 175 | "language_info": { 176 | "name": "python" 177 | } 178 | }, 179 | "nbformat": 4, 180 | "nbformat_minor": 5 181 | } 182 | -------------------------------------------------------------------------------- /notebooks/drone_data_capture.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "912f70c0", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.aerial.airgen_drone import AirGenDrone \n", 11 | "airgen_drone__0 = AirGenDrone()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "233a2aef", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "# Take off\n", 22 | "airgen_drone_0.client.takeoffAsync().join()" 23 | ] 24 | }, 25 | { 26 | "cell_type": "code", 27 | "execution_count": null, 28 | "id": "e073595c", 29 | "metadata": {}, 30 | "outputs": [], 31 | "source": [ 32 | "import h5py, os\n", 33 | "from typing import List\n", 34 | "from airgen import ImageType, VehicleClient\n", 35 | "from airgen.utils.visualize import rr_log_airgen_image\n", 36 | "from grid import GRID_USER_SESSION_BLOB_DIR\n", 37 | "\n", 38 | "\n", 39 | "def capture_data(drone: VehicleClient, sensor_types: List[str], capture_types: List[str], hdf5_file_path: str, idx=0):\n", 40 | " with h5py.File(hdf5_file_path, 'a') as hdf5_file:\n", 41 | " frame_group = hdf5_file.create_group(f\"frame_{idx}\")\n", 42 | " sensor_group = frame_group.create_group(\"sensor_data\")\n", 43 | " image_group = frame_group.create_group(\"image_data\")\n", 44 | "\n", 45 | " capture_name_map = {\n", 46 | " \"rgb\": ImageType.Scene, \n", 47 | " \"depth\": ImageType.DepthPerspective, \n", 48 | " \"segmentation\": ImageType.Segmentation\n", 49 | " }\n", 50 | " \n", 51 | " # Capture sensor data\n", 52 | " for sensor_name in sensor_types:\n", 53 | " if sensor_name == \"imu\":\n", 54 | " imu_data = drone.getImuData()\n", 55 | " imu_group = sensor_group.create_group(\"imu\")\n", 56 | " imu_group.create_dataset(\"time_stamp\", data=imu_data.time_stamp)\n", 57 | " imu_group.create_dataset(\"orientation\", data=[\n", 58 | " imu_data.orientation.w_val, \n", 59 | " imu_data.orientation.x_val, \n", 60 | " imu_data.orientation.y_val, \n", 61 | " imu_data.orientation.z_val\n", 62 | " ])\n", 63 | " imu_group.create_dataset(\"angular_velocity\", data=[\n", 64 | " imu_data.angular_velocity.x_val, \n", 65 | " imu_data.angular_velocity.y_val, \n", 66 | " imu_data.angular_velocity.z_val\n", 67 | " ])\n", 68 | " imu_group.create_dataset(\"linear_acceleration\", data=[\n", 69 | " imu_data.linear_acceleration.x_val, \n", 70 | " imu_data.linear_acceleration.y_val, \n", 71 | " imu_data.linear_acceleration.z_val\n", 72 | " ])\n", 73 | " elif sensor_name == \"gps\":\n", 74 | " gps_loc_data = drone.getGpsData().gnss.geo_point \n", 75 | " gps_group = sensor_group.create_group(\"gps\")\n", 76 | " gps_group.create_dataset(\"latitude\", data=gps_loc_data.latitude)\n", 77 | " gps_group.create_dataset(\"longitude\", data=gps_loc_data.longitude)\n", 78 | " gps_group.create_dataset(\"altitude\", data=gps_loc_data.altitude)\n", 79 | " else:\n", 80 | " raise ValueError(f\"Unknown sensor name: {sensor_name}\")\n", 81 | "\n", 82 | " # Capture image data\n", 83 | " image_types = [capture_name_map[capture_type] for capture_type in capture_types]\n", 84 | " images = drone.getImages(\"front_center\", image_types)\n", 85 | " \n", 86 | " for capture_type, image in zip(capture_types, images):\n", 87 | " rr_log_airgen_image(\"grid\", capture_name_map[capture_type], image[0])\n", 88 | " image_group.create_dataset(capture_type, data=image[0])\n", 89 | "\n", 90 | " if idx % 10 == 0:\n", 91 | " print(f\"Saved {idx} frames\")" 92 | ] 93 | }, 94 | { 95 | "cell_type": "code", 96 | "execution_count": null, 97 | "id": "8143811b", 98 | "metadata": {}, 99 | "outputs": [], 100 | "source": [ 101 | "import time\n", 102 | "\n", 103 | "# Example usage\n", 104 | "hdf5_file_path = os.path.join(GRID_USER_SESSION_BLOB_DIR, 'drone_data.h5')\n", 105 | "start_time = time.time()\n", 106 | "\n", 107 | "# Apply velocity commands for a given duration\n", 108 | "airgen_drone_0.client.moveByVelocityAsync(2, 2, -2, 5)\n", 109 | "\n", 110 | "for i in range(50):\n", 111 | " airgen_drone_0.client.simPause(True)\n", 112 | " capture_data(airgen_drone_0.client, ['imu', 'gps'], ['rgb'], hdf5_file_path, idx=i)\n", 113 | " airgen_drone_0.client.simPause(False)\n", 114 | " time.sleep(0.1)\n", 115 | "\n", 116 | "airgen_drone_0.client.hoverAsync().join()\n", 117 | "\n", 118 | "elapsed_time = time.time() - start_time\n", 119 | "print(f\"Total time taken: {elapsed_time:.2f} seconds\")" 120 | ] 121 | } 122 | ], 123 | "metadata": { 124 | "language_info": { 125 | "name": "python" 126 | } 127 | }, 128 | "nbformat": 4, 129 | "nbformat_minor": 5 130 | } 131 | -------------------------------------------------------------------------------- /notebooks/drone_gps_nav.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "4d60dfeb", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.aerial.airgen_drone import AirGenDrone \n", 11 | "airgen_drone__0 = AirGenDrone()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "0416c8cd", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "# Take off\n", 22 | "airgen_drone_0.client.takeoffAsync().join()" 23 | ] 24 | }, 25 | { 26 | "cell_type": "code", 27 | "execution_count": null, 28 | "id": "17dafaea", 29 | "metadata": {}, 30 | "outputs": [], 31 | "source": [ 32 | "# Teleport vehicle to a GPS position (in the vicinity of Seattle)\n", 33 | "import airgen\n", 34 | "geopose = airgen.GeoPose(\n", 35 | " airgen.GeoPoint(47.61967073130296, -122.35028344435351, 60), airgen.Quaternionr()\n", 36 | ")\n", 37 | "airgen_drone_0.client.simSetVehicleGeoPose(geopose, True)\n", 38 | "\n", 39 | "# Takeoff\n", 40 | "airgen_drone_0.client.takeoffAsync().join()\n", 41 | "print(airgen_drone_0.client.getGpsData())" 42 | ] 43 | }, 44 | { 45 | "cell_type": "code", 46 | "execution_count": null, 47 | "id": "669843c4", 48 | "metadata": {}, 49 | "outputs": [], 50 | "source": [ 51 | "# fly to a GPS position\n", 52 | "import airgen\n", 53 | "from functools import partial\n", 54 | "\n", 55 | "goal = airgen.GeoPoint(47.62019363630756, -122.35158563162173, 60)\n", 56 | "airgen_drone_0.client.moveToGPSAsync(\n", 57 | " goal,\n", 58 | " 4.0,\n", 59 | " timeout_sec=3e38,\n", 60 | " drivetrain=airgen.DrivetrainType.ForwardOnly,\n", 61 | " yaw_mode=airgen.YawMode(False, 0),\n", 62 | " ).join()\n", 63 | "print(airgen_drone_0.client.getGpsData())" 64 | ] 65 | } 66 | ], 67 | "metadata": { 68 | "kernelspec": { 69 | "display_name": "grid", 70 | "language": "python", 71 | "name": "python3" 72 | }, 73 | "language_info": { 74 | "codemirror_mode": { 75 | "name": "ipython", 76 | "version": 3 77 | }, 78 | "file_extension": ".py", 79 | "mimetype": "text/x-python", 80 | "name": "python", 81 | "nbconvert_exporter": "python", 82 | "pygments_lexer": "ipython3", 83 | "version": "3.11.7" 84 | } 85 | }, 86 | "nbformat": 4, 87 | "nbformat_minor": 5 88 | } 89 | -------------------------------------------------------------------------------- /notebooks/drone_path_planning.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "6fec4dac", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.aerial.airgen_drone import AirGenDrone \n", 11 | "airgen_drone__0 = AirGenDrone()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "a2ea83c7", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "# Take off\n", 22 | "airgen_drone_0.client.takeoffAsync().join()" 23 | ] 24 | }, 25 | { 26 | "cell_type": "code", 27 | "execution_count": null, 28 | "id": "65e74c46", 29 | "metadata": {}, 30 | "outputs": [], 31 | "source": [ 32 | "# Get navigation mesh extents from the simulation world\n", 33 | "nav_mesh_info = airgen_drone_0.client.getNavMeshInfo()\n", 34 | "print(\"Nav mesh info: {}\".format(nav_mesh_info))" 35 | ] 36 | }, 37 | { 38 | "cell_type": "code", 39 | "execution_count": null, 40 | "id": "9c03d7e4", 41 | "metadata": {}, 42 | "outputs": [], 43 | "source": [ 44 | "import airgen\n", 45 | "import numpy as np\n", 46 | "\n", 47 | "def sample_random_pose(nav_mesh_info):\n", 48 | " # Calculate bounds of the nav mesh from the given info\n", 49 | " amplitude = (\n", 50 | " np.absolute(\n", 51 | " np.array(\n", 52 | " [\n", 53 | " nav_mesh_info[2][\"x_val\"] - nav_mesh_info[1][\"x_val\"],\n", 54 | " nav_mesh_info[2][\"y_val\"] - nav_mesh_info[1][\"y_val\"],\n", 55 | " nav_mesh_info[2][\"z_val\"] - nav_mesh_info[1][\"z_val\"],\n", 56 | " ]\n", 57 | " )\n", 58 | " )\n", 59 | " / 2.0\n", 60 | " )\n", 61 | "\n", 62 | " # Sample a random point on the nav mesh\n", 63 | " random_point = airgen.Vector3r(\n", 64 | " np.random.uniform(\n", 65 | " nav_mesh_info[0][\"x_val\"] - amplitude[0],\n", 66 | " nav_mesh_info[0][\"x_val\"] + amplitude[0],\n", 67 | " ),\n", 68 | " np.random.uniform(\n", 69 | " nav_mesh_info[0][\"y_val\"] - amplitude[1],\n", 70 | " nav_mesh_info[0][\"y_val\"] + amplitude[1],\n", 71 | " ),\n", 72 | " np.random.uniform(\n", 73 | " nav_mesh_info[0][\"z_val\"] - amplitude[2],\n", 74 | " nav_mesh_info[0][\"z_val\"] + amplitude[2],\n", 75 | " ),\n", 76 | " )\n", 77 | "\n", 78 | " # Sample a random yaw angle\n", 79 | " random_yaw = np.random.uniform(-np.pi, np.pi)\n", 80 | "\n", 81 | " # Return the sampled pose\n", 82 | " return airgen.Pose(\n", 83 | " random_point, airgen.Quaternionr(airgen.Vector3r(0, 0, random_yaw))\n", 84 | " )\n" 85 | ] 86 | }, 87 | { 88 | "cell_type": "code", 89 | "execution_count": null, 90 | "id": "75cc9678", 91 | "metadata": {}, 92 | "outputs": [], 93 | "source": [ 94 | "# Get current pose of the drone in 6-DOF\n", 95 | "start_pose = airgen_drone_0.client.simGetVehiclePose()\n", 96 | "\n", 97 | "# Sample a random valid pose in the environment\n", 98 | "goal_pose = sample_random_pose(nav_mesh_info)\n", 99 | "\n", 100 | "# Set altitude to same as the start\n", 101 | "goal_pose.position.z_val = start_pose.position.z_val\n", 102 | "\n", 103 | "# Compute a collision-free path between start and goal\n", 104 | "path = airgen_drone_0.client.simPlanPath(\n", 105 | " start_pose.position, goal_pose.position, smooth_path=True, draw_path=True\n", 106 | ")" 107 | ] 108 | }, 109 | { 110 | "cell_type": "code", 111 | "execution_count": null, 112 | "id": "77ccbf5d", 113 | "metadata": {}, 114 | "outputs": [], 115 | "source": [ 116 | "points = []\n", 117 | "\n", 118 | "# Convert path into AirGen waypoints for control\n", 119 | "for waypoint in path:\n", 120 | " points.append(\n", 121 | " airgen.Vector3r(waypoint[\"x_val\"], waypoint[\"y_val\"], waypoint[\"z_val\"])\n", 122 | " )\n", 123 | "\n", 124 | "# Move the drone along the planned path at a velocity of 5 m/s\n", 125 | "velocity = 5.0\n", 126 | "airgen_drone_0.client.moveOnPathAsync(\n", 127 | " points,\n", 128 | " velocity,\n", 129 | " 120,\n", 130 | " airgen.DrivetrainType.ForwardOnly,\n", 131 | " airgen.YawMode(False, 0),\n", 132 | " -1,\n", 133 | " 0,\n", 134 | ").join()" 135 | ] 136 | } 137 | ], 138 | "metadata": { 139 | "language_info": { 140 | "name": "python" 141 | } 142 | }, 143 | "nbformat": 4, 144 | "nbformat_minor": 5 145 | } 146 | -------------------------------------------------------------------------------- /notebooks/drone_thermal_camera.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "1561e743", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.aerial.airgen_drone import AirGenDrone \n", 11 | "airgen_drone__0 = AirGenDrone()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "de35f3bc", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "airgen_drone_0.client.takeoffAsync().join()\n", 22 | "airgen_drone_0.client.moveToZAsync(-25, 2).join()\n", 23 | "airgen_drone_0.client.rotateToYawAsync(-90).join()" 24 | ] 25 | }, 26 | { 27 | "cell_type": "code", 28 | "execution_count": null, 29 | "id": "0baa40c4", 30 | "metadata": {}, 31 | "outputs": [], 32 | "source": [ 33 | "# Let's capture and display an infrared image\n", 34 | "import airgen\n", 35 | "import rerun as rr\n", 36 | "\n", 37 | "ir_image, camera_pose = airgen_drone_0.client.getImages(\"front_center\", [airgen.ImageType.Infrared])[0]\n", 38 | "\n", 39 | "rr.log(\"infrared\", rr.Image(ir_image))\n" 40 | ] 41 | }, 42 | { 43 | "cell_type": "code", 44 | "execution_count": null, 45 | "id": "f9db538a", 46 | "metadata": {}, 47 | "outputs": [], 48 | "source": [ 49 | "# Let's now make the fire the highest intensity in the infrared camera\n", 50 | "\n", 51 | "# Get a list of all objects in the scene with the name Fire*\n", 52 | "fire_obj_refs = airgen_drone_0.client.simListSceneObjects(\"Fire.*\")\n", 53 | "print(fire_obj_refs)\n", 54 | "\n", 55 | "fire_obj = fire_obj_refs[0]\n", 56 | "\n", 57 | "airgen_drone_0.client.simSetSegmentationObjectID(fire_obj, 255, True)\n" 58 | ] 59 | }, 60 | { 61 | "cell_type": "code", 62 | "execution_count": null, 63 | "id": "9049bc5e", 64 | "metadata": {}, 65 | "outputs": [], 66 | "source": [ 67 | "# Capture and display an infrared image again\n", 68 | "import airgen\n", 69 | "import rerun as rr\n", 70 | "\n", 71 | "ir_image, camera_pose = airgen_drone_0.client.getImages(\"front_center\", [airgen.ImageType.Infrared])[0]\n", 72 | "\n", 73 | "rr.log(\"infrared_new\", rr.Image(ir_image))\n" 74 | ] 75 | } 76 | ], 77 | "metadata": { 78 | "language_info": { 79 | "name": "python" 80 | } 81 | }, 82 | "nbformat": 4, 83 | "nbformat_minor": 5 84 | } 85 | -------------------------------------------------------------------------------- /notebooks/grid-isaac/arm_api_control.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "e1d59b82", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.arm.isaac_arm import IsaacArm\n", 11 | "armisaacsim_0 = IsaacArm()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "524cb8bf", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "armisaacsim_0.run()" 22 | ] 23 | }, 24 | { 25 | "cell_type": "code", 26 | "execution_count": null, 27 | "id": "bed7d371", 28 | "metadata": {}, 29 | "outputs": [], 30 | "source": [ 31 | "import numpy as np\n", 32 | "\n", 33 | "goal_pos_np = np.array([0.3, -0.2, 0.3])\n", 34 | "goal_quat_np = np.array([0.0, 0.0, 0.0, 1.0]) # Assuming quaternion interpolation isn't needed\n", 35 | "threshold = 0.01 # Define a threshold for stopping\n", 36 | "step_size = 0.01 # Define a step size for movement" 37 | ] 38 | }, 39 | { 40 | "cell_type": "code", 41 | "execution_count": null, 42 | "id": "3d76e644", 43 | "metadata": {}, 44 | "outputs": [], 45 | "source": [ 46 | "from grid.utils.types import Position, Orientation\n", 47 | "from grid.utils import log\n", 48 | "\n", 49 | "import time\n", 50 | "\n", 51 | "while True:\n", 52 | " curr_pos = armisaacsim_0.getPosition()\n", 53 | " curr_rot = armisaacsim_0.getOrientation()\n", 54 | " # print (f'Current Position: {curr_pos}') \n", 55 | "\n", 56 | " img = armisaacsim_0.getImage()\n", 57 | "\n", 58 | " if img is not None and img.data is not None:\n", 59 | " log(\"grid/rgb\", img)\n", 60 | " \n", 61 | " if curr_pos is None or curr_rot is None:\n", 62 | " time.sleep(0.2)\n", 63 | " continue\n", 64 | "\n", 65 | " curr_pos_np = np.array([curr_pos.x_val, curr_pos.y_val, curr_pos.z_val])\n", 66 | " \n", 67 | " # Calculate the Euclidean distance to the goal\n", 68 | " distance = np.linalg.norm(goal_pos_np[:2] - curr_pos_np[:2])\n", 69 | " # print(f'Current Distance to Goal: {distance}')\n", 70 | " \n", 71 | " if distance < threshold:\n", 72 | " print(\"Goal x, y reached!\")\n", 73 | " break\n", 74 | " \n", 75 | " # Compute delta position\n", 76 | " direction = (goal_pos_np - curr_pos_np) / distance # Normalize direction\n", 77 | " delta_pos = direction * step_size # Move step_size or remaining distance\n", 78 | " \n", 79 | " # Move towards the goal with delta position\n", 80 | " armisaacsim_0.moveToDeltaPose(Position(delta_pos[0], delta_pos[1], 0), Orientation(*goal_quat_np))\n", 81 | " time.sleep(0.2)\n", 82 | "\n", 83 | "\n", 84 | "armisaacsim_0.release()\n", 85 | "\n", 86 | "while True:\n", 87 | " curr_pos = armisaacsim_0.getPosition()\n", 88 | " curr_rot = armisaacsim_0.getOrientation()\n", 89 | " # print (f'Current Position: {curr_pos}')\n", 90 | "\n", 91 | " img = armisaacsim_0.getImage()\n", 92 | "\n", 93 | " if img is not None and img.data is not None:\n", 94 | " log(\"grid/rgb\", img)\n", 95 | " \n", 96 | " if curr_pos is None or curr_rot is None:\n", 97 | " time.sleep(0.2)\n", 98 | " continue\n", 99 | "\n", 100 | " curr_pos_np = np.array([curr_pos.x_val, curr_pos.y_val, curr_pos.z_val])\n", 101 | " \n", 102 | " # Calculate the Euclidean distance to the goal\n", 103 | " distance = np.linalg.norm(goal_pos_np[2] - curr_pos_np[2])\n", 104 | " # print(f'Current Distance to Goal: {distance}')\n", 105 | " \n", 106 | " if distance < threshold:\n", 107 | " print(\"Goal z reached!\")\n", 108 | " break\n", 109 | " \n", 110 | " # Compute delta position\n", 111 | " direction = (goal_pos_np - curr_pos_np) / distance # Normalize direction\n", 112 | " delta_pos = direction * step_size # Move step_size or remaining distance\n", 113 | " \n", 114 | " # Move towards the goal with delta position\n", 115 | " armisaacsim_0.moveToDeltaPose(Position(0, 0, delta_pos[2]), Orientation(*goal_quat_np))\n", 116 | " time.sleep(0.2)\n" 117 | ] 118 | } 119 | ], 120 | "metadata": { 121 | "language_info": { 122 | "name": "python" 123 | } 124 | }, 125 | "nbformat": 4, 126 | "nbformat_minor": 5 127 | } 128 | -------------------------------------------------------------------------------- /notebooks/grid-isaac/arm_reach_object_UR.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "4e6b3892", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.arm.isaac_arm import IsaacArm\n", 11 | "from grid.utils.types import Position, Orientation\n", 12 | "from grid.model.perception.detection.owlv2 import OWLv2\n", 13 | "from grid.utils.types import Velocity\n", 14 | "from grid.utils import log\n", 15 | "\n", 16 | "import time\n", 17 | "import numpy as np" 18 | ] 19 | }, 20 | { 21 | "cell_type": "code", 22 | "execution_count": null, 23 | "id": "cd260bf2", 24 | "metadata": {}, 25 | "outputs": [], 26 | "source": [ 27 | "agent = IsaacArm()\r\n", 28 | "agent.run()" 29 | ] 30 | }, 31 | { 32 | "cell_type": "code", 33 | "execution_count": null, 34 | "id": "f493b89c", 35 | "metadata": {}, 36 | "outputs": [ 37 | { 38 | "data": { 39 | "application/vnd.jupyter.widget-view+json": { 40 | "model_id": "65dc5a2838cd4ac8aae203a2228d5626", 41 | "version_major": 2, 42 | "version_minor": 0 43 | }, 44 | "text/plain": [ 45 | "preprocessor_config.json: 0%| | 0.00/425 [00:00 world)\r\n", 261 | "depth_data = depth.data[:, :, 0]\r\n", 262 | "Z = depth_data[int(center_y), int(center_x)]\r\n", 263 | "X_c = ((center_x - cx) / fx) * Z\r\n", 264 | "Y_c = ((center_y - cy) / fy) * Z\r\n", 265 | "Z_c = Z\r\n", 266 | "\r\n", 267 | "camera_point_h = np.array([X_c, Y_c, Z_c, 1.0])\r\n", 268 | "world_point_h = T_world_from_cam @ camera_point_h\r\n", 269 | "X_w, Y_w, Z_w = world_point_h[:3]\r\n", 270 | "\r\n", 271 | "goal_pos_np = np.array([X_w-0.25, Y_w-0.1, Z_w]) # offset for (cube side / 2.)" 272 | ] 273 | }, 274 | { 275 | "cell_type": "code", 276 | "execution_count": null, 277 | "id": "e623a616", 278 | "metadata": {}, 279 | "outputs": [], 280 | "source": [ 281 | "# Move the robot to the (x, y) position of the object\r\n", 282 | "\r\n", 283 | "# Iterate to reach the goal position\r\n", 284 | "for i in range(150):\r\n", 285 | " curr_pos = agent.getPosition()\r\n", 286 | " while curr_pos is None or curr_pos.x_val is None or curr_pos.y_val is None:\r\n", 287 | " curr_pos = agent.getPosition()\r\n", 288 | " time.sleep(0.1)\r\n", 289 | "\r\n", 290 | " curr_pos_np = np.array([curr_pos.x_val, curr_pos.y_val, curr_pos.z_val])\r\n", 291 | " \r\n", 292 | " # Compute delta position\r\n", 293 | " direction = (goal_pos_np - curr_pos_np) \r\n", 294 | " delta_pos = direction * step_size # Move step_size or remaining distance\r\n", 295 | " \r\n", 296 | " # Move towards the goal with delta position\r\n", 297 | " agent.moveToDeltaPose(Position(delta_pos[0], delta_pos[1], 0.), Orientation(*goal_quat_np))\r\n", 298 | " time.sleep(0.1)" 299 | ] 300 | }, 301 | { 302 | "cell_type": "code", 303 | "execution_count": null, 304 | "id": "7d5c2d6b", 305 | "metadata": {}, 306 | "outputs": [], 307 | "source": [ 308 | "# Move the robot to the goal z position\r\n", 309 | "\r\n", 310 | "# Iterate to reach the goal position\r\n", 311 | "for i in range(300):\r\n", 312 | " curr_pos = agent.getPosition()\r\n", 313 | " while curr_pos is None or curr_pos.x_val is None or curr_pos.y_val is None:\r\n", 314 | " curr_pos = agent.getPosition()\r\n", 315 | " time.sleep(0.1)\r\n", 316 | "\r\n", 317 | " delta_pos = (goal_z - curr_pos.z_val) * step_size_z\r\n", 318 | " # Move towards the goal with delta position\r\n", 319 | " agent.moveToDeltaPose(Position(0., 0., delta_pos), Orientation(*goal_quat_np))\r\n", 320 | " time.sleep(0.1)" 321 | ] 322 | } 323 | ], 324 | "metadata": {}, 325 | "nbformat": 4, 326 | "nbformat_minor": 5 327 | } 328 | -------------------------------------------------------------------------------- /notebooks/grid-isaac/arm_reach_object_franka.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "f9fea303", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.arm.isaac_arm import IsaacArm\n", 11 | "from grid.utils.types import Position, Orientation\n", 12 | "from grid.model.perception.detection.owlv2 import OWLv2\n", 13 | "from grid.utils.types import Velocity\n", 14 | "from grid.utils import log\n", 15 | "\n", 16 | "import time\n", 17 | "import numpy as np" 18 | ] 19 | }, 20 | { 21 | "cell_type": "code", 22 | "execution_count": null, 23 | "id": "9a2516d1", 24 | "metadata": {}, 25 | "outputs": [], 26 | "source": [ 27 | "agent = IsaacArm()\r\n", 28 | "agent.run()" 29 | ] 30 | }, 31 | { 32 | "cell_type": "code", 33 | "execution_count": null, 34 | "id": "b1bf8cbe", 35 | "metadata": {}, 36 | "outputs": [ 37 | { 38 | "data": { 39 | "application/vnd.jupyter.widget-view+json": { 40 | "model_id": "45ae8190f93c4790ae98c10a14dfceca", 41 | "version_major": 2, 42 | "version_minor": 0 43 | }, 44 | "text/plain": [ 45 | "preprocessor_config.json: 0%| | 0.00/425 [00:00 world)\r\n", 261 | "depth_data = depth.data[:, :, 0]\r\n", 262 | "Z = depth_data[int(center_y), int(center_x)]\r\n", 263 | "X_c = ((center_x - cx) / fx) * Z\r\n", 264 | "Y_c = ((center_y - cy) / fy) * Z\r\n", 265 | "Z_c = Z\r\n", 266 | "\r\n", 267 | "camera_point_h = np.array([X_c, Y_c, Z_c, 1.0])\r\n", 268 | "world_point_h = T_world_from_cam @ camera_point_h\r\n", 269 | "X_w, Y_w, Z_w = world_point_h[:3]\r\n", 270 | "\r\n", 271 | "goal_pos_np = np.array([X_w-0.25, Y_w-0.1, Z_w]) # offset for (cube side / 2.)" 272 | ] 273 | }, 274 | { 275 | "cell_type": "code", 276 | "execution_count": null, 277 | "id": "8c94d465", 278 | "metadata": {}, 279 | "outputs": [], 280 | "source": [ 281 | "# Move the robot to the (x, y) position of the object\r\n", 282 | "\r\n", 283 | "# Iterate to reach the goal position\r\n", 284 | "for i in range(250):\r\n", 285 | " curr_pos = agent.getPosition()\r\n", 286 | " while curr_pos is None or curr_pos.x_val is None or curr_pos.y_val is None:\r\n", 287 | " curr_pos = agent.getPosition()\r\n", 288 | " time.sleep(0.1)\r\n", 289 | "\r\n", 290 | " curr_pos_np = np.array([curr_pos.x_val, curr_pos.y_val, curr_pos.z_val])\r\n", 291 | " \r\n", 292 | " # Compute delta position\r\n", 293 | " direction = (goal_pos_np - curr_pos_np) \r\n", 294 | " delta_pos = direction * step_size # Move step_size or remaining distance\r\n", 295 | " \r\n", 296 | " # Move towards the goal with delta position\r\n", 297 | " agent.moveToDeltaPose(Position(delta_pos[0], delta_pos[1], 0.), Orientation(*goal_quat_np))\r\n", 298 | " time.sleep(0.1)" 299 | ] 300 | }, 301 | { 302 | "cell_type": "code", 303 | "execution_count": null, 304 | "id": "1eeb240d", 305 | "metadata": {}, 306 | "outputs": [], 307 | "source": [ 308 | "# Move the robot to the goal z position\r\n", 309 | "agent.release()\r\n", 310 | "# Iterate to reach the goal position\r\n", 311 | "for i in range(700):\r\n", 312 | " curr_pos = agent.getPosition()\r\n", 313 | " while curr_pos is None or curr_pos.x_val is None or curr_pos.y_val is None:\r\n", 314 | " curr_pos = agent.getPosition()\r\n", 315 | " time.sleep(0.1)\r\n", 316 | "\r\n", 317 | " delta = (goal_z - curr_pos.z_val)\r\n", 318 | " delta_pos = (goal_z - curr_pos.z_val) * step_size_z\r\n", 319 | "\r\n", 320 | " if abs(delta) < 0.01:\r\n", 321 | " print(\"Z Reached\")\r\n", 322 | " break\r\n", 323 | " # Move towards the goal with delta position\r\n", 324 | " agent.moveToDeltaPose(Position(0., 0., delta_pos), Orientation(*goal_quat_np))\r\n", 325 | " time.sleep(0.1)\r\n", 326 | "\r\n", 327 | "agent.grasp()" 328 | ] 329 | } 330 | ], 331 | "metadata": {}, 332 | "nbformat": 4, 333 | "nbformat_minor": 5 334 | } 335 | -------------------------------------------------------------------------------- /notebooks/grid-isaac/humanoid_ai_models.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "172aa063", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.humanoid.isaac_humanoid import IsaacHumanoid\n", 11 | "humanoidisaacsim_0 = IsaacHumanoid()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "a5cf555f", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "# Start the humanoid simulation\n", 22 | "humanoidisaacsim_0.run()\n", 23 | "\n", 24 | "# Import necessary utilities for velocity control, logging, and timing\n", 25 | "from grid.utils.types import Velocity\n", 26 | "from grid.utils import log\n", 27 | "import time" 28 | ] 29 | }, 30 | { 31 | "cell_type": "code", 32 | "execution_count": null, 33 | "id": "a7ea146d", 34 | "metadata": {}, 35 | "outputs": [], 36 | "source": [ 37 | "# Rotate the robot to get a better view of the scene, while capturing and logging images for 5 seconds.\n", 38 | "\n", 39 | "start_time = time.time()\n", 40 | "while time.time() - start_time < 5:\n", 41 | " # Turn the robot by applying a small angular velocity (in this case, rotating about the Z-axis)\n", 42 | " humanoidisaacsim_0.locomotion.moveByVelocity(Velocity(0, 0, 0), Velocity(0, 0, -2))\n", 43 | "\n", 44 | " # Retrieve the RGB and Depth images from the simulation\n", 45 | " rgb_img = humanoidisaacsim_0.locomotion.getImage()\n", 46 | " depth_img = humanoidisaacsim_0.locomotion.getImage(\"camera_depth_0\")\n", 47 | "\n", 48 | " # Log the RGB image if available\n", 49 | " if rgb_img.data is not None:\n", 50 | " log(\"rgb_img\", rgb_img)\n", 51 | " else:\n", 52 | " print(\"RGB image is none\")\n", 53 | "\n", 54 | " # Log the depth image if available; scale the depth data for visualization purposes\n", 55 | " if depth_img.data is not None:\n", 56 | " depth_img.data = depth_img.data * 255\n", 57 | " log(\"depth_img\", depth_img)\n", 58 | " else:\n", 59 | " print(\"Depth image is none\")\n", 60 | " \n", 61 | " time.sleep(0.2)\n", 62 | "\n", 63 | "# Stop the robot's movement after the loop completes\n", 64 | "humanoidisaacsim_0.locomotion.moveByVelocity(Velocity(0, 0, 0), Velocity(0, 0, 0))" 65 | ] 66 | }, 67 | { 68 | "cell_type": "code", 69 | "execution_count": null, 70 | "id": "a611c187", 71 | "metadata": {}, 72 | "outputs": [], 73 | "source": [ 74 | "# Import and initialize perception models for segmentation, depth estimation, and vision-language processing.\n", 75 | "\n", 76 | "from grid.model.perception.segmentation.oneformer import OneFormer \n", 77 | "from grid.model.perception.depth.depth_anything_v2 import DepthAnything_V2 \n", 78 | "from grid.model.perception.vlm.llava import LLaVA\n", 79 | "\n", 80 | "# Create model instances\n", 81 | "seg_model = OneFormer()\n", 82 | "depth_model = DepthAnything_V2()\n", 83 | "vlm_model = LLaVA()\n", 84 | "\n", 85 | "# Retrieve the latest RGB image from the simulation for processing\n", 86 | "rgb = humanoidisaacsim_0.getImage()\n", 87 | "\n", 88 | "# Import rerun for logging the perception outputs\n", 89 | "import rerun as rr" 90 | ] 91 | }, 92 | { 93 | "cell_type": "code", 94 | "execution_count": null, 95 | "id": "d2110578", 96 | "metadata": {}, 97 | "outputs": [], 98 | "source": [ 99 | "# Run the segmentation model with a \"panoptic\" prompt and log the result.\n", 100 | "seg = seg_model.run(rgb_img.data, \"panoptic\")\n", 101 | "rr.log(\"humanoid/segmentation\", rr.SegmentationImage(seg))\n", 102 | "\n", 103 | "# Run the depth estimation model and log the result.\n", 104 | "depth = depth_model.run(rgb_img.data)\n", 105 | "rr.log(\"humanoid/depth\", rr.DepthImage(depth))\n", 106 | "\n", 107 | "# Run the vision-language model to generate a caption describing the scene.\n", 108 | "caption = vlm_model.run(rgb_img.data, \"Describe what you see.\")\n", 109 | "print(caption)\n", 110 | "\n", 111 | "# You can modify the caption prompt, or ask a question, and rerun this cell to see updated results." 112 | ] 113 | } 114 | ], 115 | "metadata": { 116 | "language_info": { 117 | "name": "python" 118 | } 119 | }, 120 | "nbformat": 4, 121 | "nbformat_minor": 5 122 | } 123 | -------------------------------------------------------------------------------- /notebooks/grid-isaac/humanoid_detect_navigate.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "a611c187", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.humanoid.isaac_humanoid import IsaacHumanoid\n", 11 | "humanoidisaacsim_0 = IsaacHumanoid()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "4a334b2c", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "from grid.model.perception.detection.owlv2 import OWLv2\n", 22 | "det_model = OWLv2()\n", 23 | "obj = \"forklift\"\n", 24 | "\n", 25 | "humanoidisaacsim_0.run()" 26 | ] 27 | }, 28 | { 29 | "cell_type": "code", 30 | "execution_count": null, 31 | "id": "c6e2af13", 32 | "metadata": {}, 33 | "outputs": [], 34 | "source": [ 35 | "def get_box_center(box, image_size):\n", 36 | " \"\"\"\n", 37 | " Calculate the center of a single bounding box given in xyxy format.\n", 38 | "\n", 39 | " Args:\n", 40 | " box (list or np.ndarray): A bounding box with format [x_min, y_min, x_max, y_max].\n", 41 | " \n", 42 | " Returns:\n", 43 | " tuple: Center point (x_center, y_center).\n", 44 | " \"\"\"\n", 45 | " x_min, y_min, x_max, y_max = box\n", 46 | " if image_size[0] * image_size[1] * .4 < (x_max-x_min) * (y_max-y_min):\n", 47 | " return -1, -1\n", 48 | " print(\"proportion: \", (((x_max-x_min)*(y_max-y_min))/(image_size[0]*image_size[1])))\n", 49 | " x_center = (x_min + x_max) / 2\n", 50 | " y_center = (y_min + y_max) / 2\n", 51 | " return x_center, y_center\n", 52 | "\n", 53 | "from grid.utils.types import Velocity\n", 54 | "\n", 55 | "def get_velocity(target, image_size):\n", 56 | " \"\"\"\n", 57 | " Calculate velocity components based on the target position in the image.\n", 58 | "\n", 59 | " Args:\n", 60 | " target (list or np.ndarray): Target position in the image (x, y).\n", 61 | " image_size (list or np.ndarray): Size of the image (width, height).\n", 62 | " \n", 63 | " Returns:\n", 64 | " tuple: Linear velocities (x, y) and angular velocity (yaw).\n", 65 | " \"\"\"\n", 66 | " image_center_x = image_size[0] / 2\n", 67 | " image_center_y = image_size[1] / 2\n", 68 | "\n", 69 | " # Compute offset from the center of the image\n", 70 | " offset_x = target[0] - image_center_x\n", 71 | " offset_y = target[1] - image_center_y\n", 72 | "\n", 73 | " # Normalized offset\n", 74 | " norm_offset_x = offset_x / image_center_x\n", 75 | " norm_offset_y = offset_y / image_center_y\n", 76 | "\n", 77 | " # Velocity calculations\n", 78 | " linear_velocity_x = norm_offset_x # [-1, 1] range\n", 79 | " linear_velocity_y = norm_offset_y # [-1, 1] range\n", 80 | "\n", 81 | " # Angular velocity (yaw)\n", 82 | " angular_velocity = norm_offset_x # Using x offset for yaw\n", 83 | "\n", 84 | " # Clamp values to ensure they're within the range [-1, 1]\n", 85 | " linear_velocity_x = 0.8\n", 86 | " linear_velocity_y = 0\n", 87 | " angular_velocity_z = max(min(-angular_velocity, 2), -2)\n", 88 | "\n", 89 | " linear_velocity = Velocity(linear_velocity_x, linear_velocity_y, 0)\n", 90 | " angular_velocity = Velocity(0, 0, angular_velocity_z)\n", 91 | "\n", 92 | " return linear_velocity, angular_velocity" 93 | ] 94 | }, 95 | { 96 | "cell_type": "code", 97 | "execution_count": null, 98 | "id": "23d49c4c", 99 | "metadata": {}, 100 | "outputs": [], 101 | "source": [ 102 | "import time\n", 103 | "from grid.utils import log\n", 104 | "import numpy as np\n", 105 | "\n", 106 | "while True:\n", 107 | " # Get the RGB image from the camera\n", 108 | " rgb = humanoidisaacsim_0.getImage(\"camera_rgb_0\")\n", 109 | "\n", 110 | " if rgb is not None and rgb.data is not None:\n", 111 | " log(\"grid/rgb_img\", rgb)\n", 112 | " # Detect the object\n", 113 | "\n", 114 | " data = rgb.data\n", 115 | " boxes, scores, labels = det_model.run(rgbimage=data[:, :, :3], prompt=obj)\n", 116 | " if boxes is not None and len(boxes) > 0:\n", 117 | " print(\"scores: \", scores)\n", 118 | " i = np.argmax(scores)\n", 119 | " mid_x, mid_y = get_box_center(boxes[i], [data.shape[1], data.shape[0]])\n", 120 | " if mid_x == -1 and mid_y == -1:\n", 121 | " humanoidisaacsim_0.locomotion.moveByVelocity(Velocity(0, 0, 0), Velocity(0, 0, 0))\n", 122 | " break\n", 123 | " linear_vel, angular_vel = get_velocity([mid_x, mid_y], [data.shape[1], data.shape[0]])\n", 124 | " else:\n", 125 | " linear_vel = Velocity(0, 0, 0)\n", 126 | " angular_vel = Velocity(0, 0, 1)\n", 127 | " humanoidisaacsim_0.locomotion.moveByVelocity(linear_vel, angular_vel)\n", 128 | " \n", 129 | " time.sleep(.1)" 130 | ] 131 | } 132 | ], 133 | "metadata": { 134 | "language_info": { 135 | "name": "python" 136 | } 137 | }, 138 | "nbformat": 4, 139 | "nbformat_minor": 5 140 | } 141 | -------------------------------------------------------------------------------- /notebooks/grid-isaac/locomotion_data_capture.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "b0ccd951", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.locomotion.isaac_locomotion import IsaacLocomotion\n", 11 | "locomotionisaacsim_0 = IsaacLocomotion()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "4baa3c64", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "# Activate the robot client\n", 22 | "\n", 23 | "locomotionisaacsim_0.run()" 24 | ] 25 | }, 26 | { 27 | "cell_type": "code", 28 | "execution_count": null, 29 | "id": "ce31ed2d", 30 | "metadata": {}, 31 | "outputs": [], 32 | "source": [ 33 | "import time\n", 34 | "import h5py\n", 35 | "import numpy as np\n", 36 | "from grid.utils.types import Velocity\n", 37 | "from grid.utils import log\n", 38 | "\n", 39 | "# -----------------------------------------------------------------------------\n", 40 | "# Initialize lists for storing captured image data and timestamps.\n", 41 | "# - The RGB images and depth images are stored in separate lists.\n", 42 | "# - Timestamps are recorded to associate each capture with a time.\n", 43 | "# -----------------------------------------------------------------------------\n", 44 | "rgb_images_list = [] # Will hold raw RGB image data for HDF5 saving.\n", 45 | "depth_images_list = [] # Will hold raw depth image data for HDF5 saving.\n", 46 | "timestamps_list = [] # Will record the timestamp for each image capture.\n", 47 | "\n", 48 | "# -----------------------------------------------------------------------------\n", 49 | "# First Loop: Rotate in Place and Capture Images for 5 Seconds\n", 50 | "# -----------------------------------------------------------------------------\n", 51 | "start_time = time.time()\n", 52 | "while time.time() - start_time < 5:\n", 53 | " # Command the robot to rotate in place.\n", 54 | " # The first argument is linear velocity (set to zero) and the second is angular velocity.\n", 55 | " locomotionisaacsim_0.moveByVelocity(Velocity(0, 0, 0), Velocity(0, 0, 2))\n", 56 | "\n", 57 | " # Retrieve images from the RGB and depth cameras.\n", 58 | " rgb_img = locomotionisaacsim_0.getImage()\n", 59 | " depth_img = locomotionisaacsim_0.getImage(\"camera_depth_0\")\n", 60 | " current_time = time.time() # Capture the current time for timestamping.\n", 61 | "\n", 62 | " # ------------------------------\n", 63 | " # Process and Log the RGB Image\n", 64 | " # ------------------------------\n", 65 | " if rgb_img.data is not None:\n", 66 | " # Log the RGB image for visualization.\n", 67 | " log(\"rgb_img\", rgb_img)\n", 68 | " # Save the raw RGB image data for later HDF5 storage.\n", 69 | " rgb_images_list.append(rgb_img.data)\n", 70 | " else:\n", 71 | " print(\"Did not receive RGB data\")\n", 72 | "\n", 73 | " # ------------------------------\n", 74 | " # Process and Log the Depth Image\n", 75 | " # ------------------------------\n", 76 | " if depth_img.data is not None:\n", 77 | " # Save the raw depth image data for HDF5 storage.\n", 78 | " depth_images_list.append(depth_img.data)\n", 79 | " # For visualization only: scale the depth image data.\n", 80 | " depth_img.data = depth_img.data * 255\n", 81 | " log(\"depth_img\", depth_img)\n", 82 | " else:\n", 83 | " print(\"Did not receive depth data\")\n", 84 | "\n", 85 | " # Record the timestamp for this capture.\n", 86 | " timestamps_list.append(current_time)\n", 87 | "\n", 88 | " # Sleep briefly to control the capture rate (0.2 seconds between captures).\n", 89 | " time.sleep(0.2)\n", 90 | "\n", 91 | "# -----------------------------------------------------------------------------\n", 92 | "# Second Loop: Move Forward and Capture Images for 10 Seconds\n", 93 | "# -----------------------------------------------------------------------------\n", 94 | "start_time = time.time()\n", 95 | "while time.time() - start_time < 10:\n", 96 | " # Command the robot to move forward.\n", 97 | " # Linear velocity is set to 1 while angular velocity remains zero.\n", 98 | " locomotionisaacsim_0.moveByVelocity(Velocity(1, 0, 0), Velocity(0, 0, 0))\n", 99 | "\n", 100 | " # Retrieve images from the RGB and depth cameras.\n", 101 | " rgb_img = locomotionisaacsim_0.getImage()\n", 102 | " depth_img = locomotionisaacsim_0.getImage(\"camera_depth_0\")\n", 103 | " current_time = time.time() # Record the current time.\n", 104 | "\n", 105 | " # ------------------------------\n", 106 | " # Process and Log the RGB Image\n", 107 | " # ------------------------------\n", 108 | " if rgb_img.data is not None:\n", 109 | " # Append the raw RGB image data to our list for HDF5 saving.\n", 110 | " rgb_images_list.append(rgb_img.data)\n", 111 | " # Log the RGB image for display.\n", 112 | " log(\"rgb_img\", rgb_img)\n", 113 | " else:\n", 114 | " print(\"Did not receive RGB data\")\n", 115 | "\n", 116 | " # ------------------------------\n", 117 | " # Process and Log the Depth Image\n", 118 | " # ------------------------------\n", 119 | " if depth_img.data is not None:\n", 120 | " # Save the raw depth image data for HDF5 storage.\n", 121 | " depth_images_list.append(depth_img.data)\n", 122 | " # For visualization only: scale the depth image data.\n", 123 | " depth_img.data = depth_img.data * 255\n", 124 | " log(\"depth_img\", depth_img)\n", 125 | " else:\n", 126 | " print(\"Did not receive depth data\")\n", 127 | "\n", 128 | " # Save the timestamp for this image capture.\n", 129 | " timestamps_list.append(current_time)\n", 130 | "\n", 131 | " # Sleep briefly to maintain the capture rate.\n", 132 | " time.sleep(0.2)\n", 133 | "\n", 134 | "# -----------------------------------------------------------------------------\n", 135 | "# Stop the Robot's Movement\n", 136 | "# -----------------------------------------------------------------------------\n", 137 | "# Send zero velocities to halt any movement.\n", 138 | "locomotionisaacsim_0.moveByVelocity(Velocity(0, 0, 0), Velocity(0, 0, 0))\n", 139 | "print(\"Done!\")\n" 140 | ] 141 | }, 142 | { 143 | "cell_type": "code", 144 | "execution_count": null, 145 | "id": "e5d14cf9", 146 | "metadata": {}, 147 | "outputs": [], 148 | "source": [ 149 | "import os\n", 150 | "\n", 151 | "# -----------------------------------------------------------------------------\n", 152 | "# Convert the Captured Data to NumPy Arrays\n", 153 | "# -----------------------------------------------------------------------------\n", 154 | "# This conversion is done so that the data can be easily stored in HDF5.\n", 155 | "# It assumes that all images have consistent dimensions.\n", 156 | "rgb_images_np = np.array(rgb_images_list)\n", 157 | "depth_images_np = np.array(depth_images_list)\n", 158 | "timestamps_np = np.array(timestamps_list)\n", 159 | "\n", 160 | "# Access the path of the session cloud storage\n", 161 | "from grid import GRID_USER_SESSION_BLOB_DIR\n", 162 | "\n", 163 | "hdf5_file_path = os.path.join(GRID_USER_SESSION_BLOB_DIR, \"data_log.h5\")\n", 164 | "# -----------------------------------------------------------------------------\n", 165 | "# Save the Data to an HDF5 File\n", 166 | "# -----------------------------------------------------------------------------\n", 167 | "# The HDF5 file will contain:\n", 168 | "# - 'rgb_images': the raw RGB image data.\n", 169 | "# - 'depth_images': the raw depth image data (without the 255 multiplier).\n", 170 | "# - 'timestamps': the timestamps for each image capture.\n", 171 | "with h5py.File(hdf5_file_path, \"w\") as h5f:\n", 172 | " h5f.create_dataset(\"rgb_images\", data=rgb_images_np, compression=\"gzip\")\n", 173 | " h5f.create_dataset(\"depth_images\", data=depth_images_np, compression=\"gzip\")\n", 174 | " h5f.create_dataset(\"timestamps\", data=timestamps_np, compression=\"gzip\")\n", 175 | "\n", 176 | "print(\"Images and timestamps have been saved to 'data_log.h5'\")\n" 177 | ] 178 | } 179 | ], 180 | "metadata": { 181 | "kernelspec": { 182 | "display_name": "Python 3 (ipykernel)", 183 | "language": "python", 184 | "name": "python3" 185 | }, 186 | "language_info": { 187 | "codemirror_mode": { 188 | "name": "ipython", 189 | "version": 3 190 | }, 191 | "file_extension": ".py", 192 | "mimetype": "text/x-python", 193 | "name": "python", 194 | "nbconvert_exporter": "python", 195 | "pygments_lexer": "ipython3", 196 | "version": "3.10.12" 197 | } 198 | }, 199 | "nbformat": 4, 200 | "nbformat_minor": 5 201 | } 202 | -------------------------------------------------------------------------------- /notebooks/grid-isaac/locomotion_safexploration.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "a6281c57", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "from grid.robot.locomotion.isaac_locomotion import IsaacLocomotion\n", 11 | "locomotionisaacsim_0 = IsaacLocomotion()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "21c620a7", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "from grid.model.perception.depth.midas import MIDAS\n", 22 | "from grid.utils.types import Image, Position, Velocity\n", 23 | "from grid.utils.logger import log\n", 24 | "\n", 25 | "import numpy as np\n", 26 | "import time\n", 27 | "import cv2\n", 28 | "\n", 29 | "locomotionisaacsim_0.run()" 30 | ] 31 | }, 32 | { 33 | "cell_type": "code", 34 | "execution_count": null, 35 | "id": "51d49492", 36 | "metadata": {}, 37 | "outputs": [], 38 | "source": [ 39 | "def move(agent, speed, rad):\n", 40 | " \"\"\"\n", 41 | " Moves the agent for a short duration using a given linear speed and angular velocity.\n", 42 | " \"\"\"\n", 43 | " start_time = time.time()\n", 44 | " while time.time() - start_time < 0.1:\n", 45 | " agent.moveByVelocity(Velocity(speed, 0, 0), Velocity(0, 0, rad))\n", 46 | "\n", 47 | "\n", 48 | "def get_formatted_midas_image(rgb_image: Image, depth_model):\n", 49 | " \"\"\"\n", 50 | " Converts an RGB image to a normalized depth image using the depth model.\n", 51 | " \"\"\"\n", 52 | " depth_image = depth_model.run(rgb_image.data)\n", 53 | " formatted = (depth_image * 255 / np.max(depth_image)).astype(\"uint8\")\n", 54 | " return formatted\n", 55 | "\n", 56 | "\n", 57 | "def divide_into_grid(depth_image: np.ndarray, num_horizontal_patches: int, num_vertical_patches: int):\n", 58 | " \"\"\"\n", 59 | " Splits a depth image into a grid of patches.\n", 60 | " \"\"\"\n", 61 | " patches = []\n", 62 | " patch_height = depth_image.shape[0] // num_vertical_patches\n", 63 | " patch_width = depth_image.shape[1] // num_horizontal_patches\n", 64 | " for v in range(num_vertical_patches):\n", 65 | " for h in range(num_horizontal_patches):\n", 66 | " patch = depth_image[v * patch_height:(v + 1) * patch_height,\n", 67 | " h * patch_width:(h + 1) * patch_width]\n", 68 | " patches.append(patch)\n", 69 | " return patches\n", 70 | "\n", 71 | "\n", 72 | "def compute_safety_metric(patches: list):\n", 73 | " \"\"\"\n", 74 | " Computes a safety metric (mean depth) for each patch.\n", 75 | " \"\"\"\n", 76 | " return [np.mean(patch) for patch in patches]\n", 77 | "\n", 78 | "\n", 79 | "def determine_best_patch(safety_metrics, num_horizontal_patches, num_vertical_patches):\n", 80 | " \"\"\"\n", 81 | " Finds the patch with the lowest mean depth (i.e. the safest patch).\n", 82 | " \"\"\"\n", 83 | " metrics_grid = np.array(safety_metrics).reshape((num_vertical_patches, num_horizontal_patches))\n", 84 | " best_patch_index = np.unravel_index(np.argmin(metrics_grid), metrics_grid.shape)\n", 85 | " return best_patch_index\n", 86 | "\n", 87 | "\n", 88 | "def check_blocked(patches: list, means: list, num_horizontal_patches: int, num_vertical_patches: int,\n", 89 | " obs_threshold_ratio: float = 0.5, dist_threshold: float = 100):\n", 90 | " \"\"\"\n", 91 | " Checks if the center patch is too blocked or if all patches have high depth values.\n", 92 | " \"\"\"\n", 93 | " np_patches = np.array(patches)\n", 94 | " odd = len(patches) % 2\n", 95 | " middle_i = [len(patches) // 2]\n", 96 | " if not odd:\n", 97 | " middle_i.append(len(patches) // 2 - 1)\n", 98 | " for i in middle_i:\n", 99 | " patch = np_patches[i]\n", 100 | " patch[patch < 100] = 0\n", 101 | " patch[patch > 0] = 1\n", 102 | " print(f\"Center patch has {np.mean(patch)} blocked ratio\")\n", 103 | " if np.mean(patch) > obs_threshold_ratio:\n", 104 | " return True, -1\n", 105 | " np_means = np.array(means)\n", 106 | " print(f\"Means: {np_means}\")\n", 107 | " if np_means.all() > dist_threshold:\n", 108 | " return True, -1\n", 109 | " else:\n", 110 | " return False, determine_best_patch(means, num_horizontal_patches, num_vertical_patches)\n", 111 | "\n", 112 | "\n", 113 | "def map_patch_to_steering(patch_index, num_horizontal_patches, num_vertical_patches):\n", 114 | " \"\"\"\n", 115 | " Maps the chosen patch index to steering commands (yaw and pitch).\n", 116 | " \"\"\"\n", 117 | " yaw = (patch_index[1] - num_horizontal_patches // 2) * (180 / num_horizontal_patches)\n", 118 | " pitch = (patch_index[0] - num_vertical_patches // 2) * (180 / num_vertical_patches)\n", 119 | " print(\"Steering command (yaw, pitch):\", yaw, pitch)\n", 120 | " return yaw, pitch" 121 | ] 122 | }, 123 | { 124 | "cell_type": "code", 125 | "execution_count": null, 126 | "id": "2a870740", 127 | "metadata": {}, 128 | "outputs": [], 129 | "source": [ 130 | "def safenav_main(agent, rgb_image, depth_model):\n", 131 | " \"\"\"\n", 132 | " Processes the RGB image to produce a depth map, divides it into patches, computes safety metrics,\n", 133 | " checks for obstacles, and steers the agent accordingly.\n", 134 | " \"\"\"\n", 135 | " try:\n", 136 | " grid_size = 3 # Number of horizontal patches (vertical patches is set to 1)\n", 137 | " depth_image = get_formatted_midas_image(rgb_image, depth_model)\n", 138 | " log(\"grid/safenav/depth_image\", Image(depth_image))\n", 139 | " \n", 140 | " patches_img = divide_into_grid(depth_image, grid_size, 1)\n", 141 | " means = compute_safety_metric(patches_img)\n", 142 | " blocked, best_patch_index = check_blocked(patches_img, means, grid_size, 1)\n", 143 | " \n", 144 | " if blocked:\n", 145 | " print(\"Path is blocked! Rotating in place.\")\n", 146 | " move(agent, 0, 1) # Rotate if blocked\n", 147 | " return\n", 148 | "\n", 149 | " yaw, pitch = map_patch_to_steering(best_patch_index, grid_size, 1)\n", 150 | " yaw_clip = (-30, 30)\n", 151 | " yaw = np.clip(yaw, yaw_clip[0], yaw_clip[1])\n", 152 | " print(\"Clipped yaw:\", yaw)\n", 153 | "\n", 154 | " yaw_rad = np.deg2rad(yaw)\n", 155 | " move(agent, 1, -yaw_rad)\n", 156 | " except KeyboardInterrupt:\n", 157 | " raise\n", 158 | " except Exception as e:\n", 159 | " print(f\"Error in safenav_main: {e}\")\n", 160 | "\n", 161 | "\n", 162 | "def main_navigation_loop(agent, camera_intrinsics=[128, 128, 128, 128]):\n", 163 | " \"\"\"\n", 164 | " Continuously captures images from the agent, processes them for depth, and navigates safely.\n", 165 | " \"\"\"\n", 166 | " depth = MIDAS()\n", 167 | "\n", 168 | " while True:\n", 169 | " rgb_image = agent.getImage()\n", 170 | " if rgb_image is None or rgb_image.data is None:\n", 171 | " print(\"RGB image is none, waiting...\")\n", 172 | " time.sleep(1)\n", 173 | " continue\n", 174 | " \n", 175 | " log(\"grid/rgb_image\", rgb_image)\n", 176 | " safenav_main(agent, rgb_image, depth)" 177 | ] 178 | }, 179 | { 180 | "cell_type": "code", 181 | "execution_count": null, 182 | "id": "505a29a0", 183 | "metadata": {}, 184 | "outputs": [], 185 | "source": [ 186 | "def start():\n", 187 | " \"\"\"\n", 188 | " Initializes and starts the safe navigation loop.\n", 189 | " \"\"\"\n", 190 | " agent = locomotionisaacsim_0\n", 191 | " agent.run()\n", 192 | " main_navigation_loop(agent)\n", 193 | "\n", 194 | "# Begin safe exploration using the navigation loop\n", 195 | "start()" 196 | ] 197 | } 198 | ], 199 | "metadata": { 200 | "language_info": { 201 | "name": "python" 202 | } 203 | }, 204 | "nbformat": 4, 205 | "nbformat_minor": 5 206 | } 207 | -------------------------------------------------------------------------------- /notebooks/hello_grid.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": { 7 | "vscode": { 8 | "languageId": "plaintext" 9 | } 10 | }, 11 | "outputs": [], 12 | "source": [ 13 | "\n", 14 | "import rerun as rr\n", 15 | "import airgen\n", 16 | "import time\n", 17 | "client = airgen_car_0.client\n", 18 | "client.enableApiControl(True)\n", 19 | "client.setCarTargetSpeed(5)\n", 20 | "time.sleep(10)\n", 21 | "client.setCarTargetSpeed(0)\n", 22 | "image = client.getImages(\"front_center\", [0])\n", 23 | "rr.log(\"Scene\", rr.Image(image[0][0]))" 24 | ] 25 | }, 26 | { 27 | "cell_type": "code", 28 | "execution_count": null, 29 | "metadata": { 30 | "vscode": { 31 | "languageId": "plaintext" 32 | } 33 | }, 34 | "outputs": [], 35 | "source": [ 36 | "road = seg_gsam_0.run(image[0][0], \"road\")" 37 | ] 38 | }, 39 | { 40 | "cell_type": "code", 41 | "execution_count": null, 42 | "metadata": { 43 | "vscode": { 44 | "languageId": "plaintext" 45 | } 46 | }, 47 | "outputs": [], 48 | "source": [ 49 | "image_new = client.getImages(\"back_center\", [0])\n", 50 | "rr.log(\"Car\", rr.Image(image_new[0][0]))" 51 | ] 52 | }, 53 | { 54 | "cell_type": "code", 55 | "execution_count": null, 56 | "metadata": { 57 | "vscode": { 58 | "languageId": "plaintext" 59 | } 60 | }, 61 | "outputs": [], 62 | "source": [ 63 | "car = seg_gsam_0.run(image_new[0][0], \"car\")" 64 | ] 65 | }, 66 | { 67 | "cell_type": "code", 68 | "execution_count": null, 69 | "metadata": { 70 | "vscode": { 71 | "languageId": "plaintext" 72 | } 73 | }, 74 | "outputs": [], 75 | "source": [ 76 | "client.setCarTargetSpeed(5)\n", 77 | "while True:\n", 78 | " if client.simGetVehiclePose().position.x_val <= -120:\n", 79 | " client.setCarTargetSpeed(0)\n", 80 | " break\n", 81 | "print(client.simGetVehiclePose().position)" 82 | ] 83 | }, 84 | { 85 | "cell_type": "code", 86 | "execution_count": null, 87 | "metadata": { 88 | "vscode": { 89 | "languageId": "plaintext" 90 | } 91 | }, 92 | "outputs": [], 93 | "source": [ 94 | "controls = client.getCarControls()\n", 95 | "controls.is_manual_gear = False\n", 96 | "controls.steering = 1.0\n", 97 | "client.setCarControls(controls)\n", 98 | "client.setCarTargetSpeed(2.1)\n", 99 | "time.sleep(3)\n", 100 | "client.setCarTargetSpeed(0)" 101 | ] 102 | }, 103 | { 104 | "cell_type": "code", 105 | "execution_count": null, 106 | "metadata": { 107 | "vscode": { 108 | "languageId": "plaintext" 109 | } 110 | }, 111 | "outputs": [], 112 | "source": [ 113 | "controls.steering = 0.0\n", 114 | "client.setCarControls(controls)\n", 115 | "client.setCarTargetSpeed(2)\n", 116 | "time.sleep(3)\n", 117 | "client.setCarTargetSpeed(0)" 118 | ] 119 | }, 120 | { 121 | "cell_type": "code", 122 | "execution_count": null, 123 | "metadata": { 124 | "vscode": { 125 | "languageId": "plaintext" 126 | } 127 | }, 128 | "outputs": [], 129 | "source": [ 130 | "def get_lidar_data(client):\n", 131 | " lidar_data = client.getLidarData()\n", 132 | " if len(lidar_data.point_cloud) < 3:\n", 133 | " print(\"No points received from Lidar\")\n", 134 | " else:\n", 135 | " points = np.array(lidar_data.point_cloud, dtype=np.float32)\n", 136 | " points = np.reshape(points, (int(points.shape[0] / 3), 3))\n", 137 | " points_xyz = np.zeros_like(points)\n", 138 | " points_xyz[:, 0] = points[:, 0]\n", 139 | " points_xyz[:, 1] = points[:, 1]\n", 140 | " points_xyz[:, 2] = -points[:, 2] + 1\n", 141 | " print(\"Got LiDAR Data\")\n", 142 | " return points_xyz\n", 143 | " " 144 | ] 145 | }, 146 | { 147 | "cell_type": "code", 148 | "execution_count": null, 149 | "metadata": { 150 | "vscode": { 151 | "languageId": "plaintext" 152 | } 153 | }, 154 | "outputs": [], 155 | "source": [ 156 | "\n", 157 | "%matplotlib inline\n", 158 | "import os\n", 159 | "from grid import GRID_USER_SESSION_BLOB_DIR\n", 160 | "import numpy as np\n", 161 | "import matplotlib.pyplot as plt\n", 162 | "points = get_lidar_data(client)\n", 163 | "fig = plt.figure()\n", 164 | "ax = fig.add_subplot(projection='3d')\n", 165 | "for point in points:\n", 166 | " ax.scatter(point[0], point[1], point[2])\n", 167 | "ax.set_xlabel('X Label')\n", 168 | "ax.set_ylabel('Y Label')\n", 169 | "ax.set_zlabel('Z Label')\n", 170 | "save_path = os.path.join(GRID_USER_SESSION_BLOB_DIR, \"pcl.png\")\n", 171 | "plt.savefig(save_path, bbox_inches='tight')\n", 172 | "print(\"Saved!\")" 173 | ] 174 | }, 175 | { 176 | "cell_type": "code", 177 | "execution_count": null, 178 | "metadata": { 179 | "vscode": { 180 | "languageId": "plaintext" 181 | } 182 | }, 183 | "outputs": [], 184 | "source": [ 185 | "\n", 186 | "import cv2\n", 187 | "img = cv2.imread(save_path)\n", 188 | "rr.log(\"PCL\", rr.Image(img))" 189 | ] 190 | } 191 | ], 192 | "metadata": { 193 | "language_info": { 194 | "name": "python" 195 | } 196 | }, 197 | "nbformat": 4, 198 | "nbformat_minor": 2 199 | } 200 | -------------------------------------------------------------------------------- /notebooks/metadata.json: -------------------------------------------------------------------------------- 1 | { 2 | "drone_ai_models": { 3 | "filename": "ai_models_drone.ipynb", 4 | "title": "Drone AI Models", 5 | "simMode": "Multirotor", 6 | "geo": false 7 | }, 8 | "drone_data_capture": { 9 | "filename": "drone_data_capture.ipynb", 10 | "title": "Drone Data Capture", 11 | "simMode": "Multirotor", 12 | "geo": false 13 | }, 14 | "drone_gps_nav": { 15 | "filename": "drone_gps_nav.ipynb", 16 | "title": "Drone GPS Navigation", 17 | "simMode": "Multirotor", 18 | "geo": true 19 | }, 20 | "drone_control": { 21 | "filename": "drone_control.ipynb", 22 | "title": "Drone Control", 23 | "simMode": "Multirotor", 24 | "geo": false 25 | }, 26 | "drone_thermal_camera": { 27 | "filename": "drone_thermal_camera.ipynb", 28 | "title": "Drone Thermal Camera", 29 | "simMode": "Multirotor", 30 | "geo": false 31 | }, 32 | "drone_path_planning": { 33 | "filename": "drone_path_planning.ipynb", 34 | "title": "Drone Path Planning", 35 | "simMode": "Multirotor", 36 | "geo": true 37 | }, 38 | "car_ai_models": { 39 | "filename": "ai_models_car.ipynb", 40 | "title": "Car AI Models", 41 | "simMode": "Car", 42 | "geo": false 43 | }, 44 | "car_control": { 45 | "filename": "car_control.ipynb", 46 | "title": "Car Control", 47 | "simMode": "Car", 48 | "geo": false 49 | }, 50 | "vehicle_control": { 51 | "filename": "vehicle_control.ipynb", 52 | "title": "Vehicle Control", 53 | "simMode": "ComputerVision", 54 | "geo": false 55 | }, 56 | "car_data_capture": { 57 | "filename": "car_data_capture.ipynb", 58 | "title": "Car Data Capture", 59 | "simMode": "Car", 60 | "geo": false 61 | }, 62 | "car_lidar": { 63 | "filename": "car_lidar.ipynb", 64 | "title": "Car LIDAR", 65 | "simMode": "Car", 66 | "geo": false 67 | }, 68 | "car_path_planning": { 69 | "filename": "car_path_planning.ipynb", 70 | "title": "Car Path Planning", 71 | "simMode": "Car", 72 | "geo": true 73 | }, 74 | "car_visual_odometry": { 75 | "filename": "car_visual_odometry.ipynb", 76 | "title": "Car Visual Odometry", 77 | "simMode": "Car", 78 | "geo": false 79 | }, 80 | "simulation_control": { 81 | "filename": "simulation_control.ipynb", 82 | "title": "Simulation Control", 83 | "simMode": "any", 84 | "geo": false 85 | }, 86 | "depth_point_cloud": { 87 | "filename": "depth_point_cloud.ipynb", 88 | "title": "Depth to Point Cloud", 89 | "simMode": "any", 90 | "geo": false 91 | }, 92 | "scalable_data_generation": { 93 | "filename": "scalable_data_generation.ipynb", 94 | "title": "Scalable Data Generation", 95 | "simMode": "Car", 96 | "geo": false 97 | } 98 | } 99 | -------------------------------------------------------------------------------- /notebooks/scalable_data_generation.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "id": "11ca3773-1b2d-4284-8d3e-ed8008538af6", 6 | "metadata": {}, 7 | "source": [ 8 | "# **Data Generation Pipeline Tutorial**\n", 9 | "\n", 10 | "---" 11 | ] 12 | }, 13 | { 14 | "cell_type": "markdown", 15 | "id": "5a1d3e2c-cbbe-4607-b599-0a418c42a8d8", 16 | "metadata": {}, 17 | "source": [ 18 | "Scalable data collection is crucial in robotics because it enables rapid prototyping, robust model training, and extensive testing across various real-world scenarios. Traditional data collection can be time-consuming, resource-intensive, and limited by physical constraints. GRID overcomes these challenges by providing a virtual environment where users can quickly simulate and customize complex scenarios, configure sensors, and automate data generation at scale. This means faster iterations, broader coverage of edge cases, and robust datasets that accelerate the development of reliable, intelligent and safe robotics systems.\n", 19 | "\n", 20 | "In this tutorial, we will guide you through a comprehensive end-to-end workflow for generating diverse multimodal sensor data at scale on the GRID platform." 21 | ] 22 | }, 23 | { 24 | "cell_type": "markdown", 25 | "id": "552139d2-2b97-4930-ad54-9f4677c25fee", 26 | "metadata": {}, 27 | "source": [ 28 | "Let’s begin by setting up the environment, configuring the robot, and defining simulation parameters. We’ll generate a trajectory, set up sensors, and enable autonomous data collection. Finally, we’ll see how GRID Enterprise scales this process for efficient, large-scale data generation.\n", 29 | "\n", 30 | "### Tutorial Outline\n", 31 | "\n", 32 | "1. **Scene Selection and Customisation**: Initialize environment, configure robot and sensors, customize weather, wind, and time settings.\n", 33 | "2. **Trajectory Generation and Sensor Selection**: Generate random path, extract waypoints, and select RGB, LiDAR, and IMU modalities.\n", 34 | "3. **Autonomous Data Generation**: Gather data along the trajectory, log, and visualize.\n", 35 | "4. **Scaling with GRID Enterprise**: Parallelize data generation for large-scale projects.\n" 36 | ] 37 | }, 38 | { 39 | "cell_type": "markdown", 40 | "id": "e191a0d7-6b2f-419d-83d7-590d4a740d58", 41 | "metadata": {}, 42 | "source": [ 43 | "## Scene Selection and Customisation\n", 44 | "\n", 45 | "---" 46 | ] 47 | }, 48 | { 49 | "cell_type": "markdown", 50 | "id": "c4d4a9d5-3a37-438b-a115-ec75d2c6eace", 51 | "metadata": {}, 52 | "source": [ 53 | "GRID offers multiple customizable environments, with options to choose different robots and sensor configurations.\n", 54 | "\n", 55 | "For this demo, we will set up a neighborhood scenario using the Clearpath Husky Robot equipped with an RGB camera, LiDAR, and IMU sensors.\n", 56 | "\n", 57 | "Once the session has started, let us go ahead and import our standard modules and initialise our robot." 58 | ] 59 | }, 60 | { 61 | "cell_type": "code", 62 | "execution_count": null, 63 | "id": "7f03ef1a-56a0-4462-9075-ebb7d0878f2a", 64 | "metadata": {}, 65 | "outputs": [], 66 | "source": [ 67 | "# initialise the robot\n", 68 | "import airgen\n", 69 | "from airgen.utils.collect import data_collector\n", 70 | "from airgen import WeatherParameter, Vector3r\n", 71 | "from typing import List, Tuple, Dict, Any, Optional, Callable\n", 72 | "import rerun as rr\n", 73 | "import random, h5py, numpy as np\n", 74 | "\n", 75 | "from grid.robot.wheeled.airgen_car import AirGenCar\n", 76 | "airgen_car_0 = AirGenCar()" 77 | ] 78 | }, 79 | { 80 | "cell_type": "markdown", 81 | "id": "c3717af3-398e-4a54-b9d6-0ac32e25d8b4", 82 | "metadata": {}, 83 | "source": [ 84 | "Once the session is initialized, we can further customize the environment's physical characteristics, such as wind speed, time of day, and weather conditions. For example, let's add fog to the simulation, set the time to around sunset, and adjust the wind speed to 5 m/s. Learn more about the configuration parameters in our [docs](https://docs.scaledfoundations.ai/airgen/features/environment/weather.html)." 85 | ] 86 | }, 87 | { 88 | "cell_type": "code", 89 | "execution_count": null, 90 | "id": "68e8e0ab-bebe-403c-9f91-c3d50659fffa", 91 | "metadata": {}, 92 | "outputs": [], 93 | "source": [ 94 | "# set the weather, wind, and timeofday parameters\n", 95 | "\n", 96 | "client = airgen_car_0.client\n", 97 | "\n", 98 | "client.simEnableWeather(True)\n", 99 | "client.simSetWeatherParameter(WeatherParameter.Fog, 1.0) # adds fog to the scene\n", 100 | "# client.simSetWind(airgen.Vector3r(5, 0, 0)) # sets a 5 m/s wind in X direction, only supported for drones for now\n", 101 | "client.simSetTimeOfDay(True, \"2024-07-22 17:00:00\") # sets the time of day to be around sunset" 102 | ] 103 | }, 104 | { 105 | "cell_type": "markdown", 106 | "id": "ca3c93b1-8d72-4b39-800e-32501dbab87a", 107 | "metadata": {}, 108 | "source": [ 109 | "## Trajectory Generation and Sensor Selection\n", 110 | "\n", 111 | "---" 112 | ] 113 | }, 114 | { 115 | "cell_type": "markdown", 116 | "id": "8b848cb2-bd85-4e1b-80aa-230de97070f6", 117 | "metadata": {}, 118 | "source": [ 119 | "We will begin by initializing random source and destination points for the robot's path. The `simPlanPathToRandomFreePoint` function searches for random start and end points within a specified radius." 120 | ] 121 | }, 122 | { 123 | "cell_type": "code", 124 | "execution_count": null, 125 | "id": "57fbe4f7-8ec7-4c38-9acb-6df55df044fa", 126 | "metadata": {}, 127 | "outputs": [], 128 | "source": [ 129 | "search_radius = 100 # distance in meters\n", 130 | "path = client.simPlanPathToRandomFreePoint(search_radius, smooth_path=True, draw_path=True) # generates the trajectory of points\n", 131 | "\n", 132 | "points = []\n", 133 | "for point in path:\n", 134 | " points.append(airgen.Vector3r(point['x_val'], point['y_val'], point['z_val']))" 135 | ] 136 | }, 137 | { 138 | "cell_type": "markdown", 139 | "id": "559f4904-da1e-4c3d-9a7b-57c0e25e1db8", 140 | "metadata": {}, 141 | "source": [ 142 | "Next, we will define the modalities of data to be collected. In this tutorial, we will gather RGB, LiDAR, and IMU data." 143 | ] 144 | }, 145 | { 146 | "cell_type": "code", 147 | "execution_count": null, 148 | "id": "41bc0fdc-7d01-4c4f-9edc-49d3764998d2", 149 | "metadata": {}, 150 | "outputs": [], 151 | "source": [ 152 | "def readSensors(client: airgen.VehicleClient) -> dict:\n", 153 | " sensor_data = {}\n", 154 | " sensor_data[\"imu\"] = client.getImuData()\n", 155 | " sensor_data['rgb'] = client.getImages(\"front_center\",[airgen.ImageType.Scene])[0]\n", 156 | " sensor_data[\"lidar\"] = client.getLidarData()\n", 157 | " return sensor_data" 158 | ] 159 | }, 160 | { 161 | "cell_type": "markdown", 162 | "id": "426885bc-fb50-4c41-9fe0-0b72e2b44295", 163 | "metadata": {}, 164 | "source": [ 165 | "## Autonomous Data Generation\n", 166 | "\n", 167 | "---" 168 | ] 169 | }, 170 | { 171 | "cell_type": "markdown", 172 | "id": "2c29322a-0532-4183-ad92-fcfaf1dee28d", 173 | "metadata": {}, 174 | "source": [ 175 | "With the sensor configurations, trajectory, and environment setup in place, we can now enable the robot to collect data autonomously." 176 | ] 177 | }, 178 | { 179 | "cell_type": "code", 180 | "execution_count": null, 181 | "id": "e148dc9f-6153-4fcd-ab05-43ec1abd706d", 182 | "metadata": {}, 183 | "outputs": [], 184 | "source": [ 185 | "@data_collector(readSensors, time_delta=0.1)\n", 186 | "def move_task(\n", 187 | " client: airgen.MultirotorClient, position: Tuple[float], **kwargs\n", 188 | ") -> None | Tuple[None, List[dict]]:\n", 189 | " client.moveOnPath(points, velocity=5.0)\n", 190 | "\n", 191 | "\n", 192 | "_, sensor_data = move_task(client, (0, 0, -10), _collect_data=True)\n", 193 | "for i, data in enumerate(sensor_data):\n", 194 | " lidar = data[\"lidar\"]\n", 195 | " rgb, _ = data[\"rgb\"]\n", 196 | " rr.log(\"grid/imagery\",rr.Image(rgb))\n", 197 | " rr.log(\"pointcloud\", rr.Points3D(np.array(lidar.point_cloud).reshape(-1, 3)))\n", 198 | "\n", 199 | "print(f\"collected {len(sensor_data)} measurements during moving task\")" 200 | ] 201 | }, 202 | { 203 | "cell_type": "markdown", 204 | "id": "720f4872-3264-4aa5-9353-083cc594bccd", 205 | "metadata": {}, 206 | "source": [ 207 | "The data collected by the robot can be visualized on the rerun panel." 208 | ] 209 | }, 210 | { 211 | "cell_type": "markdown", 212 | "id": "8e447671-7960-4c15-ae83-a3beba1ccc42", 213 | "metadata": {}, 214 | "source": [ 215 | "## Scaling up the generation\n", 216 | "\n", 217 | "---" 218 | ] 219 | }, 220 | { 221 | "cell_type": "markdown", 222 | "id": "83b3b5e0-b635-4c20-87ac-097fd5c036f8", 223 | "metadata": {}, 224 | "source": [ 225 | "To effectively scale up data generation, it’s crucial to simulate diverse real-world conditions and scenarios. Here, we randomize environmental parameters such as weather, wind, and time of day to introduce variability, creating a richer dataset that enhances model robustness. Additionally, GRID allows generating multiple trajectories, enabling the robot to navigate different paths under varied conditions. This combination of dynamic settings and paths ensures scalable, consistent data generation across multiple sessions, supporting efficient large-scale projects." 226 | ] 227 | }, 228 | { 229 | "cell_type": "code", 230 | "execution_count": null, 231 | "id": "94693f90-2b9a-41ce-8114-0dca076b55f2", 232 | "metadata": {}, 233 | "outputs": [], 234 | "source": [ 235 | "import random, h5py, numpy as np\n", 236 | "from airgen import WeatherParameter, Vector3r\n", 237 | "from grid import GRID_USER_SESSION_BLOB_DIR\n", 238 | "\n", 239 | "\n", 240 | "save_path = os.path.join(GRID_USER_SESSION_BLOB_DIR, \"sensor_data.h5\")\n", 241 | "\n", 242 | "client.simEnableWeather(True)\n", 243 | "\n", 244 | "# Generate data for multiple trajectories\n", 245 | "num_trajectories = 5\n", 246 | "weather_options = [\n", 247 | " WeatherParameter.Rain, WeatherParameter.Roadwetness, WeatherParameter.Snow,\n", 248 | " WeatherParameter.RoadSnow, WeatherParameter.MapleLeaf, WeatherParameter.RoadLeaf,\n", 249 | " WeatherParameter.Dust, WeatherParameter.Fog\n", 250 | "]\n", 251 | "\n", 252 | "with h5py.File(save_path, 'a') as hdf5_file: # Open the file in append mode once\n", 253 | " for traj_idx in range(num_trajectories):\n", 254 | " client.simSetWeatherParameter(random.choice(weather_options), random.uniform(0, 1))\n", 255 | " client.simSetTimeOfDay(True, f\"2024-07-22 {random.randint(0, 23):02}:{random.randint(0, 59):02}:00\")\n", 256 | "\n", 257 | " path = client.simPlanPathToRandomFreePoint(100, smooth_path=True, draw_path=True)\n", 258 | " points = [Vector3r(p['x_val'], p['y_val'], p['z_val']) for p in path]\n", 259 | "\n", 260 | " _, sensor_data = move_task(client, (0, 0, -10), _collect_data=True)\n", 261 | "\n", 262 | " # Create a group for each trajectory\n", 263 | " traj_group = hdf5_file.create_group(f\"trajectory_{traj_idx}\")\n", 264 | "\n", 265 | " for i, data in enumerate(sensor_data):\n", 266 | " # Create subgroup for each frame\n", 267 | " frame_group = traj_group.create_group(f\"frame_{i}\")\n", 268 | " frame_group.create_dataset(\"rgb\", data=data[\"rgb\"][0])\n", 269 | " frame_group.create_dataset(\"lidar\", data=np.array(data[\"lidar\"].point_cloud).reshape(-1, 3))\n", 270 | "\n", 271 | " # Logging for visualization if required\n", 272 | " rr.log(\"grid/imagery\", rr.Image(data[\"rgb\"][0]))\n", 273 | " rr.log(\"pointcloud\", rr.Points3D(np.array(data[\"lidar\"].point_cloud).reshape(-1, 3)))\n", 274 | "\n", 275 | " print(f\"Collected {len(sensor_data)} measurements for trajectory {traj_idx + 1}\")\n", 276 | "\n", 277 | "\n" 278 | ] 279 | }, 280 | { 281 | "cell_type": "markdown", 282 | "id": "706abecb-9b16-4e46-9d36-37035955f585", 283 | "metadata": {}, 284 | "source": [ 285 | "Once the robot has explored all the trajectories by itself, the entire sensor data will be stored in the `sensor_data.h5` file which you can download and store in your own system and integrate with your pipelines." 286 | ] 287 | }, 288 | { 289 | "cell_type": "markdown", 290 | "id": "200895fb-6881-426f-8572-79efa61cddd9", 291 | "metadata": {}, 292 | "source": [ 293 | "# GRID Enterprise - Parallelisation and Optimisation\n", 294 | "\n", 295 | "To optimize and scale data collection processes, GRID Enterprise enables parallelization across multiple sessions. This feature allows for efficient generation of large datasets by running multiple instances simultaneously, significantly reducing time and computational resources needed for large-scale projects.\n", 296 | "\n", 297 | "Please refer to our [GRID-Enterprise tutorial](https://github.com/ScaledFoundations/GRID-playground/blob/main/grid-samples/datagen_enterprise.ipynb) for more information\n" 298 | ] 299 | } 300 | ], 301 | "metadata": { 302 | "kernelspec": { 303 | "display_name": "Python 3 (ipykernel)", 304 | "language": "python", 305 | "name": "python3" 306 | }, 307 | "language_info": { 308 | "codemirror_mode": { 309 | "name": "ipython", 310 | "version": 3 311 | }, 312 | "file_extension": ".py", 313 | "mimetype": "text/x-python", 314 | "name": "python", 315 | "nbconvert_exporter": "python", 316 | "pygments_lexer": "ipython3", 317 | "version": "3.11.9" 318 | } 319 | }, 320 | "nbformat": 4, 321 | "nbformat_minor": 5 322 | } 323 | -------------------------------------------------------------------------------- /notebooks/simulation_control.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "abdb3e3c", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "import airgen\n", 11 | "\n", 12 | "sim_client = airgen.VehicleClient()\n", 13 | "\n", 14 | "# Let's try to place a Cube 20 meters in front of the robot\n", 15 | "pose = sim_client.simGetVehiclePose()\n", 16 | "pose_obj = pose\n", 17 | "pose_obj.position.x_val -= 20\n", 18 | "\n", 19 | "# Spawn the object\n", 20 | "sim_client.simSpawnObject(\"Cube_New\", \"Cube\", pose_obj, airgen.Vector3r(2, 2, 2), True, False)" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "id": "aa0df1fd", 27 | "metadata": {}, 28 | "outputs": [], 29 | "source": [ 30 | "# Get the 3D position and orientation of the newly spawned object\n", 31 | "curr_pose = sim_client.simGetObjectPose(\"Cube_New\")\n", 32 | "new_pose = curr_pose\n", 33 | "\n", 34 | "# Move the object 10 meters in front of the robot\n", 35 | "new_pose.position.x_val = curr_pose.position.x_val + 10\n", 36 | "sim_client.simSetObjectPose(\"Cube_New\", new_pose)" 37 | ] 38 | }, 39 | { 40 | "cell_type": "code", 41 | "execution_count": null, 42 | "id": "2eca391b", 43 | "metadata": {}, 44 | "outputs": [], 45 | "source": [ 46 | "# Get the scale of the object\n", 47 | "curr_scale = sim_client.simGetObjectScale(\"Cube_New\")\n", 48 | "print(curr_scale)\n", 49 | "\n", 50 | "# Make the object 5x bigger\n", 51 | "sim_client.simSetObjectScale(\"Cube_New\", airgen.Vector3r(10, 10, 10))" 52 | ] 53 | }, 54 | { 55 | "cell_type": "code", 56 | "execution_count": null, 57 | "id": "6388e1fa", 58 | "metadata": {}, 59 | "outputs": [], 60 | "source": [ 61 | "# Destroy the object\n", 62 | "sim_client.simDestroyObject(\"Cube_New\")" 63 | ] 64 | }, 65 | { 66 | "cell_type": "code", 67 | "execution_count": null, 68 | "id": "b0866e28", 69 | "metadata": {}, 70 | "outputs": [], 71 | "source": [ 72 | "# Change the time of day to 10 PM\n", 73 | "sim_client.simSetTimeOfDay(True, \"2024-07-11 22:00:00\")" 74 | ] 75 | } 76 | ], 77 | "metadata": { 78 | "language_info": { 79 | "name": "python" 80 | } 81 | }, 82 | "nbformat": 4, 83 | "nbformat_minor": 5 84 | } 85 | -------------------------------------------------------------------------------- /notebooks/vehicle_control.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 19, 6 | "id": "7bc7b7af", 7 | "metadata": {}, 8 | "outputs": [ 9 | { 10 | "name": "stdout", 11 | "output_type": "stream", 12 | "text": [ 13 | "Connected!\n", 14 | "Client Ver:1 (Min Req: 1), Server Ver:1 (Min Req: 1)\n", 15 | "\n" 16 | ] 17 | } 18 | ], 19 | "source": [ 20 | "from grid.robot.airgen_vehicle import AirGenVehicle\n", 21 | "airgen_vehicle_0 = AirGenVehicle()" 22 | ] 23 | }, 24 | { 25 | "cell_type": "code", 26 | "execution_count": 20, 27 | "id": "5a4121ef", 28 | "metadata": {}, 29 | "outputs": [ 30 | { 31 | "name": "stdout", 32 | "output_type": "stream", 33 | "text": [ 34 | "current pose { 'orientation': { 'w_val': 0.6620137691497803,\n", 35 | " 'x_val': -0.630634069442749,\n", 36 | " 'y_val': 0.2864948809146881,\n", 37 | " 'z_val': -0.28628501296043396},\n", 38 | " 'position': { 'x_val': 110.16702270507812,\n", 39 | " 'y_val': 16.084627151489258,\n", 40 | " 'z_val': -0.8202161192893982}}\n" 41 | ] 42 | } 43 | ], 44 | "source": [ 45 | "pose = airgen_vehicle_0.client.simGetVehiclePose()\n", 46 | "\n", 47 | "# Move to an altitude of 25 meters\n", 48 | "print(\"current pose\", pose)" 49 | ] 50 | }, 51 | { 52 | "cell_type": "code", 53 | "execution_count": 22, 54 | "id": "60860e65", 55 | "metadata": {}, 56 | "outputs": [], 57 | "source": [ 58 | "pose.position.z_val = -25\n", 59 | "pose.position.x_val = 100\n", 60 | "airgen_vehicle_0.client.simSetVehiclePose(pose, ignore_collision=True).join()" 61 | ] 62 | } 63 | ], 64 | "metadata": { 65 | "kernelspec": { 66 | "display_name": "dev", 67 | "language": "python", 68 | "name": "python3" 69 | }, 70 | "language_info": { 71 | "codemirror_mode": { 72 | "name": "ipython", 73 | "version": 3 74 | }, 75 | "file_extension": ".py", 76 | "mimetype": "text/x-python", 77 | "name": "python", 78 | "nbconvert_exporter": "python", 79 | "pygments_lexer": "ipython3", 80 | "version": "3.11.7" 81 | } 82 | }, 83 | "nbformat": 4, 84 | "nbformat_minor": 5 85 | } 86 | --------------------------------------------------------------------------------