From af28ff54c4b3bb0093293970eb31451a1ba84a99 Mon Sep 17 00:00:00 2001 From: Agnel Wang <59766821+Agnel-Wang@users.noreply.github.com> Date: Fri, 6 Jan 2023 17:16:26 +0800 Subject: [PATCH] add z1_controller --- README.md | 22 +++++- z1_controller/CMakeLists.txt | 42 ++++++++++ z1_controller/include/unitreeArm.h | 37 +++++++++ z1_controller/package.xml | 34 ++++++++ z1_controller/src/basic.cpp | 17 ++++ z1_controller/src/unitreeArm.cpp | 123 +++++++++++++++++++++++++++++ 6 files changed, 271 insertions(+), 4 deletions(-) create mode 100644 z1_controller/CMakeLists.txt create mode 100644 z1_controller/include/unitreeArm.h create mode 100644 z1_controller/package.xml create mode 100644 z1_controller/src/basic.cpp create mode 100644 z1_controller/src/unitreeArm.cpp diff --git a/README.md b/README.md index 367fcfd..cdd9276 100644 --- a/README.md +++ b/README.md @@ -2,9 +2,9 @@ Here are the ROS simulation packages for Unitree robots, You can load robots and joint controllers in Gazebo, so you can perform low-level control (control the torque, position and angular velocity) of the robot joints. Please be aware that the Gazebo simulation cannot do high-level control, namely walking. Aside from these simulation functions, you can also control your real robots in ROS with the [unitree_ros_to_real](https://github.com/unitreerobotics/unitree_ros_to_real) packages. For real robots, you can do high-level and low-level control using our ROS packages. ## Packages: -Robot description: `go1_description`, `a1_description`, `aliengo_description`, `laikago_description` +Robot description: `go1_description`, `a1_description`, `aliengo_description`, `laikago_description`, `z1_description` -Robot and joints controller: `unitree_controller` +Robot and joints controller: `unitree_controller`, `z1_controller` Simulation related: `unitree_gazebo`, `unitree_legged_control` @@ -57,7 +57,7 @@ roslaunch unitree_gazebo normal.launch rname:=a1 wname:=stairs ``` Where the `rname` means robot name, which can be `laikago`, `aliengo`, `a1` or `go1`. The `wname` means world name, which can be `earth`, `space` or `stairs`. And the default value of `rname` is `laikago`, while the default value of `wname` is `earth`. In Gazebo, the robot should be lying on the ground with joints not activated. -### Stand controller +### 1. Stand controller After launching the gazebo simulation, you can start to control the robot: ``` rosrun unitree_controller unitree_servo @@ -67,7 +67,7 @@ And you can add external disturbances, like a push or a kick: ``` rosrun unitree_controller unitree_external_force ``` -### Position and pose publisher +### 2. Position and pose publisher Here we demonstrated how to control the position and pose of robot without a controller, which should be useful in SLAM or visual development. Then run the position and pose publisher in another terminal: @@ -75,3 +75,17 @@ Then run the position and pose publisher in another terminal: rosrun unitree_controller unitree_move_kinetic ``` The robot will turn around the origin, which is the movement under the world coordinate frame. And inside of the source file [move_publisher.cpp](https://github.com/unitreerobotics/unitree_ros/blob/master/unitree_controller/src/move_publisher.cpp), we also provide the method to move using the robot coordinate frame. You can change the value of `def_frame` to `coord::ROBOT` and run the catkin_make again, then the `unitree_move_publisher` will move robot under its own coordinate frame. + +## z1_controller + +You can launch the z1 Gazebo simulation with the following command: + +``` +roslaunch unitree_gazebo z1.launch +``` + +After launching the gazebo simulation, you can start to control the z1 robot by z1_sdk. see [z1_documentation](dev-z1.unitree.com/5-sdk/run.html) +You can also send ros commands derectly to z1 robot: +``` +rosrun z1_controller unitree_move_z1 +``` diff --git a/z1_controller/CMakeLists.txt b/z1_controller/CMakeLists.txt new file mode 100644 index 0000000..875d168 --- /dev/null +++ b/z1_controller/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.0.2) +project(z1_controller) + +find_package(gazebo REQUIRED) +find_package(catkin REQUIRED COMPONENTS + controller_manager + std_msgs + joint_state_controller + robot_state_publisher + roscpp + gazebo_ros + geometry_msgs + unitree_legged_msgs + tf +) + +catkin_package( + CATKIN_DEPENDS + unitree_legged_msgs +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${GAZEBO_INCLUDE_DIRS} +) + +link_directories(${GAZEBO_LIBRARY_DIRS}) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") + +add_library(${PROJECT_NAME} + src/unitreeArm.cpp +) + +add_dependencies(${PROJECT_NAME} unitree_legged_msgs_gencpp) + +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} ${EXTRA_LIBS} +) + +add_executable(unitree_move_z1 src/basic.cpp) +target_link_libraries(unitree_move_z1 ${PROJECT_NAME} ${catkin_LIBRARIES}) \ No newline at end of file diff --git a/z1_controller/include/unitreeArm.h b/z1_controller/include/unitreeArm.h new file mode 100644 index 0000000..ec4d997 --- /dev/null +++ b/z1_controller/include/unitreeArm.h @@ -0,0 +1,37 @@ +#ifndef __UNITREEARM_H +#define __UNITREEARM_H + +#include +#include "unitree_legged_msgs/MotorCmd.h" +#include "unitree_legged_msgs/MotorState.h" + +namespace unitreeArm { + +class IOROS +{ +public: + IOROS(); + ~IOROS(); + + void sendCmd(double* targetPos, double duration); + +private: + void _joint01Callback(const unitree_legged_msgs::MotorState& msg); + void _joint02Callback(const unitree_legged_msgs::MotorState& msg); + void _joint03Callback(const unitree_legged_msgs::MotorState& msg); + void _joint04Callback(const unitree_legged_msgs::MotorState& msg); + void _joint05Callback(const unitree_legged_msgs::MotorState& msg); + void _joint06Callback(const unitree_legged_msgs::MotorState& msg); + void _gripperCallback(const unitree_legged_msgs::MotorState& msg); + + ros::NodeHandle _nm; + ros::Publisher _servo_pub[7]; + ros::Subscriber _servo_sub[7]; + unitree_legged_msgs::MotorCmd _joint_cmd[7]; + unitree_legged_msgs::MotorState _joint_state[7]; + +}; + +} + +#endif diff --git a/z1_controller/package.xml b/z1_controller/package.xml new file mode 100644 index 0000000..151c29f --- /dev/null +++ b/z1_controller/package.xml @@ -0,0 +1,34 @@ + + + z1_controller + 0.0.0 + The z1_controller package + + unitree + TODO + + catkin + + catkin + genmsg + controller_manager + joint_state_controller + robot_state_publisher + roscpp + std_msgs + controller_manager + joint_state_controller + robot_state_publisher + roscpp + std_msgs + controller_manager + joint_state_controller + robot_state_publisher + roscpp + std_msgs + unitree_legged_msgs + + + + + diff --git a/z1_controller/src/basic.cpp b/z1_controller/src/basic.cpp new file mode 100644 index 0000000..3b5c0e3 --- /dev/null +++ b/z1_controller/src/basic.cpp @@ -0,0 +1,17 @@ +#include "unitreeArm.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "z1_controller"); + ros::AsyncSpinner subSpinner(1); // one threads + subSpinner.start(); + usleep(300000); // must wait 300ms, to get first state + + unitreeArm::IOROS ioInter; + + // move z1 to [forward] + double forward[7] = {0, 1.5, -1, -0.54, 0, 0, -1}; + ioInter.sendCmd(forward, 1000); + + return 0; +} \ No newline at end of file diff --git a/z1_controller/src/unitreeArm.cpp b/z1_controller/src/unitreeArm.cpp new file mode 100644 index 0000000..6cf2539 --- /dev/null +++ b/z1_controller/src/unitreeArm.cpp @@ -0,0 +1,123 @@ +#include "unitreeArm.h" + +namespace unitreeArm { + +IOROS::IOROS() +{ + // publisher init + _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); + + // subscriber init + _servo_sub[0] = _nm.subscribe("/z1_gazebo/Joint01_controller/state", 1, &IOROS::_joint01Callback, this); + _servo_sub[1] = _nm.subscribe("/z1_gazebo/Joint02_controller/state", 1, &IOROS::_joint02Callback, this); + _servo_sub[2] = _nm.subscribe("/z1_gazebo/Joint03_controller/state", 1, &IOROS::_joint03Callback, this); + _servo_sub[3] = _nm.subscribe("/z1_gazebo/Joint04_controller/state", 1, &IOROS::_joint04Callback, this); + _servo_sub[4] = _nm.subscribe("/z1_gazebo/Joint05_controller/state", 1, &IOROS::_joint05Callback, this); + _servo_sub[5] = _nm.subscribe("/z1_gazebo/Joint06_controller/state", 1, &IOROS::_joint06Callback, this); + _servo_sub[6] = _nm.subscribe("/z1_gazebo/gripper_controller/state", 1, &IOROS::_gripperCallback, this); + + // parameter init + for(size_t i=0; i<7; ++i) + { + _joint_cmd[i].mode = 10; + _joint_cmd[i].Kp = 300.0; + _joint_cmd[i].Kd = 5; + _joint_cmd[i].q = 0; + _joint_cmd[i].dq = 0; + _joint_cmd[i].tau = 0; + } +} + +IOROS::~IOROS() +{ + +} + +void IOROS::_joint01Callback(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::_joint02Callback(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::_joint03Callback(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::_joint04Callback(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::_joint05Callback(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::_joint06Callback(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) +{ + _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; +} + +void IOROS::sendCmd(double* targetPos, double duration) +{ + double pos[7], lastPos[7], percent; + for(size_t i=0; i<7; ++i) lastPos[i] = _joint_state[i].q; + for(int i=1; i<=duration; i++) + { + if(!ros::ok()) break; + percent = (double)i/duration; + for(size_t j=0; j<7; ++j) + { + _joint_cmd[j].q = lastPos[j]*(1-percent) + targetPos[j]*percent; + _servo_pub[j].publish(_joint_cmd[j]); + } + ros::spinOnce(); + usleep(1000); + } +} + +} \ No newline at end of file