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

add z1_controller

This commit is contained in:
Agnel Wang 2023-01-06 17:16:26 +08:00
parent a1ba5abf27
commit af28ff54c4
6 changed files with 271 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
README.md
View File

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

42
z1_controller/CMakeLists.txt Normal file
View File

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

37
z1_controller/include/unitreeArm.h Normal file
View File

@ -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

34
z1_controller/package.xml Normal file
View File

@ -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>

17
z1_controller/src/basic.cpp Normal file
View File

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

123
z1_controller/src/unitreeArm.cpp Normal file
View File

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