EmbodiedAI
/
z1_controller
mirror of https://github.com/unitreerobotics/z1_controller.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

V2.2.7

This commit is contained in:
Agnel Wang 2023-05-25 21:01:46 +08:00
parent 0648b1888e
commit 7cf9f61955
16 changed files with 332 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
CMakeLists.txt
View File

@ -1,67 +1,23 @@
cmake_minimum_required(VERSION 3.0)
project(z1_controller LANGUAGES CXX)
add_definitions(-std=c++14)
set(PACKAGE_NAME z1_controller)
set(CMAKE_CXX_FLAGS "$ENV{CXXFLAGS} -O3")
set(CMAKE_CXX_FLAGS "$ENV{CXXFLAGS} -O3 -std=c++14 -pthread")
# ----------------------options----------------------
set(COMMUNICATION UDP) # UDP
# set(COMMUNICATION ROS) # ROS
# ----------------------configurations----------------------
# COMMUNICATION
if(${COMMUNICATION} STREQUAL "UDP")
set(SIMULATION OFF)
elseif(${COMMUNICATION} STREQUAL "ROS")
add_definitions(-DCOMPILE_WITH_SIMULATION)
set(SIMULATION ON)
else()
message(FATAL_ERROR "[CMake ERROR] The COMMUNICATION is error")
endif()
if(SIMULATION)
find_package(gazebo REQUIRED)
find_package(catkin REQUIRED COMPONENTS
controller_manager
genmsg
joint_state_controller
robot_state_publisher
roscpp
gazebo_ros
std_msgs
tf
geometry_msgs
)
catkin_package(
CATKIN_DEPENDS
)
include_directories(
${catkin_INCLUDE_DIRS}
${GAZEBO_INCLUDE_DIRS}
)
link_directories(
${GAZEBO_LIBRARY_DIRS}
)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
endif()
set(EIGEN_PATH /usr/include/eigen3)
find_package(Boost REQUIRED)
find_package(Eigen3 REQUIRED)
include_directories(
include
${Boost_INCLUDE_DIRS}
${EIGEN_PATH}
${Eigen3_INCLUDE_DIRS}
include
)
link_directories(lib)
# ----------------------add executable----------------------
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR})
add_executable(z1_ctrl main.cpp)
target_link_libraries(z1_ctrl libZ1_${COMMUNICATION}_${CMAKE_HOST_SYSTEM_PROCESSOR}.so ${catkin_LIBRARIES} )
target_link_libraries(z1_ctrl libZ1_${CMAKE_HOST_SYSTEM_PROCESSOR}.so)
find_package(gazebo)
if(${gazebo_FOUND})
add_subdirectory(sim)
endif()

2
include/common/math/Filter.h
View File

