GO1

README.org

• https://github.com/unitreerobotics/unitree_guide
• https://github.com/unitreerobotics/unitree_ros
• http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28python%29
• use `yyvalue’ to control GO1’s velocity, it’s message type is geometry_msgs/Point, and has x, y, z for x-axis speed, y-axis speed and rotation speed.
• use `yycmd’ to control GO1’s state, it can be 1 (free state), 2 (fixed stand), 4 (moving) and 0 (stop moving)

https://ww2.mathworks.cn/help/ros/ug/get-started-with-ros.html

https://jp.mathworks.com/help/ros/ref/send.html

CMakeLists.txt

cmake_minimum_required(VERSION 3.14)
project(unitree_guide)

set(ROBOT_TYPE Go1)         # The type of robot, support Go1 and A1 currently
set(PLATFORM amd64)         # The platform to compile, support amd64 and arm64

set(CATKIN_MAKE ON)        # Use CATKIN_MAKE or not, ON or OFF
set(SIMULATION ON)         # Use Gazebo or not, ON or OFF
set(REAL_ROBOT OFF)          # Link real robot or not, ON or OFF
set(DEBUG OFF)              # Use debug functions or not, ON or OFF
set(MOVE_BASE OFF)               # Need move_base or not, ON or OFF

if(NOT DEFINED ROBOT_TYPE)
    message(FATAL_ERROR "[CMake ERROR] Have not defined ROBOT_TYPE")
endif()
if(NOT DEFINED PLATFORM)
    message(FATAL_ERROR "[CMake ERROR] Have not defined PLATFORM")
endif()

if(${ROBOT_TYPE} STREQUAL "A1")
    add_definitions(-DROBOT_TYPE_A1)
elseif(${ROBOT_TYPE} STREQUAL "Go1")
    add_definitions(-DROBOT_TYPE_Go1)
else()
    message(FATAL_ERROR "[CMake ERROR] The ROBOT_TYPE is error")
endif()

if(((SIMULATION) AND (REAL_ROBOT)) OR ((NOT SIMULATION) AND (NOT REAL_ROBOT)))
    message(FATAL_ERROR "[CMake ERROR] The SIMULATION and REAL_ROBOT can only be one ON one OFF")
endif()

if(SIMULATION OR MOVE_BASE)
    add_definitions(-DRUN_ROS)
    set(CATKIN_MAKE ON)
endif()

set(CMAKE_CXX_STANDARD 11)
find_package(Boost)

if(CATKIN_MAKE)
    add_definitions(-DCOMPILE_WITH_ROS)
    if(MOVE_BASE)
        add_definitions(-DCOMPILE_WITH_MOVE_BASE)
    endif()
endif()

if(DEBUG)
    add_definitions(-DCOMPILE_DEBUG)
    find_package(Python2 COMPONENTS Interpreter Development NumPy)
endif()

if(CATKIN_MAKE)
    if(SIMULATION)
        add_definitions(-DCOMPILE_WITH_SIMULATION)
        find_package(catkin REQUIRED COMPONENTS
            controller_manager
            joint_state_controller
            gazebo_ros
            # gazebo
        )
    endif()

    find_package(catkin REQUIRED COMPONENTS
        genmsg
        robot_state_publisher
        roscpp
        std_msgs
        tf
        geometry_msgs
        unitree_legged_msgs
    )
    catkin_package(
        CATKIN_DEPENDS
        unitree_legged_msgs
    )
endif()

include_directories(
    include
    ${Boost_INCLUDE_DIRS}
    ${catkin_INCLUDE_DIRS}
    ${GAZEBO_INCLUDE_DIRS}
)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS} -O3")

