📄 kick.cc
字号:
#include "TheStrut.h"
#include "../Globals.h"
void TheStrut::DoKick() {
inDefaultStance = false;
bool changePID = false;
if (motionType == LocomotionCommand::TP_PAW_KICK || motionType == LocomotionCommand::TP_HEAD_KICK_BACK_RIGHT || motionType == LocomotionCommand::TP_HEAD_KICK_BACK_LEFT/* || motionType == LocomotionCommand::TP_BACK_KICK_LEFT || motionType == LocomotionCommand::TP_BACK_KICK_RIGHT*/) changePID = true;
if (kickFrame == 0 && changePID) {
for (unsigned int i = 0; i < 2; i++) {
int j1 = 3 * i;
int j2 = 3 * i + 1;
int j3 = 3 * i + 2;
OPENR::EnableJointGain(bodyJointID[j1]);
OPENR::SetJointGain(bodyJointID[j1],J1_PGAIN*3, J1_IGAIN, J1_DGAIN ,PSHIFT, ISHIFT, DSHIFT);
OPENR::EnableJointGain(bodyJointID[j2]);
OPENR::SetJointGain(bodyJointID[j2],J2_PGAIN*3, J2_IGAIN, J1_DGAIN,PSHIFT, ISHIFT, DSHIFT);
OPENR::EnableJointGain(bodyJointID[j3]);
OPENR::SetJointGain(bodyJointID[j3],J3_PGAIN*3, J3_IGAIN, J1_DGAIN,PSHIFT, ISHIFT, DSHIFT);
}
}
int numFrames = maxKickFrames;
if ((kickFrame+maxKickFrames) >= kicks[kickIndex].frames.size()) {
numFrames = (int)(kicks[kickIndex].frames.size() - kickFrame);
}
long pan = sensorValues_[S_HEAD_PAN];
if (motionType == LocomotionCommand::TP_GRAB) {
long tempChange = (long)(ABS(pan) / (double)numFrames);
if (pan < 0) pan+=tempChange;
else pan-=tempChange;
}
if (numFrames == 0) {
isKicking = false;
kickFrame = 0.0;
kickIndex = -1;
motionType = -1;
return;
}
int bodyRegionID = FindFreeBodyRegionID();
int headRegionID = FindFreeHeadRegionID();
if (bodyRegionID != -1 && headRegionID != -1) {
// stuff for Rick:---
// cout << motionType << "," << jointValues_[S_RF_ROTATOR].value << "," << jointValues_[S_RF_ROTATOR].refValue << "," << jointValues_[S_RF_ROTATOR].pwmDuty << "," << bodyJointPrevPoints[0] << "," << jointValues_[S_RF_ABDUCTOR].value << "," << jointValues_[S_RF_ABDUCTOR].refValue << "," << jointValues_[S_RF_ABDUCTOR].pwmDuty << "," << bodyJointPrevPoints[1] << "," << jointValues_[S_RF_KNEE].value << "," << jointValues_[S_RF_KNEE].refValue << "," << jointValues_[S_RF_KNEE].pwmDuty << "," << bodyJointPrevPoints[2] << endl << flush;
for (int i = 0; i < NUM_BODY_JOINTS; i++) {
OCommandData* jointData = bodyVec[bodyRegionID]->GetData(i);
bodyValue[i] = (OJointCommandValue2*)jointData->value;
OCommandInfo* jointInfo = bodyVec[bodyRegionID]->GetInfo(i);
jointInfo->numFrames = numFrames;
}
if (kickUsesHead) {
for (int i = 0; i < NUM_HEAD_JOINTS; i++) {
OCommandData* jointData = headVec[headRegionID]->GetData(i);
headValue[i] = (OJointCommandValue2*)jointData->value;
OCommandInfo* jointInfo = headVec[headRegionID]->GetInfo(i);
jointInfo->numFrames = numFrames;
}
}
//cout << "TheStrut: (Kick) kickindex - " << kickIndex << ", motiontype - " << motionType << ", kickframe - " << kickFrame << " of " << kicks[kickIndex].frames.size() << endl << flush;
for (int j = 0; j < numFrames; j++) {
if (kickFrame >= kicks[kickIndex].frames.size()) {
cout << "TheStrut: CRITICAL numFrames calculation error in DoKick()" << endl << flush;
continue;
}
double frame1 = kicks[kickIndex].frames[(int)kickFrame][1];
double frame4 = kicks[kickIndex].frames[(int)kickFrame][4];
double frame7 = kicks[kickIndex].frames[(int)kickFrame][7];
if (motionType == LocomotionCommand::TP_HEAD_KICK_FORWARDS) {
double absTurn = ABS(turn);
absTurn = MIN(absTurn,110.0);
if (turn < 0 && (kicks[kickIndex].frames[(int)kickFrame][2] < 0)) {
kicks[kickIndex].frames[(int)kickFrame][4] = absTurn;
} else if (turn > 0) {
kicks[kickIndex].frames[(int)kickFrame][1] = -1*kicks[kickIndex].frames[(int)kickFrame][1];
if (kicks[kickIndex].frames[(int)kickFrame][2] < -20) {
kicks[kickIndex].frames[(int)kickFrame][7] = absTurn;
}
}
}
for (int k = 0; k < NUM_BODY_JOINTS; k++) {
SetBodyJoint(&bodyValue[k][j].value, DEG_TO_MICRO( kicks[kickIndex].frames[(int)kickFrame][k+3]), k);
}
if (kickUsesHead) {
for (int k = 0; k < NUM_HEAD_JOINTS; k++) {
if (motionType == LocomotionCommand::TP_GRAB && k == 1)
SetHeadJoint(&headValue[k][j].value, pan, k);
else
SetHeadJoint(&headValue[k][j].value, DEG_TO_MICRO( kicks[kickIndex].frames[(int)kickFrame][k]), k);
}
HeadCommand hc;
hc.Set(kicks[kickIndex].frames[(int)kickFrame][0],kicks[kickIndex].frames[(int)kickFrame][1], kicks[kickIndex].frames[(int)kickFrame][2],false);
lcq_.SetHeadCommand(hc);
}
// resetting the default value of the head if it was changed above
if (motionType == LocomotionCommand::TP_HEAD_KICK_FORWARDS) {
kicks[kickIndex].frames[(int)kickFrame][1] = frame1;
kicks[kickIndex].frames[(int)kickFrame][4] = frame4;
kicks[kickIndex].frames[(int)kickFrame][7] = frame7;
}
kickFrame=kickFrame+1.0;
}
// do odometry data kicks. these are hard coded, since only one kick does anything serious :)
LocomotionData ld;
// nothing by default
ld.deltaForward = 0.0;
ld.deltaLeft = 0.0;
ld.deltaTurn = 0.0;
double spinKickAmount = 70.0*((double)numFrames/(double)kicks[kickIndex].frames.size());
// spin kick right or slap right
if (motionType == LocomotionCommand::TP_SLAP_RIGHT_LOW || motionType == LocomotionCommand::TP_SLAP_RIGHT ) {
ld.deltaTurn = DEG_TO_RAD(-spinKickAmount);
// spin kick left or slap left
} else if (motionType == LocomotionCommand::TP_SLAP_LEFT_LOW || motionType == LocomotionCommand::TP_SLAP_LEFT) {
ld.deltaTurn = DEG_TO_RAD(spinKickAmount);
}
// send odometry data
lcq_.AddLocomotionData(ld);
if (kickFrame >= kicks[kickIndex].frames.size()) {
// FIXME outdated. removable. This is shit slow and if we ever do it again i'm going to snap (-CM)
/*
if (motionType == 24) {
if ( (MICRO_TO_DEG(ABS(sensorValues_[S_RF_ABDUCTOR] - bodyJointPrevPoints[1])) > 3.0) && (MICRO_TO_DEG(ABS(sensorValues_[S_LF_ABDUCTOR] - bodyJointPrevPoints[4])) > 3.0) ) {
lcq_.SetGrabSuccess(true);
cout << "Grab Success !" << endl << flush;
} else {
lcq_.SetGrabSuccess(false);
cout << "Grab Fail !" << endl << flush;
}
} */
if (changePID) {
for (unsigned int i = 0; i < 2; i++) {
int j1 = 3 * i;
int j2 = 3 * i + 1;
int j3 = 3 * i + 2;
OPENR::EnableJointGain(bodyJointID[j1]);
OPENR::SetJointGain(bodyJointID[j1],J1_PGAIN, J1_IGAIN, J1_DGAIN ,PSHIFT, ISHIFT, DSHIFT);
OPENR::EnableJointGain(bodyJointID[j2]);
OPENR::SetJointGain(bodyJointID[j2],J2_PGAIN, J2_IGAIN, J2_DGAIN,PSHIFT, ISHIFT, DSHIFT);
OPENR::EnableJointGain(bodyJointID[j3]);
OPENR::SetJointGain(bodyJointID[j3],J3_PGAIN, J3_IGAIN, J3_DGAIN,PSHIFT, ISHIFT, DSHIFT);
}
}
// kick is alll done.
timeParameter = 0.0;
isKicking = false;
kickFrame = 0.0;
kickIndex = -1;
motionType = -1;
}
sbjCommandVector->SetData(cmdBodyRegion[bodyRegionID]);
if (kickUsesHead) {
sbjCommandVector->SetData(cmdHeadRegion[headRegionID]);
}
}
}
/*
//Read kick data from file
void TheStrut::ReadKicks(char* filename) {
// clear current kicks vector. we can reload kicks.dat on the fly :)
for (int z=0; z < kicks.size(); z++) {
for (int y=0; y < kicks[z].frames.size(); y++) {
short* a = kicks[z].frames[y];
delete a;
}
kicks[z].frames.clear();
}
kicks.clear();
// Load file into memory then do parsing on it. an attempt to speed up
// the cfg+data reloading process.
printf("TheStrut: Loading kicks file %s.\n", filename);
FILE* f = fopen(filename,"rb");
if (f == NULL) {
printf("TheStrut: Unable to open kicks file %s.\n",filename);
return;
}
fseek(f,0,SEEK_END);
long filesize = ftell(f);
rewind(f);
char* filebuffer = (char*) malloc(filesize);
if (filebuffer == NULL) {
printf("TheStrut: Unable to malloc %d bytes required to parse kicks file %s.\n",filesize,filename);
return;
}
fread(filebuffer,1,filesize,f);
fclose(f);
int filePos = 0;
string temp;
char c;
int i;
Kick k;
short *prev = NULL;
// File reading by MQ. Don't trust it
while (((c = filebuffer[filePos]) != EOF) && (filePos < filesize)) {
filePos++;
if (c == '\n' && temp.length() > 0 && temp != "") {
short *a = new short[16];
istringstream input(temp);
input >> a[0] >> a[1] >> a[2] >> a[3] >> a[4] >> a[5] >> a[6] >> a[7] >> a[8] >> a[9] >> a[10] >> a[11] >> a[12] >> a[13] >> a[14] >> a[15];
for (i = 0; i < 15; i++) {
a[i] = a[i];
}
if (prev != NULL) {
// interpolate. this takes lots of memory. Somewhere in the vicinity of 500kb (?) for our current kicks.dat
// basically, this allows us to easily change the speed of the transition between two joint frames in a kick.
double jointDeltas[15];
double numFrames = (double)(a[15]);
for (int j = 0; j < NUM_HEAD_JOINTS+NUM_BODY_JOINTS; j++) {
jointDeltas[j] = ((double)(a[j]-prev[j]))/numFrames;
}
for (int f = 1; f < numFrames; f++) {
short* joints = new short[16];
for (int j = 0; j < NUM_HEAD_JOINTS+NUM_BODY_JOINTS; j++) {
joints[j] = (short)(prev[j]+jointDeltas[j]*(double)(f));
}
k.frames.push_back(joints);
}
}
k.frames.push_back(a);
prev = a;
temp = "";
} else if (c=='<') {
temp = "";
} else if (c=='>') {
k.name = temp;
temp = "";
} else if (c=='(') {
temp = "";
} else if (c==')') {
k.number=atoi(temp.c_str());
temp = "";
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -