
In questo articolo scendiamo sulla parte relativa alla programmazione del nostro robot Unitree G1, nelle sue versioni G1 standard, G1 advanced, e G1 EDU Ultimate A, B e C.
Pronti con con il terminale aperto e andiamo!

Indice
SDK Software Development Kit

La versione corrente del software non supporta ancora lo streaming tramite GST
G1 utilizza DDS* come middleware per i messaggi e l'interazione principale dei dati adotta due modalità: sottoscrizione/pubblicazione e richiesta/risposta.
*Il Data Distribution Service (DDS) è uno standard di middleware sviluppato dall'Object Management Group (OMG) per sistemi in tempo reale. Utilizza un modello publish–subscribe per facilitare scambi di dati affidabili, ad alte prestazioni e scalabili tra diversi nodi. DDS è impiegato in settori come aerospaziale, difesa, veicoli autonomi, dispositivi medici e smart grid. Garantisce interoperabilità tra diverse implementazioni e gestisce automaticamente la qualità del servizio, semplificando la programmazione di applicazioni distribuite complesse.
Sottoscrizione/Pubblicazione: Il receiver si iscrive a un messaggio e il sender invia messaggi al receiver solo secondo l'elenco delle sottoscrizioni. È principalmente utilizzato per interazioni di dati a frequenza media-alta e continue.
Richiesta/Risposta: Modalità domanda e risposta, l'acquisizione dei dati o le operazioni vengono realizzate tramite richieste. Utilizzato per interazioni di dati a bassa frequenza o per il cambio di funzione. Questa modalità avviene tramite:
Chiamate API: Come per le chiamate REST, viene compilato il contenuto e l'argomento quando si invia la richiesta. La risposta avviene tramite broadcast e avviene secondo l'UUID (Universally unique identifier).
Chiamate Funzionale: Lo zucchero sintattico (syntactic sugar) della chiamata API, modello che incapsula le chiamate API in chiamate di funzione per facilitare l'uso da parte dell'utente. Per chi non digerisce le chiamate API!
Kit di Sviluppo allego le repo GITHUB:
unitree_sdk2: libreria sviluppo in C++ https://github.com/unitreerobotics/unitree_sdk2👨🏻💻
unitree_sdk2_python: https://github.com/unitreerobotics/unitree_sdk2_python 🐍
Architettura di Sistema

Cloud ☁️

G1 will connect to the cloud service only after user authorization
Le sue funzioni principali sono:
Raccogliere i dati operativi di G1 (non riguardanti la privacy), effettuare rilevamento guasti e statistiche.
Aiutare gli utenti a realizzare operazioni remote, principalmente tramite WebRTC
Sito: https://webrtc.org/ 🌐
Github: https://github.com/webrtc 👨💻
Il traffico di immagini e controllo può essere inoltrato punto a punto o tramite il server TURN (Traversal Using Relays around NAT).
Iterazione degli aggiornamenti di sistema.
Trasferimento dati Cloud:
mqtt un protocollo OASIS standard di messaggistica per l'IoT (IoT)
viene utilizzato per stabilire la comunicazione IoT con ogni dispositivo ed è responsabile del monitoraggio dei guasti, aggiornamenti di sistema e trasmissione del segnale WebRTC. https://github.com/mqtt 👨💻
http service collega l'App e il front-end Web per stabilire una relazione di binding tra l'utente e il robot.
Il server TURN/STUN viene utilizzato per facilitare le connessioni WebRTC punto a punto e fornire l'inoltro dei dati del server quando le connessioni punto a punto non possono essere realizzate.
G1 🦾

