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

added aliengo(firmwork 3.1)

This commit is contained in:
Bian Zekun 2020-08-14 11:32:28 +08:00
parent 23c4e9cd2f
commit 95373581e2
12 changed files with 360 additions and 114 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
README.md
View File

@ -16,6 +16,7 @@ Real robot control related: `unitree_legged_real`
* [ROS](https://www.ros.org/) melodic or ROS kinetic(has not been tested)
* [Gazebo8](http://gazebosim.org/)
* [unitree_legged_sdk](https://github.com/unitreerobotics)
* [aliengo_sdk](https://github.com/unitreerobotics)
# Configuration
Make sure the following exist in your `~/.bashrc` file or export them in terminal. `melodic`, `gazebo-8`, `~/catkin_ws`, `amd64` and the paths to `unitree_legged_sdk` should be replaced in your own case.
@ -27,6 +28,7 @@ export ROS_PACKAGE_PATH=~/catkin_ws:${ROS_PACKAGE_PATH}
export GAZEBO_PLUGIN_PATH=~/catkin_ws/devel/lib:${GAZEBO_PLUGIN_PATH}
export LD_LIBRARY_PATH=~/catkin_ws/devel/lib:${LD_LIBRARY_PATH}
export UNITREE_LEGGED_SDK_PATH=~/unitree_legged_sdk
export ALIENGO_SDK_PATH=~/aliengo_sdk
#amd64, arm32, arm64
export UNITREE_PLATFORM="amd64"
```
@ -130,9 +132,11 @@ First you have to run the `real_launch` under root account:
```
sudo su
source ~/catkin_ws/devel/setup.bash
roslaunch unitree_legged_real real.launch rname:=a1 ctrl_level:=lowlevel
roslaunch unitree_legged_real real.launch rname:=a1 ctrl_level:=highlevel firmwork:=3_2
```
These commands will launch a LCM server. The `rname` means robot name, which can be `a1` or `aliengo`(case does not matter), and the default value is `a1`. And the `ctrl_level` means the control level, which can be `lowlevel` or `higelevel`(case does not matter), and the default value is `higelevel`. Under the low level, you can control the joints directly. And under the high level, you can control the robot to move or change its pose. To do so, you need to run the controller in another terminal:
These commands will launch a LCM server. The `rname` means robot name, which can be `a1` or `aliengo`(case does not matter), and the default value is `a1`. And the `ctrl_level` means the control level, which can be `lowlevel` or `highlevel`(case does not matter), and the default value is `highlevel`. Under the low level, you can control the joints directly. And under the high level, you can control the robot to move or change its pose. The `firmwork` means the firmwork version of the robot. The default value is `3_2` Now all the A1's firmwork version is `3_2`, and most Aliengo's firmwork version is `3_1`.(will update in the future)
To do so, you need to run the controller in another terminal(also under root account):
```
rosrun unitree_legged_real position_lcm
```

12
unitree_legged_msgs/msg/HighCmd.msg
View File

@ -1,8 +1,8 @@
uint8 levelFlag
uint16 commVersion
uint16 robotID
uint32 SN
uint8 bandWidth
uint16 commVersion # Old version Aliengo does not have
uint16 robotID # Old version Aliengo does not have
uint32 SN # Old version Aliengo does not have
uint8 bandWidth # Old version Aliengo does not have
uint8 mode
float32 forwardSpeed
float32 sideSpeed
@ -14,6 +14,6 @@ float32 pitch
float32 roll
LED[4] led
uint8[40] wirelessRemote
uint8[40] AppRemote
uint32 reserve
uint8[40] AppRemote # Old version Aliengo does not have
uint32 reserve # Old version Aliengo does not have
int32 crc

20
unitree_legged_msgs/msg/HighState.msg
View File

@ -1,8 +1,8 @@
uint8 levelFlag
uint16 commVersion
uint16 robotID
uint32 SN
uint8 bandWidth
uint16 commVersion # Old version Aliengo does not have
uint16 robotID # Old version Aliengo does not have
uint32 SN # Old version Aliengo does not have
uint8 bandWidth # Old version Aliengo does not have
uint8 mode
IMU imu
float32 forwardSpeed
@ -10,17 +10,17 @@ float32 sideSpeed
float32 rotateSpeed
float32 bodyHeight
float32 updownSpeed
float32 forwardPosition # (will be float type next version)
float32 sidePosition # (will be float type next version)
float32 forwardPosition # (will be float type next version) # Old version Aliengo is different
float32 sidePosition # (will be float type next version) # Old version Aliengo is different
Cartesian[4] footPosition2Body
Cartesian[4] footSpeed2Body
int16[4] footForce
int16[4] footForceEst
int16[4] footForce # Old version Aliengo is different
int16[4] footForceEst # Old version Aliengo does not have
uint32 tick
uint8[40] wirelessRemote
uint32 reserve
uint32 reserve # Old version Aliengo does not have
uint32 crc
# Under are not defined in SDK yet.
# Under are not defined in SDK yet. # Old version Aliengo does not have
Cartesian[4] eeForce # It's a 1-DOF force in real robot, but 3-DOF is better for visualization.
float32[12] jointP # for visualization

14
unitree_legged_msgs/msg/LowCmd.msg
View File

@ -1,12 +1,12 @@
uint8 levelFlag
uint16 commVersion
uint16 robotID
uint32 SN
uint8 bandWidth
uint8 levelFlag
uint16 commVersion # Old version Aliengo does not have
uint16 robotID # Old version Aliengo does not have
uint32 SN # Old version Aliengo does not have
uint8 bandWidth # Old version Aliengo does not have
MotorCmd[20] motorCmd
LED[4] led
uint8[40] wirelessRemote
uint32 reserve
uint32 reserve # Old version Aliengo does not have
uint32 crc
Cartesian[4] ff # will delete
Cartesian[4] ff # will delete # Old version Aliengo does not have

16
unitree_legged_msgs/msg/LowState.msg
View File

@ -1,18 +1,18 @@
uint8 levelFlag
uint16 commVersion
uint16 robotID
uint32 SN
uint8 bandWidth
uint16 commVersion # Old version Aliengo does not have
uint16 robotID # Old version Aliengo does not have
uint32 SN # Old version Aliengo does not have
uint8 bandWidth # Old version Aliengo does not have
IMU imu
MotorState[20] motorState
int16[4] footForce # force sensors
int16[4] footForceEst # force sensors
int16[4] footForce # force sensors # Old version Aliengo is different
int16[4] footForceEst # force sensors # Old version Aliengo does not have
uint32 tick # reference real-time from motion controller (unit: us)
uint8[40] wirelessRemote # wireless commands
uint32 reserve
uint32 reserve # Old version Aliengo does not have
uint32 crc
# Old version Aliengo does not have:
Cartesian[4] eeForceRaw
Cartesian[4] eeForce #it's a 1-DOF force infact, but we use 3-DOF here just for visualization
Cartesian position # will delete

14
unitree_legged_real/CMakeLists.txt
View File

@ -24,11 +24,13 @@ include_directories(
${EIGEN3_INCLUDE_DIR}
# /home/bian/unitree_legged_sdk/include/unitree_legged_sdk
$ENV{UNITREE_LEGGED_SDK_PATH}/include
$ENV{ALIENGO_SDK_PATH}/include
)
link_directories($ENV{UNITREE_LEGGED_SDK_PATH}/lib)
link_directories($ENV{ALIENGO_SDK_PATH}/lib)
string(CONCAT LEGGED_SDK_NAME libunitree_legged_sdk_$ENV{UNITREE_PLATFORM}.so)
set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm)
set(EXTRA_LIBS ${LEGGED_SDK_NAME} libaliengo_comm.so lcm)
set(CMAKE_CXX_FLAGS "-O3")
@ -49,6 +51,10 @@ target_link_libraries(walk_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_dependencies(walk_lcm ${${PEOJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(lcm_server $ENV{UNITREE_LEGGED_SDK_PATH}/examples/lcm_server.cpp)
target_link_libraries(lcm_server ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_dependencies(lcm_server ${${PEOJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(lcm_server_3_2 $ENV{UNITREE_LEGGED_SDK_PATH}/examples/lcm_server.cpp)
target_link_libraries(lcm_server_3_2 ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_dependencies(lcm_server_3_2 ${${PEOJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(lcm_server_3_1 $ENV{ALIENGO_SDK_PATH}/examples/lcm_server.cpp)
target_link_libraries(lcm_server_3_1 ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_dependencies(lcm_server_3_1 ${${PEOJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

168
unitree_legged_real/include/convert.h
View File

@ -1,6 +1,7 @@
/*****************************************************************
Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved.
******************************************************************/
/************************************************************************
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
************************************************************************/
#ifndef _CONVERT_H_
#define _CONVERT_H_
@ -13,7 +14,12 @@
#include <unitree_legged_msgs/MotorCmd.h>
#include <unitree_legged_msgs/MotorState.h>
#include <unitree_legged_msgs/IMU.h>
#include "/home/bian/aliengo_sdk/include/aliengo_sdk/aliengo_sdk.hpp"
enum firmworkVersion{
V3_1,
V3_2
};
unitree_legged_msgs::IMU ToRos(UNITREE_LEGGED_SDK::IMU& lcm)
{
@ -35,6 +41,25 @@ unitree_legged_msgs::IMU ToRos(UNITREE_LEGGED_SDK::IMU& lcm)
return ros;
}
unitree_legged_msgs::IMU ToRos(aliengo::IMU& lcm){
unitree_legged_msgs::IMU ros;
ros.quaternion[0] = lcm.quaternion[0];
ros.quaternion[1] = lcm.quaternion[1];
ros.quaternion[2] = lcm.quaternion[2];
ros.quaternion[3] = lcm.quaternion[3];
ros.gyroscope[0] = lcm.gyroscope[0];
ros.gyroscope[1] = lcm.gyroscope[1];
ros.gyroscope[2] = lcm.gyroscope[2];
ros.accelerometer[0] = lcm.acceleration[0];
ros.accelerometer[1] = lcm.acceleration[1];
ros.accelerometer[2] = lcm.acceleration[2];
ros.rpy[0] = lcm.rpy[0];
ros.rpy[1] = lcm.rpy[1];
ros.rpy[2] = lcm.rpy[2];
ros.temperature = lcm.temp;
return ros;
}
unitree_legged_msgs::MotorState ToRos(UNITREE_LEGGED_SDK::MotorState& lcm)
{
unitree_legged_msgs::MotorState ros;
@ -52,7 +77,24 @@ unitree_legged_msgs::MotorState ToRos(UNITREE_LEGGED_SDK::MotorState& lcm)
return ros;
}
UNITREE_LEGGED_SDK::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros)
unitree_legged_msgs::MotorState ToRos(aliengo::MotorState& lcm){
unitree_legged_msgs::MotorState ros;
ros.mode = lcm.mode;
ros.q = lcm.position;
ros.dq = lcm.velocity;
ros.ddq = 0;
ros.tauEst = lcm.torque;
ros.q_raw = 0;
ros.dq_raw = 0;
ros.ddq_raw = 0;
ros.temperature = lcm.temperature;
// printf("dq: %f\n", lcm.velocity);
return ros;
}
UNITREE_LEGGED_SDK::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, UNITREE_LEGGED_SDK::MotorCmd lcmType)
{
UNITREE_LEGGED_SDK::MotorCmd lcm;
lcm.mode = ros.mode;
@ -67,6 +109,17 @@ UNITREE_LEGGED_SDK::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros)
return lcm;
}
aliengo::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, aliengo::MotorCmd lcmType){
aliengo::MotorCmd lcm;
lcm.mode = ros.mode;
lcm.position = ros.q;
lcm.velocity = ros.dq;
lcm.positionStiffness = ros.Kp;
lcm.velocityStiffness = ros.Kd;
lcm.torque = ros.tau;
return lcm;
}
unitree_legged_msgs::LowState ToRos(UNITREE_LEGGED_SDK::LowState& lcm)
{
unitree_legged_msgs::LowState ros;
@ -92,7 +145,31 @@ unitree_legged_msgs::LowState ToRos(UNITREE_LEGGED_SDK::LowState& lcm)
return ros;
}
UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros)
unitree_legged_msgs::LowState ToRos(aliengo::LowState& lcm){
unitree_legged_msgs::LowState ros;
ros.levelFlag = lcm.levelFlag;
ros.commVersion = 0;
ros.robotID = 0;
ros.SN = 0;
ros.bandWidth = 0;
ros.imu = ToRos(lcm.imu);
for(int i = 0; i < 20; i++){
ros.motorState[i] = ToRos(lcm.motorState[i]);
}
for(int i = 0; i < 4; i++){
ros.footForce[i] = lcm.footForce[i];
ros.footForceEst[i] = 0;
}
ros.tick = lcm.tick;
for(int i = 0; i < 40; i++){
ros.wirelessRemote[i] = lcm.wirelessRemote[i];
}
ros.crc = lcm.crc;
return ros;
}
UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, UNITREE_LEGGED_SDK::LowCmd lcmType)
{
UNITREE_LEGGED_SDK::LowCmd lcm;
lcm.levelFlag = ros.levelFlag;
@ -101,7 +178,7 @@ UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros)
lcm.SN = ros.SN;
lcm.bandWidth = ros.bandWidth;
for(int i = 0; i<20; i++){
lcm.motorCmd[i] = ToLcm(ros.motorCmd[i]);
lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]);
}
for(int i = 0; i<4; i++){
lcm.led[i].r = ros.led[i].r;
@ -116,6 +193,24 @@ UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros)
return lcm;
}
aliengo::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, aliengo::LowCmd lcmType){
aliengo::LowCmd lcm;
lcm.levelFlag = ros.levelFlag;
for(int i = 0; i < 20; i++){
lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]);
}
for(int i = 0; i < 4; i++){
lcm.led[i].r = ros.led[i].r;
lcm.led[i].g = ros.led[i].g;
lcm.led[i].b = ros.led[i].b;
}
for(int i = 0; i < 40; i++){
lcm.wirelessRemote[i] = ros.wirelessRemote[i];
}
lcm.crc = ros.crc;
return lcm;
}
unitree_legged_msgs::HighState ToRos(UNITREE_LEGGED_SDK::HighState& lcm)
{
unitree_legged_msgs::HighState ros;
@ -152,7 +247,42 @@ unitree_legged_msgs::HighState ToRos(UNITREE_LEGGED_SDK::HighState& lcm)
return ros;
}
UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros)
unitree_legged_msgs::HighState ToRos(aliengo::HighState& lcm){
unitree_legged_msgs::HighState ros;
ros.levelFlag = lcm.levelFlag;
ros.commVersion = 0;
ros.robotID = 0;
ros.SN = 0;
ros.bandWidth = 0;
ros.mode = lcm.mode;
ros.imu = ToRos(lcm.imu);
ros.forwardSpeed = lcm.forwardSpeed;
ros.sideSpeed = lcm.sideSpeed;
ros.rotateSpeed = lcm.rotateSpeed;
ros.bodyHeight = lcm.bodyHeight;
ros.updownSpeed = lcm.updownSpeed;
ros.forwardPosition = lcm.forwardPosition.x;
ros.sidePosition = lcm.sidePosition.y;
for(int i = 0; i < 4; i++){
ros.footPosition2Body[i].x = lcm.footPosition2Body[i].x;
ros.footPosition2Body[i].y = lcm.footPosition2Body[i].y;
ros.footPosition2Body[i].z = lcm.footPosition2Body[i].z;
ros.footSpeed2Body[i].x = lcm.footSpeed2Body[i].x;
ros.footSpeed2Body[i].y = lcm.footSpeed2Body[i].y;
ros.footSpeed2Body[i].z = lcm.footSpeed2Body[i].z;
ros.footForce[i] = lcm.footForce[i];
ros.footForceEst[i] = 0;
}
ros.tick = lcm.tick;
for(int i = 0; i<40; i++){
ros.wirelessRemote[i] = lcm.wirelessRemote[i];
}
ros.reserve = 0;
ros.crc = lcm.crc;
return ros;
}
UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, UNITREE_LEGGED_SDK::HighCmd lcmType)
{
UNITREE_LEGGED_SDK::HighCmd lcm;
lcm.levelFlag = ros.levelFlag;
@ -183,5 +313,29 @@ UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros)
return lcm;
}
aliengo::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, aliengo::HighCmd lcmType){
aliengo::HighCmd lcm;
lcm.levelFlag = ros.levelFlag;
lcm.mode = ros.mode;
lcm.forwardSpeed = ros.forwardSpeed;
lcm.sideSpeed = ros.sideSpeed;
lcm.rotateSpeed = ros.rotateSpeed;
lcm.bodyHeight = ros.bodyHeight;
lcm.footRaiseHeight = ros.footRaiseHeight;
lcm.yaw = ros.yaw;
lcm.pitch = ros.pitch;
lcm.roll = ros.roll;
for(int i = 0; i < 4; i++){
lcm.led[i].r = ros.led[i].r;
lcm.led[i].g = ros.led[i].g;
lcm.led[i].b = ros.led[i].b;
}
for(int i = 0; i < 40; i++){
lcm.wirelessRemote[i] = ros.wirelessRemote[i];
}
lcm.crc = ros.crc;
return lcm;
}
#endif

5
unitree_legged_real/launch/real.launch
View File

@ -1,11 +1,12 @@
<launch>
<arg name="rname" default="a1"/>
<arg name="ctrl_level" default="highlevel"/>
<arg name="firmwork" default="3_2"/>
<node pkg="unitree_legged_real" type="lcm_server" name="node_lcm_server"
<node pkg="unitree_legged_real" type="lcm_server_$(arg firmwork)" name="node_lcm_server"
respawn="false" output="screen" args="$(arg rname) $(arg ctrl_level)" />
<param name="robot_name" value="$(arg rname)"/>
<param name="control_level" value="$(arg ctrl_level)"/>
<param name="firmwork" value="$(arg firmwork)"/>
</launch>

70
unitree_legged_real/src/exe/position_mode.cpp
View File

@ -4,25 +4,26 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
************************************************************************/
#include <ros/ros.h>
#include <string>
#include <pthread.h>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <unitree_legged_msgs/LowCmd.h>
#include <unitree_legged_msgs/LowState.h>
#include "unitree_legged_sdk/unitree_legged_sdk.h"
#include "aliengo_sdk/aliengo_sdk.hpp"
#include "convert.h"
using namespace UNITREE_LEGGED_SDK;
class Custom
template<typename TLCM>
void* update_loop(void* param)
{
public:
Custom() {}
void LCMRecv();
LCM roslcm;
};
void Custom::LCMRecv()
{
roslcm.Recv();
TLCM *data = (TLCM *)param;
while(ros::ok){
data->Recv();
usleep(2000);
}
}
double jointLinearInterpolation(double initPos, double targetPos, double rate)
@ -33,19 +34,17 @@ double jointLinearInterpolation(double initPos, double targetPos, double rate)
return p;
}
int main(int argc, char *argv[])
template<typename TCmd, typename TState, typename TLCM>
int mainHelper(int argc, char *argv[])
{
std::cout << "WARNING: Control level is set to LOW-level." << std::endl
<< "Make sure the robot is hung up." << std::endl
<< "Press Enter to continue..." << std::endl;
std::cin.ignore();
ros::init(argc, argv, "position_ros_mode");
ros::NodeHandle n;
ros::Rate loop_rate(500);
Custom custom;
SetLevel(LOWLEVEL); // must have this
long motiontime = 0;
int rate_count = 0;
int sin_count = 0;
@ -54,17 +53,18 @@ int main(int argc, char *argv[])
float sin_mid_q[3] = {0.0, 1.2, -2.0};
float Kp[3] = {0};
float Kd[3] = {0};
LowCmd SendLowLCM = {0};
LowState RecvLowLCM = {0};
TCmd SendLowLCM = {0};
TState RecvLowLCM = {0};
unitree_legged_msgs::LowCmd SendLowROS;
unitree_legged_msgs::LowState RecvLowROS;
TLCM roslcm;
bool initiated_flag = false; // initiate need time
int count = 0;
custom.roslcm.SubscribeState();
roslcm.SubscribeState();
LoopFunc loop_lcm("LCM_Recv", 0.002, 3, boost::bind(&Custom::LCMRecv, &custom));
loop_lcm.start();
pthread_t tid;
pthread_create(&tid, NULL, update_loop<TLCM>, &roslcm);
SendLowROS.levelFlag = LOWLEVEL;
for(int i = 0; i<12; i++){
@ -77,8 +77,9 @@ int main(int argc, char *argv[])
}
while (ros::ok()){
custom.roslcm.Get(RecvLowLCM);
roslcm.Get(RecvLowLCM);
RecvLowROS = ToRos(RecvLowLCM);
printf("FR_2 position: %f\n", RecvLowROS.motorState[FR_2].q);
if(initiated_flag == true){
motiontime++;
@ -144,8 +145,8 @@ int main(int argc, char *argv[])
}
SendLowLCM = ToLcm(SendLowROS);
custom.roslcm.Send(SendLowLCM);
SendLowLCM = ToLcm(SendLowROS, SendLowLCM);
roslcm.Send(SendLowLCM);
ros::spinOnce();
loop_rate.sleep();
@ -157,3 +158,26 @@ int main(int argc, char *argv[])
}
return 0;
}
int main(int argc, char *argv[]){
ros::init(argc, argv, "position_ros_mode");
std::string firmwork;
ros::param::get("/firmwork", firmwork);
if(firmwork == "3_1"){
aliengo::Control control(aliengo::LOWLEVEL);
mainHelper<aliengo::LowCmd, aliengo::LowState, aliengo::LCM>(argc, argv);
}
else if(firmwork == "3_2"){
std::string robot_name;
UNITREE_LEGGED_SDK::LeggedType rname;
ros::param::get("/robot_name", robot_name);
if(strcasecmp(robot_name.c_str(), "A1") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::A1;
else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL);
mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv);
}
}

51
unitree_legged_real/src/exe/torque_mode.cpp
View File

@ -10,52 +10,44 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
#include <unitree_legged_msgs/LowCmd.h>
#include <unitree_legged_msgs/LowState.h>
#include "unitree_legged_sdk/unitree_legged_sdk.h"
#include "aliengo_sdk/aliengo_sdk.hpp"
#include "convert.h"
using namespace UNITREE_LEGGED_SDK;
template<typename TLCM>
void* update_loop(void* param)
{
LCM *data = (LCM *)param;
TLCM *data = (TLCM *)param;
while(ros::ok){
data->Recv();
usleep(2000);
}
}
int main(int argc, char *argv[])
template<typename TCmd, typename TState, typename TLCM>
int mainHelper(int argc, char *argv[])
{
std::cout << "WARNING: Control level is set to LOW-level." << std::endl
<< "Make sure the robot is hung up." << std::endl
<< "Press Enter to continue..." << std::endl;
std::cin.ignore();
ros::init(argc, argv, "torque_ros_mode");
ros::NodeHandle n;
ros::Rate loop_rate(500);
std::string robot_name;
LeggedType rname;
ros::param::get("/robot_name", robot_name);
if(strcasecmp(robot_name.c_str(), "A1") == 0)
rname = LeggedType::A1;
else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
rname = LeggedType::Aliengo;
Control control(rname, LOWLEVEL);
long motiontime=0;
float torque = 0;
LowCmd SendLowLCM = {0};
LowState RecvLowLCM = {0};
TCmd SendLowLCM = {0};
TState RecvLowLCM = {0};
unitree_legged_msgs::LowCmd SendLowROS;
unitree_legged_msgs::LowState RecvLowROS;
LCM roslcm;
TLCM roslcm;
roslcm.SubscribeState();
pthread_t tid;
pthread_create(&tid, NULL, update_loop, &roslcm);
pthread_create(&tid, NULL, update_loop<TLCM>, &roslcm);
SendLowROS.levelFlag = LOWLEVEL;
for(int i = 0; i<12; i++){
@ -85,10 +77,33 @@ int main(int argc, char *argv[])
SendLowROS.motorCmd[FL_1].Kd = 0;
SendLowROS.motorCmd[FL_1].tau = torque;
}
SendLowLCM = ToLcm(SendLowROS);
SendLowLCM = ToLcm(SendLowROS, SendLowLCM);
roslcm.Send(SendLowLCM);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
int main(int argc, char *argv[]){
ros::init(argc, argv, "torque_ros_mode");
std::string firmwork;
ros::param::get("/firmwork", firmwork);
if(firmwork == "3_1"){
aliengo::Control control(aliengo::LOWLEVEL);
mainHelper<aliengo::LowCmd, aliengo::LowState, aliengo::LCM>(argc, argv);
}
else if(firmwork == "3_2"){
std::string robot_name;
UNITREE_LEGGED_SDK::LeggedType rname;
ros::param::get("/robot_name", robot_name);
if(strcasecmp(robot_name.c_str(), "A1") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::A1;
else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL);
mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv);
}
}

52
unitree_legged_real/src/exe/velocity_mode.cpp
View File

@ -11,54 +11,46 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
#include <unitree_legged_msgs/LowCmd.h>
#include <unitree_legged_msgs/LowState.h>
#include "unitree_legged_sdk/unitree_legged_sdk.h"
#include "aliengo_sdk/aliengo_sdk.hpp"
#include "convert.h"
using namespace UNITREE_LEGGED_SDK;
template<typename TLCM>
void* update_loop(void* param)
{
LCM *data = (LCM *)param;
TLCM *data = (TLCM *)param;
while(ros::ok){
data->Recv();
usleep(2000);
}
}
int main(int argc, char *argv[])
template<typename TCmd, typename TState, typename TLCM>
int mainHelper(int argc, char *argv[])
{
std::cout << "WARNING: Control level is set to LOW-level." << std::endl
<< "Make sure the robot is hung up." << std::endl
<< "Press Enter to continue..." << std::endl;
std::cin.ignore();
ros::init(argc, argv, "velocity_ros_mode");
ros::NodeHandle n;
ros::Rate loop_rate(500);
std::string robot_name;
LeggedType rname;
ros::param::get("/robot_name", robot_name);
if(strcasecmp(robot_name.c_str(), "A1") == 0)
rname = LeggedType::A1;
else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
rname = LeggedType::Aliengo;
Control control(rname, LOWLEVEL);
long motiontime=0;
float Kv = 1.5;
float speed = 0;
unsigned long int Tpi =0;
LowCmd SendLowLCM = {0};
LowState RecvLowLCM = {0};
TCmd SendLowLCM = {0};
TState RecvLowLCM = {0};
unitree_legged_msgs::LowCmd SendLowROS;
unitree_legged_msgs::LowState RecvLowROS;
LCM roslcm;
TLCM roslcm;
roslcm.SubscribeState();
pthread_t tid;
pthread_create(&tid, NULL, update_loop, &roslcm);
pthread_create(&tid, NULL, update_loop<TLCM>, &roslcm);
SendLowROS.levelFlag = LOWLEVEL;
for(int i = 0; i<12; i++){
@ -87,10 +79,34 @@ int main(int argc, char *argv[])
// try 1 or 3
speed = 3 * sin(4*M_PI*Tpi/1000.0);
}
SendLowLCM = ToLcm(SendLowROS);
SendLowLCM = ToLcm(SendLowROS, SendLowLCM);
roslcm.Send(SendLowLCM);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
int main(int argc, char *argv[]){
ros::init(argc, argv, "velocity_ros_mode");
std::string firmwork;
ros::param::get("/firmwork", firmwork);
if(firmwork == "3_1"){
aliengo::Control control(aliengo::LOWLEVEL);
mainHelper<aliengo::LowCmd, aliengo::LowState, aliengo::LCM>(argc, argv);
}
else if(firmwork == "3_2"){
std::string robot_name;
UNITREE_LEGGED_SDK::LeggedType rname;
ros::param::get("/robot_name", robot_name);
if(strcasecmp(robot_name.c_str(), "A1") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::A1;
else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL);
mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv);
}
}

44
unitree_legged_real/src/exe/walk_mode.cpp
View File

@ -5,47 +5,50 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
#include <ros/ros.h>
#include <pthread.h>
#include <string>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <unitree_legged_msgs/HighCmd.h>
#include <unitree_legged_msgs/HighState.h>
#include "unitree_legged_sdk/unitree_legged_sdk.h"
#include "aliengo_sdk/aliengo_sdk.hpp"
#include "convert.h"
using namespace UNITREE_LEGGED_SDK;
// using namespace UNITREE_LEGGED_SDK;
template<typename TLCM>
void* update_loop(void* param)
{
LCM *data = (LCM *)param;
TLCM *data = (TLCM *)param;
while(ros::ok){
data->Recv();
usleep(2000);
}
}
int main(int argc, char *argv[])
template<typename TCmd, typename TState, typename TLCM>
int mainHelper(int argc, char *argv[])
{
std::cout << "WARNING: Control level is set to HIGH-level." << std::endl
<< "Make sure the robot is standing on the ground." << std::endl
<< "Press Enter to continue..." << std::endl;
std::cin.ignore();
ros::init(argc, argv, "walk_ros_mode");
ros::NodeHandle n;
ros::Rate loop_rate(500);
// SetLevel(HIGHLEVEL);
long motiontime = 0;
HighCmd SendHighLCM = {0};
HighState RecvHighLCM = {0};
TCmd SendHighLCM = {0};
TState RecvHighLCM = {0};
unitree_legged_msgs::HighCmd SendHighROS;
unitree_legged_msgs::HighState RecvHighROS;
LCM roslcm;
TLCM roslcm;
roslcm.SubscribeState();
pthread_t tid;
pthread_create(&tid, NULL, update_loop, &roslcm);
pthread_create(&tid, NULL, update_loop<TLCM>, &roslcm);
while (ros::ok()){
motiontime = motiontime+2;
@ -121,7 +124,7 @@ int main(int argc, char *argv[])
SendHighROS.mode = 1;
}
SendHighLCM = ToLcm(SendHighROS);
SendHighLCM = ToLcm(SendHighROS, SendHighLCM);
roslcm.Send(SendHighLCM);
ros::spinOnce();
loop_rate.sleep();
@ -129,3 +132,26 @@ int main(int argc, char *argv[])
return 0;
}
int main(int argc, char *argv[]){
ros::init(argc, argv, "walk_ros_mode");
std::string firmwork;
ros::param::get("/firmwork", firmwork);
if(firmwork == "3_1"){
aliengo::Control control(aliengo::HIGHLEVEL);
mainHelper<aliengo::HighCmd, aliengo::HighState, aliengo::LCM>(argc, argv);
}
else if(firmwork == "3_2"){
std::string robot_name;
UNITREE_LEGGED_SDK::LeggedType rname;
ros::param::get("/robot_name", robot_name);
if(strcasecmp(robot_name.c_str(), "A1") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::A1;
else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL);
mainHelper<UNITREE_LEGGED_SDK::HighCmd, UNITREE_LEGGED_SDK::HighState, UNITREE_LEGGED_SDK::LCM>(argc, argv);
}
}