关节运动模拟 in ODE

首先把上次的模拟结果全部画到了一张图里, 可以看一下模拟效果.

还是很有趣.

这次是可控制的关节运动, 不像上次一样全部是纯自然的动作了.

不过这次没法保证代码和结果是完全对的, 参数太多,没太多时间挨个改...

先来看效果

这些展示了关节重心的移动过程, 基本上是在很疯狂的旋转.

第一个关节设定了和大地的fixJoint却还能小幅旋转, 真是神奇....

总之先放在这吧

Mac-mini:ode$ cat example6.d
import std.stdio;
import ode;

static dWorldID world;
static dSpaceID space;
static dJointGroupID contactgroup;

struct cot{
	dBodyID link;
	dJointID joint;
	dReal l,r,theta;
	dReal x,y,z;
	dReal mass;
	dReal anchorx,anchory,anchorz;
	dReal axisx, axisy, axisz;
}

cot*[] links;

const 
	double k1 = 10., fMax = 100.;
void control(){
	foreach(l;links[1..$]){
		double tmp = l.theta - dJointGetHingeAngle(l.joint);
		dJointSetHingeParam(l.joint, dParamVel, k1*tmp);
		dJointSetHingeParam(l.joint, dParamFMax, fMax);
	}
}

extern(System) void nearCallBack(void* date, dGeomID o1, dGeomID o2){
	const maxContacts = 10;
	dContact[maxContacts] contact;
	int numc = dCollide(o1, o2, maxContacts, &contact[0].geom, dContact.sizeof);
	if (numc>0) {
		for (uint i = 0; i< numc; i++) {
			with (contact[i].surface){
				mode = dContactSoftCFM | dContactSoftERP | dContactApprox1;
				mu = dInfinity;
				soft_cfm = 1e-8;
				soft_erp = 1.;
			}
			dJointID c = dJointCreateContact(world, contactgroup, &contact[i]);
			dJointAttach(c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
		}
	}
}

void simLoop(){
	control();
	dSpaceCollide(space, null ,&nearCallBack);
	dWorldStep(world, 0.01);
	dJointGroupEmpty(contactgroup);
	dReal* pos;
	foreach(k,b;links){
		pos = dBodyGetPosition(b.link);
		foreach(i;0..3) writef("%.6f\t",*(pos+i));
	}
	writeln();
}

void create(cot* b){
	b.link = dBodyCreate(world);
	dMass m1;
	dMassSetZero(&m1);
	dMassSetCylinderTotal(&m1,b.mass,2,b.r, b.l);
	dBodySetMass(b.link,&m1);
	dBodySetPosition(b.link, b.x,b.y,b.z);
}

void createFixedJoint(cot* b1, cot* b2){
		b1.joint = dJointCreateFixed(world,null);
		if (!b2){ dJointAttach(b1.joint, b1.link, null);}
		else{ dJointAttach(b1.joint,b1.link,b2.link); }
		dJointSetFixed(b1.joint);    
}

void createHinge(cot* a1, cot* b1){
	b1.joint = dJointCreateHinge(world, null);
	dJointAttach(b1.joint,a1.link, b1.link); 
	dJointSetHingeAnchor(b1.joint, b1.anchorx, b1.anchory, b1.anchorz);
	dJointSetHingeAxis(b1.joint, b1.axisx, b1.axisy, b1.axisz);
}

/*
struct cot{
	dBodyID link;
	dJointID joint;
	dReal l,r,theta;
	dReal x,y,z;
	dReal mass;
	dReal anchorx,anchory,anchorz;
	dReal axisx, axisy, axisz;
}*/

void main(){
	links ~= new cot(null,null,0.1,0.2,0,0,0,0.05,10,0,0,0,0,0,1);
	links ~= new cot(null,null,0.9,0.04,90,0.0,0,0.5,2,0,0,0.1,0,0,1);
	links ~= new cot(null,null,1,0.04,120,0,0.0,1.5,2,0,0,1,0,1,0);
	links ~= new cot(null,null,1,0.04,70,0,0,2.55,2,0,0,2,0,1,0);
	dInitODE();
	world = dWorldCreate();
	space = dHashSpaceCreate(null);
	contactgroup = dJointGroupCreate(0);
	dWorldSetERP(world, 0.5);
	dWorldSetCFM(world, 1e-3);

	dWorldSetGravity(world, 0, 0, -9.8);

	foreach(b;links) create(b);
	createFixedJoint(links[0], null);
	for(uint i=1;i