Il modulo OTA (Over The Air) comunica con il server cloud tramite mqtt ed è responsabile del caricamento delle informazioni sui guasti, degli aggiornamenti di sistema e dell'inoltro del segnale WebRTC.
Il modulo WebRTC implementa la principale pipeline di dati con l'App, inclusi flussi audio e video, nuvole di punti radar, stato del movimento e istruzioni di controllo.
La parte Bluetooth (BLE) viene utilizzata per stabilire il contatto con l'App ed è principalmente utilizzata per la configurazione della rete e la verifica della sicurezza.
La comunicazione tra ogni modulo funzionale è principalmente realizzata tramite DDS. DDS IDL è compatibile con ROS2 (è necessario selezionare l'RMW adattato). La versione EDU può chiamare l'interfaccia tramite DDS o ROS2.
I dati dei sensori come motori e radar vengono raccolti tramite la porta seriale e poi inoltrati al livello intermedio DDS.
G1 EDU dispone di 2 unità di calcolo integrate. PC1 è dedicato al programma di controllo del movimento Unitree e non è aperto al pubblico. Gli sviluppatori possono utilizzare solo PC2 per lo sviluppo secondario. Gli indirizzi IP per PC2 sono rispettivamente 192.168.123.164.
Si prega di contattare il supporto Unitree per il nome utente e la password iniziali.
App
Modulo di gestione utenti, collegato alla piattaforma di gestione Yushu tramite HTTP Web API. Responsabile del binding del robot, dell'instaurazione della connessione WebRTC e di altre funzioni.
Il modulo Bluetooth di G1 viene utilizzato per configurare la rete.
Il modulo WebRTC, il principale traffico di dati viene realizzato tramite WebRTC, inclusa la trasmissione di immagini, nuvole di punti, stato del movimento e emissione di comandi di controllo.
Piattaforma di sviluppo
DDS, supporta C++ e Python.
Interfaccia ROS2.
GST, solo per la trasmissione di immagini.
SDK
unitree_sdk2 è un SDK sviluppato da Unitree Technology per la nuova generazione di robot.
L'SDK incapsula interfacce come il controllo di base dei motori e il controllo del movimento avanzato, e fornisce interfacce funzionali pertinenti. Puoi seguire le interfacce e le routine che forniamo, insieme alla Guida allo Sviluppo SDK G1, per imparare il controllo del robot e completare lo sviluppo secondario di G1.
Download SDK
unitree_sdk2: libreria sviluppo in C++ https://github.com/unitreerobotics/unitree_sdk2👨🏻💻
unitree_sdk2_python: https://github.com/unitreerobotics/unitree_sdk2_python 🐍
Modello Semplificato 3D - STEP file
Modello | G1-EDU Standard | G1-EDU Avanzato | G1-EDU Ultimate A | G1-EDU Ultimate B | G1-EDU Ultimate C |
Configurazione | L'intera macchina ha 23dof | L'intera macchina ha 29dof | L'intera macchina ha 29dof + 2 mani dexterose Dex3-1 a 3 dita (senza feedback tattile) | L'intera macchina ha 29dof + 2 mani dexterose Dex3-1 a 3 dita (con feedback tattile) | L'intera macchina ha 29dof + 2 mani dexterose a 5 dita di ispirazione |
Modello Semplificato | G1-EDU Standard | G1-EDU Avanzato | G1-EDU Ultimate A | G1-EDU Ultimate B | G1-EDU Ultimate C |
URDF
Tutte le versioni di URDF, Unified Robot Description Format, di G1 sono state caricate nel repository GitHub di Yushu e possono essere scaricate tramite il seguente link:
Quick start Development
Requisiti dell'Ambiente
Si consiglia di sviluppare su Ubuntu 20.04. Non ci sono particolari richieste di potenza di calcolo.
Lo sviluppo su sistemi Mac e Windows attualmente non è supportato.
PC1 esegue il servizio ufficiale e non supporta lo sviluppo; PC2 è accessibile per lo sviluppo.
Requisiti di Rete
Connettere il computer dell'utente e lo switch G1 alla stessa rete.
Si consiglia di collegare il computer dell'utente allo switch G1 utilizzando un cavo di rete e un adattatore, e configurare la scheda di rete che comunica con il robot sulla subnet 192.168.123.X, preferibilmente utilizzando 192.168.123.99.
Gli utenti esperti possono configurare l'ambiente di rete secondo necessità.
Installazione e Compilazione
Installazione di unitree_sdk2
Per installare unitree_sdk2, navigare nella directory unitree_sdk2 ed eseguire i seguenti comandi:
cd unitree_sdk2/
mkdir build
cd build
cmake ..
sudo make install
Compilazione degli Esempi InclusiPer compilare gli esempi inclusi, navigare nella directory unitree_sdk2 ed eseguire i seguenti comandi:
cd unitree_sdk2
mkdir build
cd build
cmake ..
make
Se la compilazione ha successo, gli esempi compilati si troveranno nella directory unitree_sdk2/build.
Modifica della Configurazione di Rete
Passaggi di Configurazione:
Collegare un'estremità di un cavo di rete al robot e l'altra estremità al computer dell'utente. Abilitare USB Ethernet sul computer e configurarla. L'indirizzo IP del computer a bordo del robot è 192.168.123.161, quindi impostare l'indirizzo USB Ethernet del computer nella stessa sottorete, ad esempio 192.168.123.222.


Per testare se il computer dell'utente e il computer a bordo del robot sono correttamente connessi, inserire ping 192.168.123.161 nel terminale. Un output simile all'immagine sottostante indica una connessione riuscita.

Identificare il nome della scheda di rete corrispondente alla sottorete 123 utilizzando il comando ifconfig, come mostrato di seguito:
Il nome della scheda di rete corrispondente all'IP 192.168.123.222 è enxf8e43b808e06. Annotare questo nome poiché sarà richiesto quando si eseguono gli esempi.
Esecuzione degli Esempi
Ingresso in Modalità Debug
Seguire la Quick Start Guide per assicurarsi che il robot sia in modalità debug.
Compilazione ed Esecuzione della Routine di Controllo a Basso Livello
Nota: Eseguire questa routine muoverà più giunti del G1. Per proteggere il robot, sospendere il robot prima di eseguire questa routine.
cd unitree_sdk2
cmake -Bbuild
cmake --build build
./build/bin/g1_ankle_swing_example network_interface_name
Basic Motion Routine
La routine di sviluppo del motion a basso livello implementa il reset del robot da qualsiasi posizione articolare iniziale alla zero position, quindi oscilla le articolazioni della caviglia del robot G1 in due modalità diverse e stampa i dati degli angoli di Eulero a una certa frequenza.
Il codice sorgente della routine si trova in unitree_sdk2/example/g1/low_level/g1_ankle_swing_example.cpp, accessibile da qui. Per le istruzioni di esecuzione, fare riferimento a Quick Development.
Per introdurre la routine, prima si deve conoscere le modalità di controllo delle articolazioni della caviglia del G1.
G1 Robot Ankle Joint Control Modes

L'articolazione della caviglia del robot G1 adotta un meccanismo parallelo e offre agli utenti due modalità di controllo:
PR Mode: Controlla i motori Pitch (P) e Roll (R) dell'articolazione della caviglia (modalità predefinita, corrispondente al modello URDF).
AB Mode: Controlla direttamente i motori A e B dell'articolazione della caviglia (richiede agli utenti di calcolare autonomamente la cinematica del meccanismo parallelo).
Poiché esistono due serie di relazioni di vincolo tra i movimenti delle articolazioni PR e AB della caviglia del robot G1, l'articolazione della caviglia ha effettivamente solo due gradi di libertà.
Nell'implementazione hardware, l'articolazione AB è sotto controllo attivo (guidata direttamente dai motori), mentre l'articolazione PR è in uno stato passivo.
Per ottenere la PR Mode, utilizziamo la regolazione indiretta dell'articolazione AB per controllare l'articolazione PR.
Le articolazioni Pitch e Roll della vita del robot G1 utilizzano anch'esse un meccanismo parallelo, fornendo due modalità di controllo: la PR mode e la AB mode.
Code Analysis
Il seguente codice è per il modello versione G1 29dof. Esempi per altre versioni sono disponibili nel repository unitree_sdk2.
Core Types and Functions
Nome Tipo | Descrizione |
DataBuffer | Classe strumento buffer dati multi-threaded |
ImuState | Struttura relativa all'IMU |
MotorCommand | Struttura relativa ai comandi del motore |
MotorState | Struttura relativa allo stato del motore |
Mode | Modalità di controllo dell'articolazione della caviglia G1 |
G1JointIndex | Indice di tutte le articolazioni ordinate per nome |
G1Example | Classe logica di controllo core |
Il programma principale della routine è definito in g1_ankle_swing_example.cpp, dove il programma principale istanzia la classe G1Example.
Nel costruttore di G1Example, vengono creati due thread e un callback:
Thread di invio comandi a basso livello che viene eseguito periodicamente, chiamando la funzione G1Example::LowCommandWriter a una frequenza predefinita di 500Hz.
Thread di logica di controllo definito dall'utente, il thread di controllo del motore, che chiama la funzione G1Example::Control a una frequenza predefinita di 500Hz.
Callback di ricezione dello stato a basso livello, che chiama la funzione G1Example::LowStateHandler.
Il codice specifico è mostrato di seguito:
G1Example(std::string networkInterface)
: time_(0.0),
control_dt_(0.002),
duration_(3.0),
mode_pr_(Mode::PR),
mode_machine_(0) {
ChannelFactory::Instance()->Init(0, networkInterface);
// create publisher
lowcmd_publisher_.reset(new ChannelPublisher<LowCmd_>(HG_CMD_TOPIC));
lowcmd_publisher_->InitChannel();
// create subscriber
lowstate_subscriber_.reset(new ChannelSubscriber<LowState_>(HG_STATE_TOPIC));
lowstate_subscriber_->InitChannel(std::bind(&G1Example::LowStateHandler, this, std::placeholders::_1), 1);
// create threads
command_writer_ptr_ = CreateRecurrentThreadEx("command_writer", UT_CPU_ID_NONE, 2000, &G1Example::LowCommandWriter, this);
control_thread_ptr_ = CreateRecurrentThreadEx("control", UT_CPU_ID_NONE, 2000, &G1Example::Control, this);
}
Funzione di Invio Comandi a Basso Livello
La funzione G1Example::LowCommandWriter viene utilizzata per inviare periodicamente comandi a basso livello.
void LowCommandWriter() {
LowCmd_ dds_low_command;
dds_low_command.mode_pr() = static_cast<uint8_t>(mode_pr_);
dds_low_command.mode_machine() = mode_machine_;
const std::shared_ptr<const MotorCommand> mc = motor_command_buffer_.GetData();
if (mc) {
for (size_t i = 0; i < G1_NUM_MOTOR; i++) {
dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable
dds_low_command.motor_cmd().at(i).tau() = mc->tau_ff.at(i);
dds_low_command.motor_cmd().at(i).q() = mc->q_target.at(i);
dds_low_command.motor_cmd().at(i).dq() = mc->dq_target.at(i);
dds_low_command.motor_cmd().at(i).kp() = mc->kp.at(i);
dds_low_command.motor_cmd().at(i).kd() = mc->kd.at(i);
}
dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command, (sizeof(dds_low_command) >> 2) - 1);
lowcmd_publisher_->Write(dds_low_command);
}
}
Funzione Callback di Ricezione Stato a Basso Livello
Quando viene ricevuto un messaggio di stato a basso livello dal topic rt/lowstate, il programma chiama automaticamente la funzione G1Example::LowStateHandler. La funzione callback estrae lo stato del motore e lo stato dell'IMU. La struttura dello stato a basso livello può essere consultata in Basic Services Interface.
void LowStateHandler(const void *message) {
LowState_ low_state = *(const LowState_ *)message;
if (low_state.crc() != Crc32Core((uint32_t *)&low_state, (sizeof(LowState_) >> 2) - 1)) {
std::cout << "[ERROR] CRC Error" << std::endl;
return;
}
// get motor state
MotorState ms_tmp;
for (int i = 0; i < G1_NUM_MOTOR; ++i) {
ms_tmp.q.at(i) = low_state.motor_state()[i].q();
ms_tmp.dq.at(i) = low_state.motor_state()[i].dq();
if (low_state.motor_state()[i].motorstate() && i <= RightAnkleRoll)
std::cout << "[ERROR] motor " << i << " with code " << low_state.motor_state()[i].motorstate() << "\n";
}
motor_state_buffer_.SetData(ms_tmp);
// get imu state
ImuState imu_tmp;
imu_tmp.omega = low_state.imu_state().gyroscope();
imu_tmp.rpy = low_state.imu_state().rpy();
imu_state_buffer_.SetData(imu_tmp);
// update mode machine
if (mode_machine_ != low_state.mode_machine()) {
if (mode_machine_ == 0) std::cout << "G1 type: " << unsigned(low_state.mode_machine()) << std::endl;
mode_machine_ = low_state.mode_machine();
}
}
Funzione di Logica di Controllo Definita dall'Utente
G1Example::Control implementa:
Stage 1: Resettare il robot G1 da qualsiasi posizione articolare iniziale alla posizione zero (i primi 3 secondi dopo l'avvio del programma);
Stage 2: Oscillare l'articolazione della caviglia in modalità PR per 3 secondi;
Stage 3: Oscillare continuamente l'articolazione della caviglia in modalità AB fino alla fine del programma.
void Control() {
ReportRPY();
MotorCommand motor_command_tmp;
const std::shared_ptr<const MotorState> ms = motor_state_buffer_.GetData();
for (int i = 0; i < G1_NUM_MOTOR; ++i) {
motor_command_tmp.tau_ff.at(i) = 0.0;
motor_command_tmp.q_target.at(i) = 0.0;
motor_command_tmp.dq_target.at(i) = 0.0;
motor_command_tmp.kp.at(i) = Kp[i];
motor_command_tmp.kd.at(i) = Kd[i];
}
if (ms) {
time_ += control_dt_;
if (time_ < duration_) {
// [Stage 1]: set robot to zero posture
for (int i = 0; i < G1_NUM_MOTOR; ++i) {
double ratio = std::clamp(time_ / duration_, 0.0, 1.0);
motor_command_tmp.q_target.at(i) = (1.0 - ratio) * ms->q.at(i);
}
} else if (time_ < duration_ * 2) {
// [Stage 2]: swing ankle using PR mode
mode_pr_ = Mode::PR;
double max_P = M_PI * 30.0 / 180.0;
double max_R = M_PI * 10.0 / 180.0;
double t = time_ - duration_;
double L_P_des = max_P * std::sin(2.0 * M_PI * t);
double L_R_des = max_R * std::sin(2.0 * M_PI * t);
double R_P_des = max_P * std::sin(2.0 * M_PI * t);
double R_R_des = -max_R * std::sin(2.0 * M_PI * t);
motor_command_tmp.q_target.at(LeftAnklePitch) = L_P_des;
motor_command_tmp.q_target.at(LeftAnkleRoll) = L_R_des;
motor_command_tmp.q_target.at(RightAnklePitch) = R_P_des;
motor_command_tmp.q_target.at(RightAnkleRoll) = R_R_des;
} else {
// [Stage 3]: swing ankle using AB mode
mode_pr_ = Mode::AB;
double max_A = M_PI * 30.0 / 180.0;
double max_B = M_PI * 10.0 / 180.0;
double t = time_ - duration_ * 2;
double L_A_des = +max_A * std::sin(M_PI * t);
double L_B_des = +max_B * std::sin(M_PI * t + M_PI);
double R_A_des = -max_A * std::sin(M_PI * t);
double R_B_des = -max_B * std::sin(M_PI * t + M_PI);
motor_command_tmp.q_target.at(LeftAnkleA) = L_A_des;
motor_command_tmp.q_target.at(LeftAnkleB) = L_B_des;
motor_command_tmp.q_target.at(RightAnkleA) = R_A_des;
motor_command_tmp.q_target.at(RightAnkleB) = R_B_des;
}
motor_command_buffer_.SetData(motor_command_tmp);
}
}
Funzione di Stampa dello Stato degli Angoli di Eulero
G1Example::ReportRPY viene utilizzata per uscire periodicamente i dati degli angoli di Eulero.
void ReportRPY() {
const std::shared_ptr<const ImuState> imu = imu_state_buffer_.GetData();
if (imu) std::cout << "rpy: [" << imu->rpy.at(0) << ", " << imu->rpy.at(1) << ", " << imu->rpy.at(2) << "]" << std::endl;
}
Dex3-1 Mano Dexterous
Introduzione alle Mani Dexterous
Il G1 può essere opzionalmente equipaggiato con la mano dexterous Dex3-1 a controllo di forza sviluppata internamente da Unitree. Questa mano dexterous ha 3 dita, con 2 gradi di libertà di flessione per ciascuna, e un ulteriore grado di libertà rotazionale per il pollice, per un totale di 7 gradi di libertà.

La mano dexterous può essere opzionalmente equipaggiata con array di sensori tattili, con un array sensoriale 3x4 in ciascuna posizione della punta delle dita (6 posizioni in totale).
Questo documento copre solo come controllare la mano dexterous del G1 tramite unitree_sdk2, con programmi demo allegati alla fine che mostrano esempi di ottenimento delle informazioni sullo stato della mano e delle funzioni di controllo.
Le mani dexterous sono distinte tra mano sinistra e destra. Ogni mano dexterous Dex3-1 è simmetrica sinistra-destra. Quando si controllano le mani dexterous, la comunicazione avviene attraverso diversi canali di messaggistica per controllare la mano corrispondente. Vedere la sezione sui metodi di controllo delle mani dexterous per i dettagli.
Puoi accedere a unitree_sdk2 qui, e vedere lo Sviluppo Rapido per le istruzioni operative. Il codice sorgente per gli esempi di questo capitolo si trova su GitHub.
Parametri Tecnici
Modello | Interfaccia di Controllo | Gradi di Libertà | Tensione Nominale |
Dex3-1 | RS485 | 7 | 24V |
Metodo di Controllo della Mano Dexterous
Il G1 fornisce internamente un programma di servizio residente che comunica con Dex3-1 e converte i messaggi in DDS. Questo servizio riceve ed elabora le richieste degli utenti.
Dopo aver ottenuto unitree_sdk2, il sistema caricherà la libreria della struttura dei messaggi. Gli utenti possono controllare la mano dexterous corrispondente utilizzando correttamente le strutture dei messaggi e comunicando con le interfacce corrispondenti.
I file header della mano dexterous si trovano in:
/usr/local/include/unitree/hg_idl/HandState_.hpp
/usr/local/include/unitree/hg_idl/HandCmd_.hpp
Gli utenti possono verificare le informazioni sulla struttura in questi file.
Ordinamento per Struttura dei Messaggi
Secondo l'ordinamento della struttura dei messaggi, le dita sono definite come pollice, indice e medio basandosi sulla percezione umana, e sono disposte in sequenza.
Le strutture dei messaggi unitree_hg::msg::dds_::HandCmd_.motor_cmd e unitree_hg::msg::dds_::HandState_.motor_state contengono informazioni per tutti i motori della mano dexterous, con il seguente ordine dei motori:
Indice Articolazione Mano in IDL | Nome Articolazione Mano |
0 | thumb_0 |
1 | thumb_1 |
2 | thumb_2 |
3 | index_0 |
4 | index_1 |
5 | middle_0 |
6 | middle_1 |
Ordinamento per URDF
Poiché la versione URDF della mano dexterous genera informazioni di posizione nello stesso sistema di coordinate, ci sono leggere differenze tra le mani sinistra e destra. L'URDF può essere ottenuto dalla piattaforma open-source.
Mano Sinistra

Indice Articolazione Mano in URDF | Indice Articolazione Mano in IDL | Nome Articolazione Mano |
left_hand_zero | 0 | thumb_0 |
left_hand_one | 1 | thumb_1 |
left_hand_two | 2 | thumb_2 |
left_hand_five | 3 | middle_0 |
left_hand_six | 4 | middle_1 |
left_hand_three | 5 | index_0 |
left_hand_four | 6 | index_1 |
Mano Destra

Indice Articolazione Mano in URDF | Indice Articolazione Mano in IDL | Nome Articolazione Mano |
right_hand_zero | 0 | thumb_0 |
right_hand_one | 1 | thumb_1 |
right_hand_two | 2 | thumb_2 |
right_hand_three | 3 | index_0 |
right_hand_four | 4 | index_1 |
right_hand_five | 5 | middle_0 |
right_hand_six | 6 | middle_1 |
Descrizione dell'Interfaccia
Gli utenti possono controllare la mano dexterous inviando messaggi unitree_hg::msg::dds_::HandCmd_ al topic "rt/dex3/(sinistra o destra)/cmd". Per ricevere lo stato della mano dexterous, iscriversi al topic "rt/dex3/(sinistra o destra)/state" per i messaggi unitree_hg::msg::dds_::HandState_.
Formato Dati IDL (interface definition language):
Di seguito sono riportate le strutture di controllo e stato utilizzate in questa comunicazione:
Struttura di Controllo HandCmd_:
struct HandCmd_ {
sequence<unitree_hg::msg::dds_::MotorCmd_> motor_cmd;
unsigned long reserve[4];
};//struct HandCmd_
Struttura MotorCmd_:
typedef struct { // Pacchetto dati di comando di controllo del motore 20 byte
uint8_t head[2]; // Header 0xFE 0xEE partecipa al CRC
RIS_Mode_t mode; // Modalità di controllo del motore 1 Byte
uint8_t res; // Riservato
int16_t tor_des; // Coppia di uscita desiderata dell'articolazione 256 rappresenta 1mNM
int16_t spd_des; // Velocità di uscita desiderata dell'articolazione 256/2π rappresenta 100rad/s
int32_t pos_des; // Posizione di uscita desiderata dell'articolazione 32768/2π rappresenta 1rad
int16_t k_pos; // Coefficiente di rigidità desiderato dell'articolazione 1280 rappresenta 1mN.m/rad
int16_t k_spd; // Coefficiente di smorzamento desiderato dell'articolazione 1280 rappresenta 1mN.m/100rad/s
uint32_t CRC32; // CRC32
} MotorCmd_t;
Struttura RIS_Mode_t:
typedef struct {
uint8_t id : 4; // ID del motore: 0,1...,13,14 15 rappresenta la trasmissione a tutti i motori
uint8_t status : 3; // Modalità di lavoro: 0.Lock 1.FOC 6
uint8_t timeout: 1; // Master->Motor: 0.Disabilita protezione timeout 1.Abilita (timeout predefinito di 1s)
// Motor->Master: 0.Nessun timeout 1.Protezione timeout attivata (necessita di impostare bit di controllo a 0 per resettare)
} RIS_Mode_t; // Modalità di controllo 1 Byte
Struttura di Stato HandState_:
struct HandState_ {
sequence<unitree_hg::msg::dds_::MotorState_> motor_state;
sequence<unitree_hg::msg::dds_::PressSensorState_> press_sensor_state;
unitree_hg::msg::dds_::IMUState_ imu_state;
float power_v;
float power_a;
float system_v;
float device_v;
unsigned long error[2];
unsigned long reserve[2];
};
La struttura PressSensorState_ sarà spiegata di seguito.
I campi nella struttura di stato rappresentano quanto segue:
power_v: Tensione totale di alimentazione in ingresso per la mano dexterous
power_a: Corrente totale di alimentazione in ingresso per la mano dexterous
system_v: Tensione di alimentazione interna della mano dexterous
device_v: Tensione di uscita del modulo di riduzione della tensione nella mano dexterous
error: Codice di errore emesso dalla mano dexterous. Per definizioni dettagliate dei codici di errore, fare riferimento alla sezione Motor Status Error Information nella documentazione G1 SDK
reserve: Campi riservati per usi futuri
Struttura PressSensorState:
struct PressSensorState {
unsigned short data[12]; // 12 valori dei sensori tattili
unsigned char id; // ID del dito
unsigned char temp; // Temperatura
unsigned short reserve[2]; // Riservato
};//struct PressSensorState


Valore di pressione per un singolo sensore:
L'ID nella figura rappresenta un pacchetto dati del sensore PressSensorState_. Le posizioni dei sensori corrispondenti agli indici dei dati pressure[12] sono segnate nella figura.
Valori dei dati dei sensori:
Valori validi: quando data >= 100000 (10w)
Valori non validi/Assenti: quando data = 30000 (3w)
Nota: È consigliato ridimensionare 100000 a 10.0000 per scopi di visualizzazione e calcolo.
Il campo temperatura rappresenta il valore della temperatura del sensore.
Esempi di Funzioni dell'Interfaccia di Controllo CPP:
Esempi di Rotazione dei Motori: rotateMotors Demo
void rotateMotors(bool isLeftHand) {
static int _count = 1; // Contatore per il movimento della mano
static int dir = 1; // Controllo della direzione della presa
const float* maxTorqueLimits = isLeftHand ? maxTorqueLimits_left : maxTorqueLimits_right;
const float* minTorqueLimits = isLeftHand ? minTorqueLimits_left : minTorqueLimits_right;
for (int i = 0; i < MOTOR_MAX; i++) {
RIS_Mode_t ris_mode;
ris_mode.id = i; // Imposta ID
ris_mode.status = 0x01; // Imposta stato a 0x01
ris_mode.timeout = 0x01; // Imposta timeout a 0x01
uint8_t mode = 0;
mode |= (ris_mode.id & 0x0F); // Ottieni i 4 bit inferiori dell'ID
mode |= (ris_mode.status & 0x07) << 4; // Ottieni i 3 bit superiori dello stato e sposta a sinistra di 4 bit
mode |= (ris_mode.timeout & 0x01) << 7; // Ottieni il bit superiore del timeout e sposta a sinistra di 7 bit
msg.motor_cmd()[i].mode(mode);
msg.motor_cmd()[i].tau(0);
msg.motor_cmd()[i].kp(0.5); // Imposta il guadagno di controllo kp
msg.motor_cmd()[i].kd(0.1); // Imposta il guadagno di controllo kd
// Calcola la posizione target q
float range = maxTorqueLimits[i] - minTorqueLimits[i]; // Limita l'intervallo
float mid = (maxTorqueLimits[i] + minTorqueLimits[i]) / 2.0; // Valore medio
float amplitude = range / 2.0; // Ampiezza
// Usa _count per regolare dinamicamente il valore di q
float q = mid + amplitude * sin(_count / 20000.0 * M_PI); // Genera un'onda sinusoidale variabile nel tempo
// if(i == 0)std::cout << q << std::endl;
msg.motor_cmd()[i].q(q); // Imposta la posizione target q
}
handcmd_publisher->Write(msg);
_count += dir;
// Controlla la direzione della presa
if (_count >= 10000) {
dir = -1;
}
if (_count <= -10000) {
dir = 1;
}
usleep(100); // Frequenza del ciclo di controllo, evita di inviare comandi troppo rapidamente
}
Esempio di Presa della Mano: gripHand Demo
void gripHand(bool isLeftHand) {
// Definisci i limiti per la mano sinistra e destra
const float* maxTorqueLimits = isLeftHand ? maxTorqueLimits_left : maxTorqueLimits_right;
const float* minTorqueLimits = isLeftHand ? minTorqueLimits_left : minTorqueLimits_right;
for (int i = 0; i < MOTOR_MAX; i++) {
RIS_Mode_t ris_mode;
ris_mode.id = i; // Imposta ID
ris_mode.status = 0x01; // Imposta stato a 0x01
ris_mode.timeout = 0; // Imposta timeout a 0x00
uint8_t mode = 0;
mode |= (ris_mode.id & 0x0F); // Ottieni i 4 bit inferiori dell'ID
mode |= (ris_mode.status & 0x07) << 4; // Ottieni i 3 bit superiori dello stato e sposta a sinistra di 4 bit
mode |= (ris_mode.timeout & 0x01) << 7; // Ottieni il bit superiore del timeout e sposta a sinistra di 7 bit
msg.motor_cmd()[i].mode(mode);
msg.motor_cmd()[i].tau(0);
// Calcola la posizione media
float mid = (maxTorqueLimits[i] + minTorqueLimits[i]) / 2.0; // Valore medio
// Imposta i comandi del motore
msg.motor_cmd()[i].q(mid); // Imposta la posizione target al valore medio
msg.motor_cmd()[i].dq(0); // Imposta la velocità a 0
msg.motor_cmd()[i].kp(1.5); // Imposta il guadagno di controllo kp
msg.motor_cmd()[i].kd(0.1); // Imposta il guadagno di controllo kd
}
msg.reserve()[0] = 0;
// Pubblica il comando
handcmd_publisher->Write(msg);
usleep(1000000); // Invia il comando una volta al secondo
}
Esempio di Arresto dei Motori: stopMotors Demo
void stopMotors() {
for (int i = 0; i < MOTOR_MAX; i++) {
RIS_Mode_t ris_mode;
ris_mode.id = i; // Imposta ID
ris_mode.status = 0x01; // Imposta stato a 0x06
ris_mode.timeout = 0x01; // Imposta timeout a 0x01
uint8_t mode = 0;
mode |= (ris_mode.id & 0x0F); // Ottieni i 4 bit inferiori dell'ID
mode |= (ris_mode.status & 0x07) << 4; // Ottieni i 3 bit superiori dello stato e sposta a sinistra di 4 bit
mode |= (ris_mode.timeout & 0x01) << 7; // Ottieni il bit superiore del timeout e sposta a sinistra di 7 bit
msg.motor_cmd()[i].mode(mode);
msg.motor_cmd()[i].tau(0);
msg.motor_cmd()[i].dq(0);
msg.motor_cmd()[i].kp(0);
msg.motor_cmd()[i].kd(0);
msg.motor_cmd()[i].q(0); // Arresta tutti i motori
}
handcmd_publisher->Write(msg);
usleep(1000000); // Invia il comando di arresto una volta al secondo
}
Esempio di Stampa dello Stato dei Motori: printState Demo
void printState(bool isLeftHand){
Eigen::Matrix<float, 7, 1> q;
// Definisci i limiti per la mano sinistra e destra
const float* maxTorqueLimits = isLeftHand ? maxTorqueLimits_left : maxTorqueLimits_right;
const float* minTorqueLimits = isLeftHand ? minTorqueLimits_left : minTorqueLimits_right;
for(int i = 0; i < 7; i++)
{
q(i) = state.motor_state()[i].q();
// Normalizza q a [0, 1]
q(i) = (q(i) - minTorqueLimits[i] ) / (maxTorqueLimits[i] - minTorqueLimits[i]);
q(i) = std::clamp(q(i), 0.0f, 1.0f);
}
if(isLeftHand){
std::cout << " L: " << q.transpose() << std::endl;
}else std::cout << " R: " << q.transpose() << std::endl;
usleep(0.1 * 1e6);
}
RL Reinforcement Learning
Vediamo un semplice esempio di apprendimento RL per trainare Unitree G1. Di seguito viene descritto come utilizzare la piattaforma di simulazione isaac_gym per addestrare l'algoritmo di controllo G1.
Preparazione dell'hardware
Poiché la piattaforma di simulazione isaac_gym richiede CUDA, questo articolo raccomanda che l'hardware sia configurato con una scheda grafica NVIDIA (memoria video >8GB, scheda grafica della serie RTX) e il driver della scheda grafica corrispondente installato. Si consiglia che il sistema utilizzi Ubuntu 18/20, versione del driver della scheda grafica 525.

Configurazione dell'ambiente
Si raccomanda di configurare questo ambiente in un ambiente virtuale con conda.
Crea un ambiente virtuale
conda create -n rl-g1 python=3.8
Attiva l'ambiente virtuale
conda activate rl-g1
Installa CUDA e PyTorch
pip3 install torch==1.10.0+cu113 torchvision==0.11.1+cu113 torchaudio==0.10.0+cu113 -f https://download.pytorch.org/whl/cu113/torch_stable.html
Nota che la versione della libreria numpy non dovrebbe essere troppo alta. Si raccomanda di installare la versione 1.23.5.
Scarica la piattaforma di simulazione Isaac Gym Preview 4, decomprimi e entra nella directory python, quindi usa pip per installarla.
# directory corrente: isaacgym/python
pip install -e .
Esegui le routine nella directory python/examples per verificare se l'installazione è riuscita.
# directory corrente: isaacgym/python/examples
python 1080_balls_of_solitude.py
Se l'installazione ha successo, vedrai la seguente finestra.

Installa la libreria rsl_rl (usa v1.0.2)
git clone https://github.com/leggedrobotics/rsl_rl
cd rsl_rl
git checkout v1.0.2
pip install -e .
Uso dell'addestramento del modello
Scarica il codice di esempio ufficiale di Unitree:
git clone https://github.com/unitreerobotics/unitree_rl_gym.git
Modifica legged_gym/scripts/train.py e sys.path.append("/home/unitree/h1/legged_gym") in legged_gym/scripts/play.py per impostare il tuo percorso.
Attiva l'ambiente virtuale di apprendimento per rinforzo
conda activate rl-g1
Passa alla directory legged_gym/scripts, esegui le istruzioni di addestramento e avvia l'addestramento.
python3 train.py --task=g1
Modifica il parametro args.headless nel file train.py per attivare o disattivare l'interfaccia visiva.
isaac_gym
Quando appare la seguente interfaccia, inizia l'addestramento.

La finestra di output del terminale è la seguente:

Dopo 1500 sessioni di addestramento, esegui le istruzioni di test.
python play.py --task=g1
Routine di Comunicazione ROS2
Introduzione
unitree_sdk2 implementa un meccanismo di comunicazione robotica facile da usare basato su CycloneDDS, che consente agli sviluppatori di realizzare comunicazioni e controlli robotici (supporta Unitree Go2, B2, H1 e G1).
DDS è utilizzato anche in ROS2 come meccanismo di comunicazione. Pertanto, gli strati sottostanti dei robot Unitree H1, B2 e G1 possono essere compatibili con ROS2. I messaggi ROS2 possono essere utilizzati direttamente per la comunicazione e il controllo del robot Unitree senza avvolgere l'interfaccia SDK.
Configurazione
Requisiti di Sistema
Sistemi Operativi | Distribuzione ROS2 |
Ubuntu 20.04 | Foxy |
Ubuntu 22.04 | Humble |
Prendendo come esempio ROS2 Foxy, se hai bisogno di un'altra versione di ROS2, sostituisci "foxy" con il nome della versione ROS2 corrente nel punto corrispondente.
Nota: Potrebbero esserci differenze nelle API delle diverse versioni di ROS2, come rosbag. Gli esempi nel repository sono sviluppati sotto ROS2 Foxy. Se utilizzi altre distribuzioni ROS2, fai riferimento alla documentazione ufficiale per le regolazioni necessarie.
L'installazione di ROS2 Foxy può essere consultata qui: Installazione ROS2 Foxy su Ubuntu
Clonazione del Repository: Apri il terminale con Ctrl+Alt+T e clona il repository:
git clone https://github.com/unitreerobotics/unitree_ros2
Struttura del Workspace:
cyclonedds_ws: Workspace del pacchetto ROS2 Unitree. I messaggi per il robot Unitree sono forniti nelle sottocartelle cyclonedds_ws/unitree/unitree_go e cyclonedds_ws/unitree/unitree_api.
Installazione del Pacchetto ROS2 Unitree
Dipendenze:
sudo apt install ros-foxy-rmw-cyclonedds-cpp sudo apt install ros-foxy-rosidl-generator-dds-idl
Per comodità nell'uso dell'interfaccia, si raccomanda di installare unitree_sdk2: Unitree SDK2 su GitHub
Compilazione di CycloneDDS:
La versione CycloneDDS per i robot Unitree è la 0.10.2. Per comunicare con i robot Unitree utilizzando ROS2, è necessario cambiare l'implementazione DDS. Vedi: Middleware in ROS2
Prima di compilare CycloneDDS: Assicurati che l'ambiente ROS2 NON sia stato "sourced" all'avvio del terminale per evitare errori di compilazione. Se source /opt/ros/foxy/setup.bash è stato aggiunto al file ~/.bashrc, commentalo:
sudo apt install gedit sudo gedit ~/.bashrc # source /opt/ros/foxy/setup.bash
Compilazione di CycloneDDS:
cd ~/unitree_ros2/cyclonedds_ws/src git clone https://github.com/ros2/rmw_cyclonedds -b foxy git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x cd .. colcon build --packages-select cyclonedds # Compila il pacchetto cyclonedds
Compilazione dei Pacchetti unitree_go e unitree_api:
Dopo aver compilato CycloneDDS, sono necessarie le dipendenze ROS2 per la compilazione dei pacchetti unitree_go e unitree_api. Pertanto, prima di compilare, è necessario "source" l'ambiente ROS2.
source /opt/ros/foxy/setup.bash # Sorgente dell'ambiente ROS2 colcon build # Compila tutti i pacchetti nel workspace
Connessione al Robot Unitree
Configurazione di Rete:
Collega il robot Unitree e il computer tramite cavo Ethernet. Usa ifconfig per visualizzare l'interfaccia di rete a cui è collegato il robot, ad esempio "enp3s0".
Impostazioni IPv4:
Modalità IPv4: Manuale
Indirizzo: 192.168.123.99
Maschera: 255.255.255.0
Gateway: 192.168.123.1
Dopo aver applicato le impostazioni, attendi il reinserimento della rete.
Configurazione dello Script di Setup:
Apri il file unitree_ros2_setup.sh:
sudo gedit ~/unitree_ros2/unitree_ros2_setup.sh
Contenuto dello Script:
#!/bin/bash echo "Setup unitree ros2 environment" source /opt/ros/foxy/setup.bash source $HOME/unitree_ros2/cyclonedds_ws/install/setup.bash export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces> <NetworkInterface name="enp3s0" priority="default" multicast="default" /> </Interfaces></General></Domain></CycloneDDS>'
Sostituisci "enp3s0" con il nome dell'interfaccia di rete del robot Unitree collegato.
Sorgente dell'Ambiente:
source ~/unitree_ros2/unitree_ros2_setup.sh
Nota: Se non vuoi eseguire lo script ogni volta che apri un nuovo terminale, puoi aggiungere il contenuto dello script al file ~/.bashrc, ma fai attenzione se ci sono più ambienti ROS che coesistono nel sistema.
Connessione e Test:
Dopo aver completato la configurazione, è consigliato riavviare il computer.
Verifica della Connessione:
Apri un terminale e inserisci:
source ~/unitree_ros2/unitree_ros2_setup.sh ros2 topic list
Dovresti vedere i seguenti topic:
(Inserisci l'immagine pertinente)
Esempio di Echo di un Topic:
ros2 topic echo /sportmodestate
Vedrai i dati del topic come mostrato nella figura seguente:
(Inserisci l'immagine pertinente)
Routine della Telecamera di Profondità
1. Introduzione alla Telecamera di Profondità del Robot G1 e Acquisizione delle Informazioni Ufficiali
La telecamera di profondità del robot G1 è situata sopraelevata ed è una RealSense D435i. La telecamera include:
Telecamera infrarossa binocular (shutter globale)
Trasmettitore laser
Telecamera RGB (shutter rotante)
IMU a 6 assi
Pagina Prodotto: Intel RealSense D435Documentazione Dettagliata: Guide di Inizio Rapido GitHub Ufficiale: IntelRealSense su GitHub
Sistema di Coordinate: Relazione tra il sistema di coordinate della telecamera di profondità, il sistema di coordinate della telecamera e il sistema di coordinate centrale del robot:


2. Acquisizione dei Dati

2.1 Visualizzazione dei Dati tramite il Computer Host
Su Windows, scarica Intel RealSense Viewer.exe dal seguente link: Liberalsense Releases
Su Linux, è necessario compilare l'SDK seguendo le istruzioni:
Ubuntu 18/20/22 LTS: Installazione su Ubuntu
Sistema NVIDIA Jetson: Installazione su Jetson
Sistema Raspberry Pi: Installazione su Raspberry Pi
Abbiamo installato librealsense sulla piattaforma computer NX del robot e può essere utilizzata direttamente. Quando la connessione hardware è normale, apri realsense-viewer nel terminale per aprire il software e visualizzare:
(Inserire screenshot o descrizione dettagliata se disponibile)
2.2 Acquisizione dei Dati tramite ROS
Repository del Driver ROS per la Telecamera: livox_ros_driver2 su GitHub
Segui le istruzioni nel file README del repository per il codice di riferimento.
Per il LiDAR del robot G1:
Modifiche necessarie:
L'indirizzo IP di lidar_configs nel file ../livox_ros_driver2/config/MID360_config.json è 192.168.123.120
L'indirizzo IP di host_net_info è l'indirizzo IP dell'host.
File di Configurazione Esempio:
{
"lidar_summary_info" : {
"lidar_type": 8
},
"MID360": {
"lidar_net_info" : {
"cmd_data_port": 56100,
"push_msg_port": 56200,
"point_data_port": 56300,
"imu_data_port": 56400,
"log_data_port": 56500
},
"host_net_info" : {
"cmd_data_ip" : "192.168.123.123",
"cmd_data_port": 56101,
"push_msg_ip": "192.168.123.123",
"push_msg_port": 56201,
"point_data_ip": "192.168.123.123",
"point_data_port": 56301,
"imu_data_ip" : "192.168.123.123",
"imu_data_port": 56401,
"log_data_ip" : "",
"log_data_port": 56501
}
},
"lidar_configs" : [
{
"ip" : "192.168.123.120",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}
Modifica del File di Lancio:
Cambia il parametro user_config_path nel file di lancio per puntare al percorso del file di configurazione MID360_config.json.
Esempio di File di Lancio (launch_ROS1):
<launch>
<!-- Parametri di configurazione utente per ROS -->
<arg name="lvx_file_path" default="livox_test.lvx"/>
<arg name="bd_list" default="100000000000000"/>
<arg name="xfer_format" default="1"/>
<arg name="multi_topic" default="0"/>
<arg name="data_src" default="0"/>
<arg name="publish_freq" default="10.0"/>
<arg name="output_type" default="0"/>
<arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<arg name="lidar_bag" default="false"/>
<arg name="imu_bag" default="false"/>
<!-- Fine Parametri di configurazione utente per ROS -->
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
<param name="data_src" value="$(arg data_src)"/>
<param name="publish_freq" type="double" value="$(arg publish_freq)"/>
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver2)/config/MID360_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
<param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>
<node name="livox_lidar_publisher2" pkg="livox_ros_driver2"
type="livox_ros_driver2_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>
<group if="$(arg rosbag_enable)">
<node pkg="rosbag" type="record" name="record" output="screen"
args="-a"/>
</group>
</launch>
Avvio dei Topic:
Se la connessione hardware è normale, puoi iscriverti ai topic /livox/lidar e /livox/imu dopo aver eseguito:
ros2 launch livox_ros_driver2 msg_MID360.launch
Nota: Il formato del topic /livox/lidar è controllato dal parametro xfer_format del file di lancio. Consulta la descrizione del file README del codice per ulteriori dettagli.
2.3 Acquisizione dei Dati tramite SDK
Repository del Codice SDK della Telecamera: librealsense su GitHub
Abbiamo installato librealsense sulla piattaforma computer NX del robot e può essere utilizzata direttamente.
Uso dell'SDK: Consulta il contenuto del codice nella cartella /librealsense-2.54.1/examples per ulteriori dettagli.
Demo di Riferimento:
/**
* @file test.cpp
* get_camera_imu_stream_test
*
*/
#include <librealsense2/rs.hpp>
#include <librealsense2/hpp/rs_internal.hpp>
#include <unistd.h>
#include <iostream>
#include <mutex>
#include <thread>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
std::mutex mutex;
rs2::frameset fs_frame = {};
double fs_timestamp = 0.0;
bool fs_flag = false;
// Funzioni di callback e gestione dei frame...
int main()
{
rs2::device selected_device;
selected_device = get_a_realsense_device();
std::vector<rs2::sensor> sensors = selected_device.query_sensors();
// Stampa e impostazione dei parametri dei sensori...
// Istanziazione di pipeline e configurazione...
rs2::pipeline pipe;
rs2::config cfg;
// Abilitazione dei flussi...
cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 60);
cfg.enable_stream(RS2_STREAM_INFRARED, 1, 640, 480, RS2_FORMAT_Y8, 60);
cfg.enable_stream(RS2_STREAM_INFRARED, 2, 640, 480, RS2_FORMAT_Y8, 60);
cfg.enable_stream(RS2_STREAM_COLOR, 848, 480, RS2_FORMAT_BGR8, 60);
cfg.enable_stream(RS2_STREAM_GYRO , RS2_FORMAT_MOTION_XYZ32F, 400);
cfg.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F, 200);
rs2::pipeline_profile pipe_profile = pipe.start(cfg, stream_callback);
// Ottenimento delle intrinseche della telecamera di profondità...
float mCamera_fx, mCamera_fy, mCamera_cx, mCamera_cy;
l_get_intrinsics(pipe_profile.get_stream(RS2_STREAM_DEPTH), mCamera_fx, mCamera_fy, mCamera_cx, mCamera_cy);
// Post-processing...
rs2::temporal_filter temp_filter;
temp_filter.set_option(rs2_option::RS2_OPTION_HOLES_FILL, 6);
temp_filter.set_option(rs2_option::RS2_OPTION_FILTER_SMOOTH_ALPHA, 0.4f);
temp_filter.set_option(rs2_option::RS2_OPTION_FILTER_SMOOTH_DELTA, 20);
double _timestamp_last = 0.0;
while (true){
rs2::frameset _frame = {};
double _timestamp = 0.0;
bool _flag = false;
std::unique_lock<std::mutex> lock(mutex);
_frame = fs_frame;
_timestamp = fs_timestamp;
_flag = fs_flag;
fs_flag = false;
lock.unlock();
if(_flag){
printf("_frame fps: %.4f \n", 1/(_timestamp - _timestamp_last));
// Ottenimento e visualizzazione dei frame...
rs2::depth_frame depth_frame = _frame.get_depth_frame();
rs2::video_frame color_frame = _frame.get_color_frame();
rs2::frame irL_frame = _frame.get_infrared_frame(1);
rs2::frame irR_frame = _frame.get_infrared_frame(2);
rs2::depth_frame depth_frame_filtered = temp_filter.process(depth_frame);
// Conversione dei dati delle immagini in formato OpenCV...
cv::Mat depth_image(cv::Size(640, 480), CV_16U , (void*)depth_frame.get_data(), cv::Mat::AUTO_STEP); // Depth
cv::Mat color_image(cv::Size(848, 480), CV_8UC3, (void*)color_frame.get_data(), cv::Mat::AUTO_STEP); // RGB
cv::Mat irL_image (cv::Size(640, 480), CV_8UC1, (void*)irL_frame.get_data() , cv::Mat::AUTO_STEP); // IR_left
cv::Mat irR_image (cv::Size(640, 480), CV_8UC1, (void*)irR_frame.get_data() , cv::Mat::AUTO_STEP); // IR_right
cv::Mat p_depth_image(cv::Size(640, 480), CV_16U , (void*)depth_frame.get_data(), cv::Mat::AUTO_STEP); // post processed Depth
// Visualizzazione delle immagini...
if (color_image.empty() || depth_image.empty() || irL_image.empty() || irR_image.empty()) {
std::cerr << "Una o più immagini sono vuote." << std::endl;
continue;
}
cv::imshow("depth", depth_image);
cv::imshow("color", color_image);
cv::imshow("irL", irL_image);
cv::imshow("irR", irR_image);
cv::imshow("p_depth", depth_image);
cv::waitKey(1);
_timestamp_last = _timestamp;
}else{
usleep(2000);
}
}
return 0;
}
CMakeLists.txt Esempio:
cmake_minimum_required(VERSION 3.1.0)
project(get_camera_imu_stream_test)
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -w")
set(CMAKE_C_FLAGS_RELEASE "-O3 -w")
# Trova il pacchetto librealsense2 installato
find_package(realsense2 REQUIRED)
find_package(OpenCV REQUIRED)
# Abilita C++11
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED TRUE)
# Aggiungi le sorgenti dell'applicazione al target
add_executable(${PROJECT_NAME} test.cpp)
# Collega librealsense2 al target
target_link_libraries(${PROJECT_NAME} ${realsense2_LIBRARY} ${OpenCV_LIBS})
2.4 Riguardo ai Problemi di Connessione USB 2.0 e USB 3.0 delle Telecamere di Profondità Realsense
Connessione a USB 3.0: Quando la telecamera di profondità è collegata a USB 3.0, il software realsense viewer può visualizzare le risoluzioni e i frame rate supportati.
Connessione a USB 2.0: È necessario utilizzare un adattatore hub per ottenere i dati tramite l'SDK normalmente. Può anche essere utilizzato direttamente sul processore NX del G1. Sotto connessione USB 2.0, il frame rate e la risoluzione ottenibili possono differire rispetto a quelli sotto connessione USB 3.0. Il software realsense viewer può visualizzare le risoluzioni e i frame rate supportati.


Per ulteriori informazioni pianifica una sessione informativa con i nostri tecnici
Comentarios