add z1_controller

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
+```

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})

z1_controller/include/unitreeArm.h

@@ -0,0 +1,37 @@
+#ifndef __UNITREEARM_H
+#define __UNITREEARM_H
+
+#include <ros/ros.h>
+#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

z1_controller/package.xml

@@ -0,0 +1,34 @@
+<?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>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>genmsg</buildtool_depend>
+  <build_depend>controller_manager</build_depend>
+  <build_depend>joint_state_controller</build_depend>
+  <build_depend>robot_state_publisher</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>controller_manager</build_export_depend>
+  <build_export_depend>joint_state_controller</build_export_depend>
+  <build_export_depend>robot_state_publisher</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>joint_state_controller</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <depend>unitree_legged_msgs</depend>
+
+  <export>
+
+  </export>
+</package>

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;
+}

z1_controller/src/unitreeArm.cpp

@@ -0,0 +1,123 @@
+#include "unitreeArm.h"
+
+namespace unitreeArm {
+
+IOROS::IOROS()
+{
+    // publisher init
+    _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);
+
+    // 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);
+    }
+}
+
+}