Spot stand up.py

import argparse

import socket
import sys
import time
import os
import bosdyn.client
import bosdyn.client.lease
import bosdyn.client.util
import bosdyn.geometry

from bosdyn.client import math_helpers
from bosdyn.client import frame_helpers
from bosdyn.client.image import ImageClient
from bosdyn.client.robot_state import RobotStateClient
from bosdyn.client.robot_command import RobotCommandBuilder, RobotCommandClient, blocking_stand
from bosdyn.client.frame_helpers import ODOM_FRAME_NAME, VISION_FRAME_NAME, get_odom_tform_body, get_vision_tform_body


HOSTNAME = '192.168.50.3'
USERNAME = 'student'
PASSWORD = 'xmiBhMWmxyVIDS9b'


if __name__ == '__main__':
    hostname = socket.gethostname()
    print('Starting at host:', hostname)
    sdk = bosdyn.client.create_standard_sdk('HelloSpotClient')
    robot = sdk.create_robot(HOSTNAME)
    robot.authenticate(USERNAME, PASSWORD)
    robot.time_sync.wait_for_sync()
    print('Authenticated')
    assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \
                                    "such as the estop SDK example, to configure E-Stop."
    lease_client = robot.ensure_client(bosdyn.client.lease.LeaseClient.default_service_name)
    lease = lease_client.acquire()
    with bosdyn.client.lease.LeaseKeepAlive(lease_client):
        print('Powering on')
        robot.power_on(timeout_sec=20)
        assert robot.is_powered_on(), "Robot power on failed."

        command_client = robot.ensure_client(RobotCommandClient.default_service_name)
        robot_state_client = robot.ensure_client(RobotStateClient.default_service_name)

        print('1. Stand')
        footprint_R_body = bosdyn.geometry.EulerZXY(yaw=0.0, roll=0.0, pitch=0.0)
        print('Trying to stand up')
        cmd = RobotCommandBuilder.synchro_stand_command(footprint_R_body=footprint_R_body)
        command_client.robot_command(cmd)
        time.sleep(5)