/***************************************************************** Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. ******************************************************************/ #include "unitree_legged_sdk/unitree_legged_sdk.h" #include #include #include #include using namespace std; using namespace UNITREE_LEGGED_SDK; // low cmd constexpr uint16_t TARGET_PORT = 8007; constexpr uint16_t LOCAL_PORT = 8082; constexpr char TARGET_IP[] = "192.168.123.10"; // target IP address const int LOW_CMD_LENGTH = 610; const int LOW_STATE_LENGTH = 771; class Custom { public: Custom(uint8_t level) : safe(LeggedType::Aliengo), udp(LOCAL_PORT, TARGET_IP,TARGET_PORT, LOW_CMD_LENGTH, LOW_STATE_LENGTH) { udp.InitCmdData(cmd); cmd.levelFlag = LOWLEVEL; } void UDPRecv(); void UDPSend(); void RobotControl(); Safety safe; UDP udp; LowCmd cmd = {0}; LowState state = {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 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 }; 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; return p; } 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 = -1.6f; // cmd.motorCmd[FL_0].tau = +1.0f; // cmd.motorCmd[RR_0].tau = -1.0f; // cmd.motorCmd[RL_0].tau = +1.0f; // if( motiontime >= 100){ if (motiontime >= 0) { // first, get record initial position // if( motiontime >= 100 && motiontime < 500){ 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) { 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; 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 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.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] + sin_joint1; qDes[2] = sin_mid_q[2] + sin_joint2; // qDes[2] = sin_mid_q[2]; } cmd.motorCmd[FR_0].q = qDes[0]; 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 = -1.6f; 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].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); // 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)); loop_udpSend.start(); loop_udpRecv.start(); loop_control.start(); while (1) { sleep(10); }; return 0; }