robotic fish locomotion

ajc · Post by **ajc** » Mon Jun 13, 2011 6:54 pm

Hello,

I am having some difficulty simulating robotic fish locomotion. The applied physics is based on http://citeseer.ist.psu.edu/viewdoc/sum ... 1.107.1505 -- the body dimensions are based on a rough approximation of our actual robotic fish. The robot is approximated by two boxes connected at a hinge. If it helps you to picture it, the body is (0.020x0.120x0.14cm) and the fin is (0.010x0.025x0.080cm).

As a starting point, the robot is simply 2 boxes that can only move in the x-z plane:

Code: Select all

    // body
    {
        Body = createBox(fMass, vSize, transform);
        // allow small oscillations
        Body->setActivationState(DISABLE_DEACTIVATION);
        // restrict angular movement to 1 dimension
        Body->setAngularFactor(btVector3(0,1,0));
    }
    
    // fin
    {
        Fin = createBox(fMass, vSize, transform);        
        // allow small oscillations
        Fin->setActivationState(DISABLE_DEACTIVATION);
        // restrict angular movement to 1 dimension
        Fin->setAngularFactor(btVector3(0,1,0));
    }

    // hinge
    {
        Hinge = new btHingeConstraint(
           *Body,                               //rbA
           *Fin,                                //rbB
           btVector3(0., 0., BODY_LENGTH/2.0),  //pivotInA
           btVector3(0., 0., -FIN_LENGTH/2.0),  //pivotInB
           btVector3(0., 1., 0.),               //axisInA
           btVector3(0., 1., 0.));              //axisInB
        
        float limit = 60.*SIMD_RADS_PER_DEG;
        Hinge->setLimit(btScalar(-limit), btScalar(limit));
        btWorld->addConstraint(Hinge, false);
    }

btRigidBody* createBox(btScalar mass, btVector3 size, btTransform trans) {
    // the location and full half extents of the box
    btVector3 loc = trans.getOrigin();
    btVector3 half = size / 2.;
    
    //
    // setup the graphics node
    //
    ...boxNode...
    
    //
    // setup the physics body
    //
    // is this a dynamic or static body
    btCollisionShape* boxShape = new btBoxShape(half);
    bool dynamic = (mass != 0.f);
    btVector3 inertia(0.,0.,0.);
    if (dynamic) boxShape->calculateLocalInertia(mass, inertia);

    // create body
    IrrMotionState* boxMotionState = new IrrMotionState(trans, boxNode);
    btRigidBody::btRigidBodyConstructionInfo boxRigidBodyCI(mass,boxMotionState,boxShape,inertia);
    btRigidBody* boxRigidBody = new btRigidBody(boxRigidBodyCI);
    btWorld->addRigidBody(boxRigidBody);
    Bodies.push_back(boxRigidBody);
    
    return boxRigidBody;
}

At each tick, I update the hinge angle, and apply drag and propulsive forces (gravity = 0).

The hinge control is similar to an example given by someone on this forum. I do not plan on using it in the long run, but It should work for testing.

Code: Select all

    float fFreq = 1.;                           // oscillations per second
    float fMaxAmp = 25 * SIMD_RADS_PER_DEG;     // maximum amplitude
    float timeSeconds = fTime / 1000000.;       // current time
    btScalar fMaxVelocity = 2*SIMD_PI*fFreq;    // maximum motor velocity
    btScalar fMaxImpulse = 0.1*SCALE;
    
    //
	// set per-frame sinusoidal velocity
	//
    // calculate motor velocity from current and desired angles
    btScalar fCurAngle = Hinge->getHingeAngle();
    btScalar fTargetAngle = fMaxAmp * sin(2*SIMD_PI*fFreq*timeSeconds);
    btScalar fAngError = fTargetAngle - fCurAngle;
    
    btScalar fMotorVelocity;
    if (fAngError > 0) fMotorVelocity = fMaxVelocity;
    else fMotorVelocity = -fMaxVelocity;
    
    if (fabs(fAngError) <= fMaxVelocity * timeStep)
    {
        fMotorVelocity *= fabs(fAngError)/(fMaxVelocity * timeStep);
    }
    
    Hinge->enableAngularMotor(true, fMotorVelocity, fMaxImpulse);

The drag force is applied in the opposite direction of the bodies velocity at the bodies center:

Code: Select all

        // get the linear velocity in the bodies local frame coordinates
        btVector3 vBodyVelocity = Body->getWorldTransform().getBasis() * Body->getLinearVelocity();
        
        // drag force only if the object is moving
        if(!vBodyVelocity.isZero()) {
            
            // calculate the force
            btScalar fMagDrag = LAMBDA_BODY * getSquare(vBodyVelocity.length());
            
            // calculate the direction of the force 
            btVector3 vDragForce = -vBodyVelocity.normalize() * fMagDrag;
            Body->applyCentralForce(vDragForce);
        }

The propulsive force is applied to the fin in the opposite direction of the angular motion:

Code: Select all

        btScalar fC1 = 1000. * (SIMD_HALF_PI*.5) * 2.e-6;
        btVector3 vFinAngVelocity = Fin->getAngularVelocity();

        // force only if moving
        if (!vFinAngVelocity.isZero()) {

            // calculate the force
            btScalar fFinAngAccel = (vFinAngVelocityPrev.y() - vFinAngVelocity.y()) / timeStep;
            btScalar fMagHydro = fabs(fC1 * fFinAngAccel);

            // calculate the fin normal vector
            btVector3 vFinFace = Fin->getWorldTransform().getBasis() * btVector3(0.,0.,1.);
            btVector3 vFinNormal = vFinAngVelocity.cross(vFinFace).normalize();

            // force given the normal vector
            btVector3 vFinForce = vFinNormal * fMagHydro;
            Fin->applyCentralForce(vFinForce);
        }
        vFinAngVelocityPrev = vFinAngVelocity;

And if it is helpful, here is how I step the simulation:

Code: Select all

    btWorld->stepSimulation(0.016, 1, 1/240.);

The problem that I am having occurs regardless of whether I apply drag and propulsion (i.e. if I comment out *->applyCentralForce()). The robot will simply rotate out-of-control to varying degrees depending on the motor's frequency and the scale (the actual robot is in the 10 cm range, but I scaled the units to cm to test the problem). I have read in different threads that the hinge's may not be suitable given the contraint solver. Are there work arounds for this? Also, I have noticed that angular velocity seems to be highest during the first half cycle of the hinge's movement - I cannot think of any physical reason for this off the top of my head.

So, I'd appreciate any comments on the following:
- How do I stop (mitigate) the out-of-control spinning?
- My code (the linear algebra may be incorrect?).

I'd appreciate all comments regarding my problem, code, linear algebra etc.
Thanks.

Notes:
- Bullet Version 2.78
- If any more information will be helpful please don't hesitate to ask for it.
- This simulation works properly in an ODE based simulator (different constraint solver?)