ODE模拟双足机器人快速行走

dBodySetPosition(foot[i].body,foot_x[i],foot_y[i],foot_z[i]);
foot[i].geom=dCreateBox(space,foot[i].x,foot[i].y,foot[i].z);
dGeomSetBody(foot[i].geom,foot[i].body);
dBodySetPosition(leg[i][j].body,leg_x[i][j],leg_y[i][j],leg_z[i][j]);
leg[i][j].geom=dCreateCapsule(space,leg[i][j].r,leg[i][j].l);
dGeomSetBody(leg[i][j].geom,leg[i][j].body);
arm[i][j].geom=dCreateCapsule(space,arm[i][j].r,arm[i][j].l);
dGeomSetBody(arm[i][j].geom,arm[i][j].body);
}
for(int i=0;i<2;i++)
for(int j=0;j<2;j++)
dReal head_x=0,head_y=0,head_z=0.51;
dReal torso_x=0,torso_y=0,torso_z=0.365;
dReal hip_x[2]={0,0},hip_y[2]={-0.04,0.04},hip_z[2]={0.27,0.27};
dReal ankle_x[2]={0,0},ankle_y[2]={-0.04,0.04},ankle_z[2]={0.03,0.03};
dReal leg_joint_x[2][3]={0,0,0,0,0,0},leg_joint_y[2][3]={-0.04,-0.04,-0.04,0.04,0.04,0.04},leg_joint_z[2][3]={0.27,0.15,0.03,0.27,0.15,0.03};
dReal hip_joint_x[2]={0,0},hip_joint_y[2]={-0.04,0.04},hip_joint_z[2]={0.27,0.27};
#include <stdio.h>
#include <stdlib.h>
#include<ode/ode.h>
#include<drawstuff/drawstuff.h>
#define stp 10
dWorldID world;
dSpaceID space=dHashSpaceCreate(0);
{{0,0,0,0},{0,0,0,0}}
};
static long int TIME=0;
static float xyz[3]={0.7,0,0.3};
static float hpr[3]={180,0.0,0.0};
static float steps[10][2][5],temp_step[2][5];
dReal leg_x[2][2]={{0,0},{0,0}},leg_y[2][2]={{-0.04,-0.04},{0.04,0.04}},leg_z[2][2]={{0.21,0.09},{0.21,0.09}};
dReal foot_x[2]={0.02,0.02},foot_y[2]={-0.04,0.04},foot_z[2]={0.015,0.015};
}
for(int i=0;i<2;i++)
{
foot[i].body=dBodyCreate(world);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass,foot[i].m,foot[i].x,foot[i].y,foot[i].z);
dBodySetMass(foot[i].body,&mass);
dReal foot_joint_x[2]={0,0},foot_joint_y[2]={-0.04,0.04},foot_joint_z[2]={0.03,0.03};
typedef struct {
dBodyID body;
dGeomID geom;
dJointID joint;
{{-0.02,ym,z0,0},{-0.02,ym,z1+0.02,0}},//2
{{-fs/2,0,z1,0},{fs/2,0,z1,0}},
{{-fs+fl*(1-cos(footc)),-(ym-k),z0+fl*sin(footc),1},{-0.02,-(ym-k),z0,0}},
dMassSetZero(&mass);
dMassSetCapsuleTotal(&mass,arm[i][j].m,3,arm[i][j].r,arm[i][j].l);
dBodySetMass(arm[i][j].body,&mass);
dBodySetPosition(arm[i][j].body,arm_x[i][j],arm_y[i][j],arm_z[i][j]);
torso.body=dBodyCreate(world);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass,torso.m,torso.x,torso.y,torso.z);
dBodySetMass(torso.body,&mass);
dBodySetPosition(torso.body,torso_x,torso_y,torso_z);
dReal m,r,l,x,y,z;
} MyLink;
MyLink leg[2][2],arm[2][2],torso,head,foot[2],ankle[2],hip[2];
void makeRobot()
{
head.m=0.5;head.r=0.05;
torso.m=1,torso.x=0.06,torso.y=0.12,torso.z=0.19;
hip[1]=hip[0];
dMass mass;
//**************make body*****************//
head.body=dBodyCreate(world);
dMassSetZero(&mass);
dMassSetSphereTotal(&mass,head.m,head.r);
foot[0].m=0.02,foot[0].x=0.1,foot[0].y=0.06,foot[0].z=0.03;
foot[1]=foot[0];
ankle[0].r=0.02,ankle[0].m=0.001;
ankle[1]=ankle[0];
hip[0].r=0.02,hip[0].m=0.001;
dReal head_joint[3]={0,0,0.46};
dReal arm_joint_x[2][2]={0,0,0,0},arm_joint_y[2][2]={-0.08,-0.08,0.08,0.08},arm_joint_z[2][2]={0.44,0.34,0.44,0.34};
{{-0.02,-ym,z1+0.02,0},{-0.02,-ym,z0,0}},
{{fs/2,0,z1,0},{-fs/2,0,z1,0}},
{{-0.02,ym-k,z0,0},{-fs+fl*(1-cos(footc)),ym-k,z0+fl*sin(footc),1}},//7
{{-0.02,ym,z0,0},{-0.02,ym,z1+0.02,0}},
ankle[i].geom=dCreateSphere(space,ankle[i].r);
dGeomSetBody(ankle[i].geom,an*********make joint*************************//
dGeomID ground=dCreatePlane(space,0,0,1,0);
dJointGroupID contactgroup=dJointGroupCreate(0);
dsFunctions fn;
dReal footc=0.4*M_PI; //footc:足補助角度
torso.geom=dCreateBox(space,torso.x,torso.y,torso.z);
dGeomSetBody(torso.geom,torso.body);
for(int i=0;i<2;i++)
for(int j=0;j<2;j++)
{
arm[i][j].body=dBodyCreate(world);
{
leg[i][j].body=dBodyCreate(world);
dMassSetZero(&mass);
dMassSetCapsuleTotal(&mass,leg[i][j].m,3,leg[i][j].r,leg[i][j].l);
dBodySetMass(leg[i][j].body,&mass);
//**************body parameter***************//
dReal arm_x[2][2]={{0,0},{0,0}},arm_y[2][2]={{-0.08,-0.08},{0.08,0.08}},arm_z[2][2]={{0.4,0.28},{0.4,0.28}};