Skip to content

Commit

Permalink
v3.8.1 B1 SDK add python interface and arm lib
Browse files Browse the repository at this point in the history
  • Loading branch information
@Bin-Ro
Bin-Ro committed Nov 21, 2022
1 parent 7a09517 commit 89e1ca3
Show file tree
Hide file tree
Showing 25 changed files with 557 additions and 361 deletions.
11 changes: 4 additions & 7 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,9 @@ cmake_minimum_required(VERSION 2.8.3)
project(unitree_legged_sdk)

include_directories(include)
include_directories(./)

# add_compile_options(-std=c++11)
add_compile_options(-std=c++14)

# check arch and os
Expand Down Expand Up @@ -36,10 +38,5 @@ add_executable(example_joystick example/example_joystick.cpp)
target_link_libraries(example_joystick ${EXTRA_LIBS})


# install
install(TARGETS
example_position example_velocity example_torque example_walk example_joystick
DESTINATION bin/unitree)
install(DIRECTORY lib/cpp/${ARCH}/
DESTINATION lib/unitree
USE_SOURCE_PERMISSIONS)


17 changes: 9 additions & 8 deletions README.md
View file
Original file line number Diff line number Diff line change
@@ -1,22 +1,23 @@
# v3.8.0
# v3.8.1
The unitree_legged_sdk is mainly used for communication between PC and Controller board.
It also can be used in other PCs with UDP.

### Notice
support robot: Go1
support robot: B1

