66 lines
1.4 KiB
C++
66 lines
1.4 KiB
C++
|
#include <unitree/robot/go2/sport/sport_client.hpp>
|
||
|
|
#include <unitree/common/thread/recurrent_thread.hpp>
|
||
|
|
#include <math.h>
|
||
|
|
|
||
|
|
class Custom
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
Custom() {}
|
||
|
|
void control();
|
||
|
|
|
||
|
|
unitree::robot::go2::SportClient tc;
|
||
|
|
|
||
|
|
int c = 0;
|
||
|
|
float dt = 0.002; // 0.001~0.01
|
||
|
|
};
|
||
|
|
|
||
|
|
void Custom::control()
|
||
|
|
{
|
||
|
|
c++;
|
||
|
|
|
||
|
|
int32_t ret;
|
||
|
|
|
||
|
|
float vx = 0.3;
|
||
|
|
float delta = 0.06;
|
||
|
|
static float count = 0;
|
||
|
|
count += dt;
|
||
|
|
std::vector<unitree::robot::go2::PathPoint> path;
|
||
|
|
for (int i=0; i<30; i++) {
|
||
|
|
unitree::robot::go2::PathPoint p;
|
||
|
|
float var = (count + i * delta);
|
||
|
|
p.timeFromStart = i * delta;
|
||
|
|
p.x = vx * var;
|
||
|
|
p.y = 0.6 * sin(M_PI * vx * var);
|
||
|
|
p.yaw = 2*0.6 * vx * M_PI * cos(M_PI * vx * var);
|
||
|
|
p.vx = vx;
|
||
|
|
p.vy = M_PI * vx * (0.6 * cos(M_PI * vx * var));
|
||
|
|
p.vyaw = - M_PI * vx*2*0.6 * vx * M_PI * sin(M_PI * vx * var);
|
||
|
|
path.push_back(p);
|
||
|
|
}
|
||
|
|
|
||
|
|
ret = tc.TrajectoryFollow(path);
|
||
|
|
if(ret != 0){
|
||
|
|
std::cout << "Call TrajectoryFollow: " << ret << std::endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
std::cout << c << std::endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
int main(int argc, char** argv)
|
||
|
|
{
|
||
|
|
unitree::robot::ChannelFactory::Instance()->Init(0);
|
||
|
|
|
||
|
|
Custom custom;
|
||
|
|
custom.tc.SetTimeout(10.0f);
|
||
|
|
custom.tc.Init();
|
||
|
|
|
||
|
|
unitree::common::ThreadPtr threadPtr = unitree::common::CreateRecurrentThread(custom.dt * 1000000, std::bind(&Custom::control, &custom));
|
||
|
|
|
||
|
|
while (1)
|
||
|
|
{
|
||
|
|
sleep(10);
|
||
|
|
}
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|