if(REAL_ROBOT)
    add_definitions(-DCOMPILE_WITH_REAL_ROBOT)
    if(${ROBOT_TYPE} STREQUAL "A1")
        include_directories(
            library/unitree_legged_sdk_3.2/include
        )
        link_directories(
            library/unitree_legged_sdk_3.2/lib
        )
    elseif(${ROBOT_TYPE} STREQUAL "Go1")
        include_directories(
            library/unitree_legged_sdk-3.8.0/include
        )
        if(${PLATFORM} STREQUAL "amd64")
            link_directories(
                library/unitree_legged_sdk-3.8.0/lib/cpp/amd64
            )
        elseif(${PLATFORM} STREQUAL "arm64")
            link_directories(
                library/unitree_legged_sdk-3.8.0/lib/cpp/arm64
            )
        endif()
    endif()
endif()

link_directories(
    ${GAZEBO_LIBRARY_DIRS}
)

# aux_source_directory(src SRC_LIST)
file(GLOB_RECURSE SRC_LIST
    "src/*/*.cpp"
    "src/*/*.cc"
)
# add here @1
add_executable(yy_ctrl src/yy.cpp ${SRC_LIST})

add_executable(junior_ctrl src/main.cpp ${SRC_LIST})
if(CATKIN_MAKE)
  target_link_libraries(junior_ctrl ${catkin_LIBRARIES})
  # add here @2
  target_link_libraries(yy_ctrl ${catkin_LIBRARIES})
  add_dependencies(junior_ctrl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
  # add here @3
  add_dependencies(yy_ctrl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()
if(DEBUG)
    target_include_directories(junior_ctrl PRIVATE ${Python2_INCLUDE_DIRS} ${Python2_NumPy_INCLUDE_DIRS})
    target_link_libraries(junior_ctrl Python2::Python Python2::NumPy)
endif()
if(REAL_ROBOT)
    if(${ROBOT_TYPE} STREQUAL "A1")
        if(${PLATFORM} STREQUAL "amd64")
            target_link_libraries(junior_ctrl libunitree_legged_sdk_amd64.so)
        elseif(${PLATFORM} STREQUAL "arm64")
            target_link_libraries(junior_ctrl libunitree_legged_sdk_arm64.so)
        endif()
    elseif(${ROBOT_TYPE} STREQUAL "Go1")
            target_link_libraries(junior_ctrl libunitree_legged_sdk.a)
    endif()
endif()
target_link_libraries(junior_ctrl -pthread lcm)

if(NOT CATKIN_MAKE)
    set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
endif()

yy.cpp

/**********************************************************************
 Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved.
***********************************************************************/
#include <iostream>
#include <unistd.h>
#include <csignal>
#include <sched.h>

#include "control/ControlFrame.h"
#include "control/CtrlComponents.h"

#include "Gait/WaveGenerator.h"

#include "interface/KeyBoard.h"
#include "interface/IOROS.h"

#include <std_msgs/Int32.h>
#include <geometry_msgs/Point.h>

// new class inherited from IOROS

class YYROS : public IOROS {
public:
	YYROS(CmdPanel *myCmdPanel);
	~YYROS();
};
// use another control pannel instead of Keyboard
YYROS::YYROS(CmdPanel *myCmdPanel):IOROS::IOROS() {
	delete cmdPanel;
	cmdPanel = myCmdPanel;
}
// do nothing
YYROS::~YYROS() {}

class YYPanel : public CmdPanel {
public:
	YYPanel();
	~YYPanel();
private:
	void* run (void *arg);
	static void* runyy(void *arg);
	pthread_t _tid;
	void checkCmdCallback(const std_msgs::Int32 i);
	void changeValueCallback(const geometry_msgs::Point p);

	// ros specified variable
	// state change listener;
	ros::Subscriber yycmd;
	// velocity change listener;
	ros::Subscriber yyvalue;
};

YYPanel::YYPanel() {
	userCmd = UserCommand::NONE;
	userValue.setZero();
	ros::NodeHandle n;
	// register message callback functions
	yycmd = n.subscribe("yycmd", 1, &YYPanel::checkCmdCallback, this);
	yyvalue = n.subscribe("yyvalue", 1, &YYPanel::changeValueCallback, this);
	pthread_create(&_tid, NULL, runyy, (void*)this);
}

YYPanel::~YYPanel() {
	pthread_cancel(_tid);
	pthread_join(_tid, NULL);
}

void* YYPanel::runyy(void *arg) {
	((YYPanel*)arg)->run(NULL);
	return NULL;
}

void* YYPanel::run(void *arg) {
	ros::MultiThreadedSpinner spinner(4);
	spinner.spin();
	return NULL;
}

void YYPanel::checkCmdCallback(std_msgs::Int32 i) {
	ROS_INFO("%d", i.data);
	//*
	UserCommand tmp;
	switch (i.data){
	case 1:
		tmp = UserCommand::L2_B;
		break;
	case 2:
		tmp = UserCommand::L2_A;
		break;
	case 3:
		tmp = UserCommand::L2_X;
		break;
	case 4:
		tmp = UserCommand::START;
		break;
#ifdef COMPILE_WITH_MOVE_BASE
	case 5:
		tmp = UserCommand::L2_Y;
		break;
#endif  // COMPILE_WITH_MOVE_BASE
	case 6:
		tmp = UserCommand::L1_X;
		break;
	case 9:
		tmp = UserCommand::L1_A;
		break;
	case 8:
		tmp = UserCommand::L1_Y;
		break;
	case 0:
		userValue.setZero();
		tmp = UserCommand::NONE;
		break;
	default:
		tmp = UserCommand::NONE;
		break;
	}
	userCmd = tmp;
	//*/
}

void YYPanel::changeValueCallback(const geometry_msgs::Point p)
{
	//ROS_INFO("speed: %f, %f, %f", p.x, p.y, p.z);
	// (x, y, z)
	// x for x-axis speed, y for y-axis speed, z for rotate speed
	//*
	  userValue.lx = p.x;
	  userValue.ly = p.y;
	  userValue.rx = p.z;
	//*/
}

/*

 */

bool running = true;

// over watch the ctrl+c command
void ShutDown(int sig)
{
	std::cout << "stop the controller" << std::endl;
	running = false;
}

int main(int argc, char **argv)
{
	/* set the print format */
	std::cout << std::fixed << std::setprecision(3);

	ros::init(argc, argv, "unitree_gazebo_servo");

	IOInterface *ioInter;
	CtrlPlatform ctrlPlat;

	ioInter = new YYROS(new YYPanel());
	ctrlPlat = CtrlPlatform::GAZEBO;

	CtrlComponents *ctrlComp = new CtrlComponents(ioInter);
	ctrlComp->ctrlPlatform = ctrlPlat;
	ctrlComp->dt = 0.002; // run at 500hz
	ctrlComp->running = &running;

	ctrlComp->robotModel = new Go1Robot();

	ctrlComp->waveGen = new WaveGenerator(0.45, 0.5, Vec4(0, 0.5, 0.5, 0)); // Trot
	// ctrlComp->waveGen = new WaveGenerator(1.1, 0.75, Vec4(0, 0.25, 0.5, 0.75));  //Crawl, only for sim
	//ctrlComp->waveGen = new WaveGenerator(0.4, 0.6, Vec4(0, 0.5, 0.5, 0));  //Walking Trot, only for sim
	//ctrlComp->waveGen = new WaveGenerator(0.4, 0.35, Vec4(0, 0.5, 0.5, 0));  //Running Trot, only for sim
	// ctrlComp->waveGen = new WaveGenerator(0.4, 0.7, Vec4(0, 0, 0, 0));  //Pronk, only for sim

	ctrlComp->geneObj();

	ControlFrame ctrlFrame(ctrlComp);

	// deal with Ctrl+C
	signal(SIGINT, ShutDown);

	while (running)
	{
		ctrlFrame.run();
	}

	delete ctrlComp;
	return 0;
}