bot.h

Go to the documentation of this file.

#pragma once
#include "unitree_legged_sdk/unitree_legged_sdk.h"
#include <Eigen/Dense>
#include <functional>
#include <cmath>
#include "utils.h"

namespace a1 = UNITREE_LEGGED_SDK;
constexpr double pi = M_PI;

struct InstructionOutput {    
    a1::HighCmd cmd; 
    bool done;
};

using Instruction = std::function<InstructionOutput(Pose, a1::HighState)>;

class Bot
{
    a1::Safety safe; 
    a1::UDP udp; 
    a1::HighCmd cmd = {0}; 
    a1::HighState state = {0}; 
    Pose initial_pose; 
    int motiontime = 0; 
    float dt = 0.002; 
    int index = 0; 
    bool executing = false; 

    std::vector<Instruction> instructions;
    
    void UDPRecv();

    void UDPSend();

    void RobotControl();

    Pose estimate_pose(Pose prev, a1::HighState state);
    bool validate_cmd(a1::HighCmd cmd);    

public:
    Bot(): safe(a1::LeggedType::A1), udp(a1::HIGHLEVEL){
        udp.InitCmdData(cmd);
    }

    void execute();

    State get_state();
    
    void move_x(float distance, float velocity);

    void move_y(float distance, float velocity);

    void move(Eigen::Vector2f position, Eigen::Vector2f velocity);

    void smooth_move(Eigen::Vector2f position, float velocity, float omega = 1*pi/3);
    
    void spline_move(); 
    
    void rotate(float theta, float omega);

    [[deprecated("unsafe")]] void set_led(std::vector<Eigen::Vector3i> lights);

    [[deprecated]] void set_pyr(Eigen::Vector3d pitch, Eigen::Vector3d yaw, Eigen::Vector3d roll);
    [[deprecated]] void set_height(float h, float v);
};