@ -2,7 +2,7 @@
#define FILTER
#include <vector>
#include <Eigen/Dense>
#include <eigen3/Eigen/Dense>
/*
Low pass filter

9
include/common/math/mathTools.h
View File

@ -25,6 +25,15 @@ inline T min(const T val0, const Args... restVal){
return val0 > (min<T>(restVal...)) ? val0 : min<T>(restVal...);
}
inline double clamp(const double& x, const double& low_value, const double& high_value) {
return (x > low_value) ? (x<high_value?x:high_value) : low_value;
}
inline double sign(const double& x, double threhold = 1e-6){
return std::fabs(x) < threhold ? 0 : ( x>0?1:-1 );
}
enum class TurnDirection{
NOMATTER,
POSITIVE,

2
include/common/utilities/typeTrans.h
View File

@ -3,7 +3,7 @@
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <eigen3/Eigen/Dense>
namespace typeTrans{

8
include/control/CtrlComponents.h
View File

@ -10,7 +10,6 @@
#include "common/utilities/CSVTool.h"
#include "model/ArmModel.h"
#include "interface/IOUDP.h"
#include "interface/IOROS.h"
#include "control/armSDK.h"
#include "model/unitree_gripper.h"
@ -27,6 +26,7 @@ public:
Z1Model *armModel;
CSVTool *stateCSV;
std::shared_ptr<Unitree_Gripper> gripper;
double gripper_max_tau = 10;
SendCmd sendCmd; // cmd that receive from SDK
RecvState recvState;// state that send to SDK
@ -41,14 +41,14 @@ public:
bool isPlot;
int trajChoose = 1;
size_t armType = 36;
std::string ctrl_IP;
uint ctrl_port;
void geneObj();
void writeData();
private:
void configProcess(int argc, char** argv);
std::string ctrl_IP;
uint ctrl_port;
double _loadWeight;
};

BIN
lib/libZ1_ROS_x86_64.so
View File

Binary file not shown.

BIN
lib/libZ1_UDP_aarch64.so
View File

Binary file not shown.

BIN
lib/libZ1_UDP_x86_64.so
View File

Binary file not shown.

BIN
lib/libZ1_aarch64.so Executable file
View File

Binary file not shown.

BIN
lib/libZ1_x86_64.so Executable file
View File

Binary file not shown.

5
main.cpp
View File

@ -39,13 +39,10 @@ int main(int argc, char **argv){
std::vector<KeyAction*> events;
CtrlComponents *ctrlComp = new CtrlComponents(argc, argv);
#ifdef COMPILE_WITH_SIMULATION
ros::init(argc, argv, "z1_controller");
#endif
ctrlComp->dt = 1.0/250.;
ctrlComp->armConfigPath = "../config/";
ctrlComp->stateCSV = new CSVTool("../config/savedArmStates.csv");
ctrlComp->ioInter = new IOUDP(ctrlComp->ctrl_IP.c_str(), ctrlComp->ctrl_port);
ctrlComp->geneObj();
if(ctrlComp->ctrl == Control::SDK){
ctrlComp->cmdPanel = new ARMSDK(events, emptyAction, "127.0.0.1", 8072, 0.002);

34
sim/CMakeLists.txt Normal file
View File

@ -0,0 +1,34 @@
find_package(catkin REQUIRED COMPONENTS
controller_manager
genmsg
joint_state_controller
robot_state_publisher
roscpp
gazebo_ros
std_msgs
tf
geometry_msgs
)
catkin_package(
CATKIN_DEPENDS
)
include_directories(
${catkin_INCLUDE_DIRS}
${GAZEBO_INCLUDE_DIRS}
sim
)
link_directories(
${GAZEBO_LIBRARY_DIRS}
)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR})
add_executable(sim_ctrl sim_ctrl.cpp IOROS.cpp)
target_link_libraries(sim_ctrl
libZ1_${CMAKE_HOST_SYSTEM_PROCESSOR}.so
${catkin_LIBRARIES}
)

142
sim/IOROS.cpp Normal file
View File

@ -0,0 +1,142 @@
#include "IOROS.h"
#include <iostream>
#include <unistd.h>
#include <csignal>
void RosShutDown(int sig){
ROS_INFO("ROS interface shutting down!");
ros::shutdown();
}
IOROS::IOROS(){
std::cout << "The control interface for ROS Gazebo simulation" << std::endl;
hasGripper = false;
/* start subscriber */
_initRecv();
ros::AsyncSpinner subSpinner(1); // one threads
subSpinner.start();
usleep(300000); //wait for subscribers start
/* initialize publisher */
_initSend();
signal(SIGINT, RosShutDown);
//check if there is UnitreeGripper
unitree_legged_msgs::MotorCmd grippercmd;
grippercmd.mode = 10;
grippercmd.q = 0;
grippercmd.dq = 0;
grippercmd.tau = 0;
grippercmd.Kp = 0;
grippercmd.Kd = 0;
_servo_pub[6].publish(grippercmd);
ros::spinOnce();
}
IOROS::~IOROS(){
}
bool IOROS::sendRecv(const LowlevelCmd *cmd, LowlevelState *state){
_sendCmd(cmd);
_recvState(state);
return true;
}
void IOROS::_sendCmd(const LowlevelCmd *lowCmd){
for(int i(0); i < lowCmd->q.size(); ++i){
_joint_cmd[i].mode = 10;
_joint_cmd[i].q = lowCmd->q[i];
_joint_cmd[i].dq = lowCmd->dq[i];
_joint_cmd[i].tau = lowCmd->tau[i];
_joint_cmd[i].Kd = lowCmd->kd[i]*0.0128;
_joint_cmd[i].Kp = lowCmd->kp[i]*25.6;
_servo_pub[i].publish(_joint_cmd[i]);
}
ros::spinOnce();
}
void IOROS::_recvState(LowlevelState *state){
for(int i(0); i < state->q.size(); ++i){
state->q[i] = _joint_state[i].q;
state->dq[i] = _joint_state[i].dq;
state->ddq[i] = _joint_state[i].ddq;
state->tau[i] = _joint_state[i].tauEst;
}
}
void IOROS::_initSend(){
_servo_pub[0] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint01_controller/command", 1);
_servo_pub[1] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint02_controller/command", 1);
_servo_pub[2] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint03_controller/command", 1);
_servo_pub[3] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint04_controller/command", 1);
_servo_pub[4] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint05_controller/command", 1);
_servo_pub[5] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint06_controller/command", 1);
_servo_pub[6] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/gripper_controller/command", 1);
}
void IOROS::_initRecv(){
_servo_sub[0] = _nm.subscribe("/z1_gazebo/Joint01_controller/state", 1, &IOROS::_joint00Callback, this);
_servo_sub[1] = _nm.subscribe("/z1_gazebo/Joint02_controller/state", 1, &IOROS::_joint01Callback, this);
_servo_sub[2] = _nm.subscribe("/z1_gazebo/Joint03_controller/state", 1, &IOROS::_joint02Callback, this);
_servo_sub[3] = _nm.subscribe("/z1_gazebo/Joint04_controller/state", 1, &IOROS::_joint03Callback, this);
_servo_sub[4] = _nm.subscribe("/z1_gazebo/Joint05_controller/state", 1, &IOROS::_joint04Callback, this);
_servo_sub[5] = _nm.subscribe("/z1_gazebo/Joint06_controller/state", 1, &IOROS::_joint05Callback, this);
_servo_sub[6] = _nm.subscribe("/z1_gazebo/gripper_controller/state", 1, &IOROS::_gripperCallback, this);
}
void IOROS::_joint00Callback(const unitree_legged_msgs::MotorState& msg){
_joint_state[0].mode = msg.mode;
_joint_state[0].q = msg.q;
_joint_state[0].dq = msg.dq;
_joint_state[0].ddq = msg.ddq;
_joint_state[0].tauEst = msg.tauEst;
}
void IOROS::_joint01Callback(const unitree_legged_msgs::MotorState& msg){
_joint_state[1].mode = msg.mode;
_joint_state[1].q = msg.q;
_joint_state[1].dq = msg.dq;
_joint_state[1].ddq = msg.ddq;
_joint_state[1].tauEst = msg.tauEst;
}
void IOROS::_joint02Callback(const unitree_legged_msgs::MotorState& msg){
_joint_state[2].mode = msg.mode;
_joint_state[2].q = msg.q;
_joint_state[2].dq = msg.dq;
_joint_state[2].ddq = msg.ddq;
_joint_state[2].tauEst = msg.tauEst;
}
void IOROS::_joint03Callback(const unitree_legged_msgs::MotorState& msg){
_joint_state[3].mode = msg.mode;
_joint_state[3].q = msg.q;
_joint_state[3].dq = msg.dq;
_joint_state[3].ddq = msg.ddq;
_joint_state[3].tauEst = msg.tauEst;
}
void IOROS::_joint04Callback(const unitree_legged_msgs::MotorState& msg){
_joint_state[4].mode = msg.mode;
_joint_state[4].q = msg.q;
_joint_state[4].dq = msg.dq;
_joint_state[4].ddq = msg.ddq;
_joint_state[4].tauEst = msg.tauEst;
}
void IOROS::_joint05Callback(const unitree_legged_msgs::MotorState& msg){
_joint_state[5].mode = msg.mode;
_joint_state[5].q = msg.q;
_joint_state[5].dq = msg.dq;
_joint_state[5].ddq = msg.ddq;
_joint_state[5].tauEst = msg.tauEst;
}
void IOROS::_gripperCallback(const unitree_legged_msgs::MotorState& msg){
hasGripper = true;
_joint_state[6].mode = msg.mode;
_joint_state[6].q = msg.q;
_joint_state[6].dq = msg.dq;
_joint_state[6].ddq = msg.ddq;
_joint_state[6].tauEst = msg.tauEst;
}

6
include/interface/IOROS.h → sim/IOROS.h
View File

@ -1,5 +1,3 @@
#ifdef COMPILE_WITH_SIMULATION
#ifndef IOROS_H
#define IOROS_H
@ -33,6 +31,4 @@ private:
void _gripperCallback(const unitree_legged_msgs::MotorState& msg);
};
#endif // IOROS_H
#endif // COMPILE_WITH_SIMULATION
#endif // IOROS_H

44
package.xml → sim/package.xml
View File

@ -1,22 +1,22 @@
<?xml version="1.0"?>
<package format="2">
<name>z1_controller</name>
<version>0.0.0</version>
<description>The z1_controller package</description>
<maintainer email="laikago@unitree.cc">unitree</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscp</build_depend>
<build_depend>urdf</build_depend>
<build_depend>xacro</build_depend>
<build_export_depend>roscp</build_export_depend>
<build_export_depend>urdf</build_export_depend>
<build_export_depend>xacro</build_export_depend>
<exec_depend>roscp</exec_depend>
<exec_depend>urdf</exec_depend>
<exec_depend>xacro</exec_depend>
</package>
<?xml version="1.0"?>
<package format="2">
<name>z1_controller</name>
<version>0.0.0</version>
<description>The z1_controller package</description>
<maintainer email="laikago@unitree.cc">unitree</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscp</build_depend>
<build_depend>urdf</build_depend>
<build_depend>xacro</build_depend>
<build_export_depend>roscp</build_export_depend>
<build_export_depend>urdf</build_export_depend>
<build_export_depend>xacro</build_export_depend>
<exec_depend>roscp</exec_depend>
<exec_depend>urdf</exec_depend>
<exec_depend>xacro</exec_depend>
</package>

