diff --git a/CMakeLists.txt b/CMakeLists.txt index 6811c18..a86e2a9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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() \ No newline at end of file diff --git a/include/common/math/Filter.h b/include/common/math/Filter.h index c5bebe3..569f551 100755 --- a/include/common/math/Filter.h +++ b/include/common/math/Filter.h @@ -2,7 +2,7 @@ #define FILTER #include -#include +#include /* Low pass filter diff --git a/include/common/math/mathTools.h b/include/common/math/mathTools.h index 11f812a..79c2a3a 100755 --- a/include/common/math/mathTools.h +++ b/include/common/math/mathTools.h @@ -25,6 +25,15 @@ inline T min(const T val0, const Args... restVal){ return val0 > (min(restVal...)) ? val0 : min(restVal...); } +inline double clamp(const double& x, const double& low_value, const double& high_value) { + return (x > low_value) ? (x0?1:-1 ); +} + + enum class TurnDirection{ NOMATTER, POSITIVE, diff --git a/include/common/utilities/typeTrans.h b/include/common/utilities/typeTrans.h index 8096676..8277749 100755 --- a/include/common/utilities/typeTrans.h +++ b/include/common/utilities/typeTrans.h @@ -3,7 +3,7 @@ #include #include -#include +#include namespace typeTrans{ diff --git a/include/control/CtrlComponents.h b/include/control/CtrlComponents.h index 852e60b..3070d2c 100755 --- a/include/control/CtrlComponents.h +++ b/include/control/CtrlComponents.h @@ -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 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; }; diff --git a/lib/libZ1_ROS_x86_64.so b/lib/libZ1_ROS_x86_64.so deleted file mode 100755 index 7b3de50..0000000 Binary files a/lib/libZ1_ROS_x86_64.so and /dev/null differ diff --git a/lib/libZ1_UDP_aarch64.so b/lib/libZ1_UDP_aarch64.so deleted file mode 100755 index f6194f2..0000000 Binary files a/lib/libZ1_UDP_aarch64.so and /dev/null differ diff --git a/lib/libZ1_UDP_x86_64.so b/lib/libZ1_UDP_x86_64.so deleted file mode 100755 index 78099fa..0000000 Binary files a/lib/libZ1_UDP_x86_64.so and /dev/null differ diff --git a/lib/libZ1_aarch64.so b/lib/libZ1_aarch64.so new file mode 100755 index 0000000..afd061f Binary files /dev/null and b/lib/libZ1_aarch64.so differ diff --git a/lib/libZ1_x86_64.so b/lib/libZ1_x86_64.so new file mode 100755 index 0000000..304231e Binary files /dev/null and b/lib/libZ1_x86_64.so differ diff --git a/main.cpp b/main.cpp index 13fc7ea..33b59d1 100644 --- a/main.cpp +++ b/main.cpp @@ -39,13 +39,10 @@ int main(int argc, char **argv){ std::vector 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); diff --git a/sim/CMakeLists.txt b/sim/CMakeLists.txt new file mode 100644 index 0000000..304fffc --- /dev/null +++ b/sim/CMakeLists.txt @@ -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} +) \ No newline at end of file diff --git a/sim/IOROS.cpp b/sim/IOROS.cpp new file mode 100644 index 0000000..38febe0 --- /dev/null +++ b/sim/IOROS.cpp @@ -0,0 +1,142 @@ +#include "IOROS.h" +#include +#include +#include + +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("/z1_gazebo/Joint01_controller/command", 1); + _servo_pub[1] = _nm.advertise("/z1_gazebo/Joint02_controller/command", 1); + _servo_pub[2] = _nm.advertise("/z1_gazebo/Joint03_controller/command", 1); + _servo_pub[3] = _nm.advertise("/z1_gazebo/Joint04_controller/command", 1); + _servo_pub[4] = _nm.advertise("/z1_gazebo/Joint05_controller/command", 1); + _servo_pub[5] = _nm.advertise("/z1_gazebo/Joint06_controller/command", 1); + _servo_pub[6] = _nm.advertise("/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; +} \ No newline at end of file diff --git a/include/interface/IOROS.h b/sim/IOROS.h similarity index 92% rename from include/interface/IOROS.h rename to sim/IOROS.h index 7eb13c0..5647fa0 100644 --- a/include/interface/IOROS.h +++ b/sim/IOROS.h @@ -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 \ No newline at end of file +#endif // IOROS_H \ No newline at end of file diff --git a/package.xml b/sim/package.xml similarity index 96% rename from package.xml rename to sim/package.xml index 2c176ed..36898d6 100644 --- a/package.xml +++ b/sim/package.xml @@ -1,22 +1,22 @@ - - - z1_controller - 0.0.0 - The z1_controller package - - unitree - - TODO - - catkin - roscp - urdf - xacro - roscp - urdf - xacro - roscp - urdf - xacro - - + + + z1_controller + 0.0.0 + The z1_controller package + + unitree + + TODO + + catkin + roscp + urdf + xacro + roscp + urdf + xacro + roscp + urdf + xacro + + diff --git a/sim/sim_ctrl.cpp b/sim/sim_ctrl.cpp new file mode 100644 index 0000000..9133047 --- /dev/null +++ b/sim/sim_ctrl.cpp @@ -0,0 +1,107 @@ +#include +#include +#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, ¶m) == -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 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 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; +} \ No newline at end of file