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

Update lowcmd_multirobots.cpp

This commit is contained in:
Agnel Wang 2023-07-13 17:26:18 +08:00
parent 337f4c2531
commit 0951110ccb
1 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
examples/lowcmd_multirobots.cpp
View File

@ -45,20 +45,26 @@ int main()
targetQ << 1, 0, 0, 0, 0, 0; targetQ << 1, 0, 0, 0, 0, 0;
int duration = 1000; int duration = 1000;
Timer timer(0.002);
for(int i(0); i<duration; i++) for(int i(0); i<duration; i++)
{ {
//robot1 //robot1
arm1->q = lastArm1Q*(1-(double)i/duration) + targetQ*((double)i/duration); arm1->q = lastArm1Q*(1-(double)i/duration) + targetQ*((double)i/duration);
arm1->qd = (targetQ-lastArm1Q)/(duration*0.002); arm1->qd = (targetQ-lastArm1Q)/(duration*0.002);
arm1->tau.setZero(); arm1->tau.setZero();
arm1->setArmCmd(arm1->q, arm1->qd, arm1->tau);
arm1->setGripperCmd(arm1->gripperQ, arm1->gripperW, arm1->gripperTau);
//robot2 //robot2
arm2->q = lastArm2Q*(1-(double)i/duration) + targetQ*((double)i/duration); arm2->q = lastArm2Q*(1-(double)i/duration) + targetQ*((double)i/duration);
arm2->qd = (targetQ-lastArm1Q)/(duration*0.002); arm2->qd = (targetQ-lastArm1Q)/(duration*0.002);
arm2->tau.setZero(); arm2->tau.setZero();
arm2->setArmCmd(arm2->q, arm2->qd, arm2->tau);
arm2->setGripperCmd(arm2->gripperQ, arm2->gripperW, arm2->gripperTau);
arm1->sendRecv(); arm1->sendRecv();
arm2->sendRecv(); arm2->sendRecv();
usleep(2000); timer.sleep();
} }
delete arm1; delete arm1;