107
sim/sim_ctrl.cpp Normal file
View File

@ -0,0 +1,107 @@
#include <csignal>
#include <sched.h>
#include "FSM/FiniteStateMachine.h"
#include "FSM/State_Passive.h"
#include "FSM/State_BackToStart.h"
#include "FSM/State_Calibration.h"
#include "FSM/State_Cartesian.h"
#include "FSM/State_JointSpace.h"
#include "FSM/State_MoveJ.h"
#include "FSM/State_MoveL.h"
#include "FSM/State_MoveC.h"
#include "FSM/State_ToState.h"
#include "FSM/State_SaveState.h"
#include "FSM/State_Teach.h"
#include "FSM/State_TeachRepeat.h"
#include "FSM/State_Trajectory.h"
#include "FSM/State_LowCmd.h"
#include "IOROS.h"
bool running = true;
//set real-time program
void setProcessScheduler(){
pid_t pid = getpid();
sched_param param;
param.sched_priority = sched_get_priority_max(SCHED_FIFO);
if (sched_setscheduler(pid, SCHED_FIFO, &param) == -1) {
// std::cout << "[ERROR] Function setProcessScheduler failed." << std::endl;
}
}
int main(int argc, char **argv){
/* set real-time process */
setProcessScheduler();
/* set the print format */
std::cout << std::fixed << std::setprecision(5);
EmptyAction emptyAction((int)ArmFSMStateName::INVALID);
std::vector<KeyAction*> events;
CtrlComponents *ctrlComp = new CtrlComponents(argc, argv);
ros::init(argc, argv, "z1_controller");
ctrlComp->dt = 1.0/250.;
ctrlComp->armConfigPath = "../config/";
ctrlComp->stateCSV = new CSVTool("../config/savedArmStates.csv");
ctrlComp->ioInter = new IOROS();
ctrlComp->geneObj();
if(ctrlComp->ctrl == Control::SDK){
ctrlComp->cmdPanel = new ARMSDK(events, emptyAction, "127.0.0.1", 8072, 0.002);
}else if(ctrlComp->ctrl == Control::KEYBOARD){
events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART));
events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE));
events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL));
events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN));
events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ));
events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL));
events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC));
events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH));
events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT));
events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE));
events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE));
events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY));
events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION));
events.push_back(new ValueAction("q", "a", 0.5));
events.push_back(new ValueAction("w", "s", 0.5));
events.push_back(new ValueAction("e", "d", 0.5));
events.push_back(new ValueAction("r", "f", 0.5));
events.push_back(new ValueAction("t", "g", 0.5));
events.push_back(new ValueAction("y", "h", 0.5));
events.push_back(new ValueAction("down", "up", 1.));
ctrlComp->cmdPanel = new Keyboard(events, emptyAction);
}
std::vector<FSMState*> states;
states.push_back(new State_Passive(ctrlComp));
states.push_back(new State_BackToStart(ctrlComp));
states.push_back(new State_JointSpace(ctrlComp));
states.push_back(new State_Cartesian(ctrlComp));
states.push_back(new State_MoveJ(ctrlComp));
states.push_back(new State_MoveL(ctrlComp));
states.push_back(new State_MoveC(ctrlComp));
states.push_back(new State_LowCmd(ctrlComp));
states.push_back(new State_SaveState(ctrlComp));
states.push_back(new State_Teach(ctrlComp));
states.push_back(new State_TeachRepeat(ctrlComp));
states.push_back(new State_ToState(ctrlComp));
states.push_back(new State_Trajectory(ctrlComp));
states.push_back(new State_Calibration(ctrlComp));
FiniteStateMachine *fsm;
fsm = new FiniteStateMachine(states, ctrlComp);
ctrlComp->running = &running;
signal(SIGINT, [](int signum){running = false;});
std::this_thread::sleep_for(std::chrono::seconds(1));
ctrlComp->cmdPanel->start();
while(running){
std::this_thread::sleep_for(std::chrono::seconds(1));
}
delete fsm;
delete ctrlComp;
return 0;
}