int Judge(Cube space,Piece p[8],int p_num,int x,int y,int z){
Piece fp=FootPoint(space,p,p_num,x,y,z);
if(space.form[x][y][z]==0&&
space.form[fp.foot[0][0]][fp.foot[0][1]][fp.foot[0][2]]==0&&
space.form[fp.foot[1][0]][fp.foot[1][1]][fp.foot[1][2]]==0&&
space.form[fp.foot[2][0]][fp.foot[2][1]][fp.foot[2][2]]==0
){return 1;}
else return 0;
}
Cube PutPiece(Cube space,Piece p[8],int p_num,int x,int y,int z){
Piece fp=FootPoint(space,p,p_num,x,y,z);
int f_num;
space.form[x][y][z]=p_num;
for(f_num=0;f_num<3;f_num++){
space.form[fp.foot[f_num][0]][fp.foot[f_num][1]][fp.foot[f_num][2]]=p_num;
}
return space;
}
int ControlT(const float* P1, const float* P2, const float* MechPara, const float* IMUdata, const float* TensionT)
{
float R1[9],R2[9];
float l1[3],l2[3];
l1[0]=l1[1]=0;
l1[2]=MechPara[0];
l2[0]=l1[2]=0;
l2[2]=MechPara[1];
if(IMUdata[0]==4||IMUdata[1]==4||IMUdata[2]==4||IMUdata[3]==4||IMUdata[4]==4||IMUdata[5]==4)//IMU数据错误
return -1;
EA2SO3(*(IMUdata+2),*(IMUdata+1),*(IMUdata),R1);//求矩阵R1，R2
EA2SO3(*(IMUdata+5),*(IMUdata+4),*(IMUdata+3),R2);
float P[3];
SolveP(R1, R2, l1, l2, P);//求腕部轨迹P
float d;
float fp[3];
d = FootPoint(P1, P2, P, fp);//求P到轨迹P1P2的距离d与垂足fp
const float an = 1;//构造末端力的参数
float F[3];
MakeF(P, fp, d, an, F);//构造末端所需的力F
float taos[3],taoe[3];
TransformF(R1, R2, l1, l2, F, taos, taoe);//末端力转化到关节力矩
float jacob_s[12],jacob_e[6];
SolveJacob(MechPara, R1, R2, jacob_s, jacob_e);//求Jacob矩阵
float ts[4];
if(QP(jacob_s,taos,ts) == -1)//求优发散了
{
return -1;
}
else
{
return 1;
// if(QP(jacob_e,taoe,te == -1))//求优发散了
// return -1;
// else//终于求出来最优解了
// {
//
// }
}
}