not support robot: Laikago, Aliengo, A1. (Check release [v3.3.1](https://github.com/unitreerobotics/unitree_legged_sdk/releases/tag/v3.3.1) for support)

### Dependencies
* [Unitree](https://www.unitree.com/go1_update)
### Sport Mode
```bash
Legged_sport >= v1.36.0
Legged_sport >= v3.24
firmware H0.1.7 >= v0.1.35
H0.1.9 >= v0.1.35
```
* [Boost](http://www.boost.org) (version 1.5.4 or higher)
* [CMake](http://www.cmake.org) (version 2.8.3 or higher)
* [g++](https://gcc.gnu.org/) (version 8.3.0 or higher)

### Dependencies
* [Boost](http://www.boost.org) (version 1.71.0 or higher)
* [CMake](http://www.cmake.org) (version 3.16.3 or higher)
* [g++](https://gcc.gnu.org/) (version 9.4.0 or higher)


### Build
Expand Down
2 changes: 1 addition & 1 deletion example/example_joystick.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ class Custom
{
public:
Custom(uint8_t level):
safe(LeggedType::Go1),
safe(LeggedType::B1),
udp(level, 8090, "192.168.123.10", 8007){
udp.InitCmdData(cmd);
}
Expand Down
105 changes: 58 additions & 47 deletions example/example_position.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,9 @@ using namespace UNITREE_LEGGED_SDK;
class Custom
{
public:
Custom(uint8_t level):
safe(LeggedType::Go1),
udp(level, 8090, "192.168.123.10", 8007) {
Custom(uint8_t level) : safe(LeggedType::B1),
udp(level, 8090, "192.168.123.10", 8007)
{
udp.InitCmdData(cmd);
}
void UDPRecv();
Expand All @@ -27,80 +27,94 @@ class Custom
UDP udp;
LowCmd cmd = {0};
LowState state = {0};
float qInit[3]={0};
float qDes[3]={0};
float qInit[3] = {0};
float qDes[3] = {0};
float sin_mid_q[3] = {0.0, 1.2, -2.0};
float Kp[3] = {0};
float Kp[3] = {0};
float Kd[3] = {0};
double time_consume = 0;
int rate_count = 0;
int sin_count = 0;
int motiontime = 0;
float dt = 0.002; // 0.001~0.01
float dt = 0.002; // 0.001~0.01
};

void Custom::UDPRecv()
{
{
udp.Recv();
}

void Custom::UDPSend()
{
{
udp.Send();
}

double jointLinearInterpolation(double initPos, double targetPos, double rate)
{
double p;
rate = std::min(std::max(rate, 0.0), 1.0);
p = initPos*(1-rate) + targetPos*rate;
p = initPos * (1 - rate) + targetPos * rate;
return p;
}

void Custom::RobotControl()
void Custom::RobotControl()
{
motiontime++;
udp.GetRecv(state);
// printf("%d %f\n", motiontime, state.motorState[FR_2].q);
printf("%d %f %f\n", motiontime, state.motorState[FR_1].q, state.motorState[FR_1].dq);

// gravity compensation
cmd.motorCmd[FR_0].tau = -0.65f;
cmd.motorCmd[FL_0].tau = +0.65f;
cmd.motorCmd[RR_0].tau = -0.65f;
cmd.motorCmd[RL_0].tau = +0.65f;
cmd.motorCmd[FR_0].tau = -5.0f;
// cmd.motorCmd[FL_0].tau = +0.65f;
// cmd.motorCmd[RR_0].tau = -0.65f;
// cmd.motorCmd[RL_0].tau = +0.65f;

// if( motiontime >= 100){
if( motiontime >= 0){
if (motiontime >= 0)
{
// first, get record initial position
// if( motiontime >= 100 && motiontime < 500){
if( motiontime >= 0 && motiontime < 10){
if (motiontime >= 0 && motiontime < 10)
{
qInit[0] = state.motorState[FR_0].q;
qInit[1] = state.motorState[FR_1].q;
qInit[2] = state.motorState[FR_2].q;
}
// second, move to the origin point of a sine movement with Kp Kd
// if( motiontime >= 500 && motiontime < 1500){
if( motiontime >= 10 && motiontime < 400){
if (motiontime >= 10 && motiontime < 400)
{
rate_count++;
double rate = rate_count/200.0; // needs count to 200
Kp[0] = 5.0; Kp[1] = 5.0; Kp[2] = 5.0;
Kd[0] = 1.0; Kd[1] = 1.0; Kd[2] = 1.0;
// Kp[0] = 20.0; Kp[1] = 20.0; Kp[2] = 20.0;
// Kd[0] = 2.0; Kd[1] = 2.0; Kd[2] = 2.0;

double rate = rate_count / 200.0; // needs count to 200
// Kp[0] = 5.0; Kp[1] = 5.0; Kp[2] = 5.0;
// Kd[0] = 1.0; Kd[1] = 1.0; Kd[2] = 1.0;
Kp[0] = 20.0;
Kp[1] = 20.0;
Kp[2] = 20.0;
Kd[0] = 2.0;
Kd[1] = 2.0;
Kd[2] = 2.0;

qDes[0] = jointLinearInterpolation(qInit[0], sin_mid_q[0], rate);
qDes[1] = jointLinearInterpolation(qInit[1], sin_mid_q[1], rate);
qDes[2] = jointLinearInterpolation(qInit[2], sin_mid_q[2], rate);
}
double sin_joint1, sin_joint2;
// last, do sine wave
if( motiontime >= 400){
float freq_Hz = 1;
// float freq_Hz = 5;
float freq_rad = freq_Hz * 2 * M_PI;
float t = dt * sin_count;
if (motiontime >= 400)
{
sin_count++;
sin_joint1 = 0.6 * sin(3*M_PI*sin_count/1000.0);
sin_joint2 = -0.6 * sin(1.8*M_PI*sin_count/1000.0);
// sin_joint1 = 0.6 * sin(3*M_PI*sin_count/1000.0);
// sin_joint2 = -0.9 * sin(3*M_PI*sin_count/1000.0);
sin_joint1 = 0.6 * sin(t * freq_rad);
sin_joint2 = -0.9 * sin(t * freq_rad);
qDes[0] = sin_mid_q[0];
qDes[1] = sin_mid_q[1];
qDes[1] = sin_mid_q[1] + sin_joint1;
qDes[2] = sin_mid_q[2] + sin_joint2;
// qDes[2] = sin_mid_q[2];
}
Expand All @@ -109,55 +123,52 @@ void Custom::RobotControl()
cmd.motorCmd[FR_0].dq = 0;
cmd.motorCmd[FR_0].Kp = Kp[0];
cmd.motorCmd[FR_0].Kd = Kd[0];
cmd.motorCmd[FR_0].tau = -0.65f;
cmd.motorCmd[FR_0].tau = -4.0f;

cmd.motorCmd[FR_1].q = qDes[1];
cmd.motorCmd[FR_1].dq = 0;
cmd.motorCmd[FR_1].Kp = Kp[1];
cmd.motorCmd[FR_1].Kd = Kd[1];
cmd.motorCmd[FR_1].tau = 0.0f;

cmd.motorCmd[FR_2].q = qDes[2];
cmd.motorCmd[FR_2].q = qDes[2];
cmd.motorCmd[FR_2].dq = 0;
cmd.motorCmd[FR_2].Kp = Kp[2];
cmd.motorCmd[FR_2].Kd = Kd[2];
cmd.motorCmd[FR_2].tau = 0.0f;

// cmd.motorCmd[FR_2].tau = 2 * sin(t*freq_rad);
}

if(motiontime > 10){
safe.PositionLimit(cmd);
int res1 = safe.PowerProtect(cmd, state, 1);
// You can uncomment it for position protection
// int res2 = safe.PositionProtect(cmd, state, 10);
if(res1 < 0) exit(-1);
}
// if(motiontime > 10){
// // safe.PositionLimit(cmd);
// safe.PowerProtect(cmd, state, 1);
// // safe.PositionProtect(cmd, state, 0.087);
// }

udp.SetSend(cmd);

}


int main(void)
{
std::cout << "Communication level is set to LOW-level." << std::endl
<< "WARNING: Make sure the robot is hung up." << std::endl
<< "Press Enter to continue..." << std::endl;
std::cin.ignore();

Custom custom(LOWLEVEL);
// InitEnvironment();
LoopFunc loop_control("control_loop", custom.dt, boost::bind(&Custom::RobotControl, &custom));
LoopFunc loop_udpSend("udp_send", custom.dt, 3, boost::bind(&Custom::UDPSend, &custom));
LoopFunc loop_udpRecv("udp_recv", custom.dt, 3, boost::bind(&Custom::UDPRecv, &custom));
InitEnvironment();
LoopFunc loop_control("control_loop", custom.dt, boost::bind(&Custom::RobotControl, &custom));
LoopFunc loop_udpSend("udp_send", custom.dt, 3, boost::bind(&Custom::UDPSend, &custom));
LoopFunc loop_udpRecv("udp_recv", custom.dt, 3, boost::bind(&Custom::UDPRecv, &custom));

loop_udpSend.start();
loop_udpRecv.start();
loop_control.start();

while(1){
while (1)
{
sleep(10);
};

return 0;
return 0;
}
36 changes: 27 additions & 9 deletions example/example_torque.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ class Custom
{
public:
Custom(uint8_t level):
safe(LeggedType::Go1),
safe(LeggedType::B1),
udp(level, 8090, "192.168.123.10", 8007){
udp.InitCmdData(cmd);
}
Expand All @@ -27,6 +27,7 @@ class Custom
LowState state = {0};
int motiontime = 0;
float dt = 0.002; // 0.001~0.01
int sin_count = 0;
};

void Custom::UDPRecv()
Expand All @@ -43,28 +44,45 @@ void Custom::RobotControl()
{
motiontime++;
udp.GetRecv(state);
// printf("%d\n", motiontime);
printf("%d %f %f\n", motiontime, state.motorState[FR_1].q, state.motorState[FR_1].dq);
// gravity compensation
cmd.motorCmd[FR_0].tau = -0.65f;
cmd.motorCmd[FL_0].tau = +0.65f;
cmd.motorCmd[RR_0].tau = -0.65f;
cmd.motorCmd[RL_0].tau = +0.65f;
cmd.motorCmd[FR_0].tau = -5.0f;
// cmd.motorCmd[FL_0].tau = +0.65f;
// cmd.motorCmd[RR_0].tau = -0.65f;
// cmd.motorCmd[RL_0].tau = +0.65f;

// float freq_Hz = 1;
// float freq_Hz = 2;
// float freq_Hz = 5;
// float freq_rad = freq_Hz * 2* M_PI;
// float t = dt*sin_count;

if( motiontime >= 500){
sin_count++;
float torque = (0 - state.motorState[FR_1].q)*10.0f + (0 - state.motorState[FR_1].dq)*1.0f;
// float torque = (0 - state.motorState[FR_1].q)*20.0f + (0 - state.motorState[FR_1].dq)*2.0f;
// float torque = (0 - state.motorState[FR_1].q)*40.0f + (0 - state.motorState[FR_1].dq)*2.0f;
// float torque = 2 * sin(t*freq_rad);
if(torque > 5.0f) torque = 5.0f;
if(torque < -5.0f) torque = -5.0f;
// if(torque > 15.0f) torque = 15.0f;
// if(torque < -15.0f) torque = -15.0f;

// cmd.motorCmd[FR_2].q = PosStopF;
// cmd.motorCmd[FR_2].dq = VelStopF;
// cmd.motorCmd[FR_2].Kp = 0;
// cmd.motorCmd[FR_2].Kd = 0;
// cmd.motorCmd[FR_2].tau = torque;

cmd.motorCmd[FR_1].q = PosStopF;
cmd.motorCmd[FR_1].dq = VelStopF;
cmd.motorCmd[FR_1].Kp = 0;
cmd.motorCmd[FR_1].Kd = 0;
cmd.motorCmd[FR_1].tau = torque;

}
int res = safe.PowerProtect(cmd, state, 1);
if(res < 0) exit(-1);
// int res = safe.PowerProtect(cmd, state, 1);
// if(res < 0) exit(-1);

udp.SetSend(cmd);
}
Expand All @@ -77,7 +95,7 @@ int main(void)
std::cin.ignore();

Custom custom(LOWLEVEL);
// InitEnvironment();
InitEnvironment();
LoopFunc loop_control("control_loop", custom.dt, boost::bind(&Custom::RobotControl, &custom));
LoopFunc loop_udpSend("udp_send", custom.dt, 3, boost::bind(&Custom::UDPSend, &custom));
LoopFunc loop_udpRecv("udp_recv", custom.dt, 3, boost::bind(&Custom::UDPRecv, &custom));
Expand Down
Loading

0 comments on commit 89e1ca3

Please sign in to comment.