[SOLVED] Setting trasformation of complex object of…

Post Reply
silmeth
Posts: 1
Joined: Tue Jun 18, 2013 11:38 am

[SOLVED] Setting trasformation of complex object of…

Post by silmeth »

EDIT: OK, I am just dumb and couldn’t find really simple error. You can close (or remove if you like to) this topic. I however, just in case sb is interested, am leaving content of the old post below.
I Ctrl+C-Ctrl+V-ed part of code and didn’t change name of used vector in one place, so the foot was placed in wrong position. Now my code looks like that:

Code: Select all

    btMatrix3x3 orient;

    double sa = sin(alfa);
    double ca = cos(alfa);

    double sb = sin(beta);
    double cb = cos(beta);

    double sg = sin(gamma);
    double cg = cos(gamma);

    orient[0][0] = cb*ca*cg-sb*sg;
    orient[0][1] = -cb*sa;
    orient[0][2] = cg*sb+cb*ca*sg;

    orient[1][0] = cg*sa;
    orient[1][1] = ca;
    orient[1][2] = sa*sg;

    orient[2][0] = -cb*sg-ca*cg*sb;
    orient[2][1] = sb*sa;
    orient[2][2] = cb*cg-ca*sb*sg;

    btVector3 pos = getPos();

    btQuaternion qorient;
    orient.getRotation(qorient);

    btTransform trans;
    trans.setIdentity();
    trans.setOrigin(pos);
    trans.setRotation(qorient);
    SLIPHeadRig->setCenterOfMassTransform(trans);

    btVector3 T2;
    T2[0] = 0.6*cb*sa;
    T2[1] = -0.6*ca;
    T2[2] = -0.6*sa*sb;

    trans.setIdentity();
    trans.setOrigin(pos+T2);
    trans.setRotation(qorient);
    SLIPLegRig->setCenterOfMassTransform(trans);

    btVector3 T3;
    T3[0] = 1.35*cb*sa;
    T3[1] = -1.35*ca;
    T3[2] = -1.35*sa*sb;

    trans.setIdentity();
    trans.setOrigin(pos+T3);
    trans.setRotation(qorient);
    SLIPFootRig->setCenterOfMassTransform(trans);
OLD CONTENT OF THE POST:

Hey, I have been searching for answer with no luck, the problem I have may be because of my wrong understanding of Bullet constraints or its geometric transformations.

I have object simulating Spring Loaded Inverted Pendulum, made of three rigid bodies – head, a ball with radius 0.1 and mass 30; leg, a box 0.1 × 1 × 0.1 (0.05 and 0.5 used creating Bullet Box shape) and mass 1; foot, a ball with radius 0.05 and mass 0.5.

Head and leg are conected using constraint with no degrees of freedom (wich probably isn’t too clever, because I could make just one rigid body for head-leg, but anyway…), leg and foot are connected using btGeneric6DofSpringConstraint with actual one linear degree of freedom and stiffness 1.5e4.

Here is figure showing geometrical configuration of the mechanical system:

Image

I want to describe orientation of this SLIP using two angles called alpha and beta, which are actually two proper Euler angles of YZ'Y'' angles set. Beta is the first angle around Y axis, alpha is the second angle around Z' axis. The third angle (which I call gamma or g in my program) really doesn’t matter for me. Below you can see drawing of those angles:

Image

Now, I want to change orientation of the SLIP (without changing position of the head) during simulation, because I want to use alpha and beta to control the movement of SLIP. I use this rotation matrix to set angles alpha, beta and gamma:

Image

I tried to build such matrix as btMatrix3x3 and then using it to set btTransform and apply it to rigid body of ball, then calculate position of leg and foot (respectively headPosition+[0.6*cb*sa, -0.6*ca, -0.6*sa*sb] and headPosition+[1.35*cb*sa, -1.35*ca, -1.35*sa*sb]) and set their rigid bodies’s transform to such positions and orientation given by aforementioned matrix. But then if I try to set alpha=15 deg and beta=0 I end up with alpha getting higher (16, 17, 18 degrees) and beta getting immediately value of ~ -30 degrees!

I understand that during falling alpha can change, but beta should be stable!

I thought that it might be caused by changing the transform during simulation with some strange forces and velocities applied to objects, so I changed the whole code so that I deleted and created again the whole structure, with new transformation (also removing all velocieties). The whole code I use now is presented below.

When I turned off the gravity, after changing the transformation (i.e. creating the whole object from beginning with new one) I saw foot of SLIP oscillating (meaning somehow spring got energy) and the whole SLIP slowly rotating, changing its both beta and alpha angles.

Did I any mistakes in the geometry? Or did I make some errors defining constraints (I’m not sure if I understand them correctly)?

Here is code creating SLIP – I deleted all display-related (using Irrlicht) code:

Code: Select all

    ball = new btSphereShape(0.1); // head of SLIP shape
    leg = new btBoxShape(btVector3(0.05, 0.5, 0.05)); // leg shape
    foot = new btSphereShape(0.05); // foot shape

    btTransform SLIPHeadTrans;
    SLIPHeadTrans.setIdentity();
    SLIPHeadTrans.setOrigin(btVector3(-10, 5, 0));

    btDefaultMotionState* SLIPHeadMS = new btDefaultMotionState(SLIPHeadTrans);
    btScalar SLIPHeadMass(30);
    btVector3 SLIPHeadInertia(0,0,0);
    ball->calculateLocalInertia(SLIPHeadMass, SLIPHeadInertia);
    btRigidBody::btRigidBodyConstructionInfo SLIPHeadCI(SLIPHeadMass, SLIPHeadMS, ball, SLIPHeadInertia);
    SLIPHeadRig = new btRigidBody(SLIPHeadCI);

    btTransform SLIPLegTrans;
    SLIPLegTrans.setIdentity();
    SLIPLegTrans.setOrigin(btVector3(-10, 4.4, 0));

    btDefaultMotionState* SLIPLegMS = new btDefaultMotionState(SLIPLegTrans);
    btScalar SLIPLegMass(1);
    btVector3 SLIPLegInertia(0,0,0);
    leg->calculateLocalInertia(SLIPLegMass, SLIPLegInertia);
    btRigidBody::btRigidBodyConstructionInfo SLIPLegCI(SLIPLegMass, SLIPLegMS, leg, SLIPLegInertia);
    SLIPLegRig = new btRigidBody(SLIPLegCI);

    btTransform SLIPFootTrans;
    SLIPFootTrans.setIdentity();
    SLIPFootTrans.setOrigin(btVector3(-10, 3.65, 0));

    btDefaultMotionState* SLIPFootMS = new btDefaultMotionState(SLIPFootTrans);
    btScalar SLIPFootMass(0.5);
    btVector3 SLIPFootInertia(0,0,0);
    foot->calculateLocalInertia(SLIPFootMass, SLIPFootInertia);
    btRigidBody::btRigidBodyConstructionInfo SLIPFootCI(SLIPFootMass, SLIPFootMS, foot, SLIPFootInertia);
    SLIPFootRig = new btRigidBody(SLIPFootCI);
    SLIPFootRig->setCcdMotionThreshold(0.05); // Anti-tunneling of small object - foot
    SLIPFootRig->setCcdSweptSphereRadius(0.04);

// HEAD-LEG NO DOF CONSTRAINT

    btTransform headtr, legtr;
    headtr.setIdentity();
    headtr.setOrigin(btVector3(0,-0.05,0));
    legtr.setIdentity();
    legtr.setOrigin(btVector3(0,0.5,0));

    HeadLeg = new btGeneric6DofConstraint(*SLIPHeadRig, *SLIPLegRig, headtr, legtr, false);
    HeadLeg->setLinearLowerLimit(btVector3(0,0,0));
    HeadLeg->setLinearUpperLimit(btVector3(0,0,0));
    HeadLeg->setAngularLowerLimit(btVector3(0,0,0));
    HeadLeg->setAngularUpperLimit(btVector3(0,0,0));

// LEG-FOOT SPRING CONSTRAINT 

    btTransform legtr2;
    legtr2.setIdentity();
    legtr2.setOrigin(btVector3(0, -0.5, 0));
    btTransform foottr;
    foottr.setIdentity();
    foottr.setOrigin(btVector3(0, 0.05, 0));

    LegFoot = new btGeneric6DofSpringConstraint(*SLIPLegRig, *SLIPFootRig, legtr2, foottr, false);
    LegFoot->setLinearLowerLimit(btVector3(0,-0.4,0));
    LegFoot->setLinearUpperLimit(btVector3(0,0,0));
    LegFoot->setAngularLowerLimit(btVector3(0,0,0));
    LegFoot->setAngularUpperLimit(btVector3(0,0,0));
    LegFoot->setStiffness(1, 1.5e4);
    LegFoot->setEquilibriumPoint(1, -0.2);
    LegFoot->enableSpring(1, true);

    world->addRigidBody(SLIPHeadRig);
    world->addRigidBody(SLIPLegRig);
    world->addRigidBody(SLIPFootRig);
    world->addConstraint(HeadLeg);
    world->addConstraint(LegFoot);
Here is code removing old SLIP and creating new one with new alpha, beta, gamma (actually much of previous repeated)

Code: Select all

void slip::setab(double alfa, double beta, double gamma = 0.0) {
    btMatrix3x3 orient;

    double sa = sin(alfa);
    double ca = cos(alfa);

    double sb = sin(beta);
    double cb = cos(beta);

    double sg = sin(gamma);
    double cg = cos(gamma);

    orient[0][0] = cb*ca*cg-sb*sg;
    orient[0][1] = -cb*sa;
    orient[0][2] = cg*sb+cb*ca*sg;

    orient[1][0] = cg*sa;
    orient[1][1] = ca;
    orient[1][2] = sa*sg;

    orient[2][0] = -cb*sg-ca*cg*sb;
    orient[2][1] = sb*sa;
    orient[2][2] = cb*cg-ca*sb*sg;

    btVector3 pos = getPos();

    btQuaternion qorient;
    orient.getRotation(qorient);

    // REMOVING OF THE SLIP

    world->removeConstraint(HeadLeg);
    world->removeConstraint(LegFoot);

    delete HeadLeg;
    delete LegFoot;

    world->removeRigidBody(SLIPHeadRig);
    world->removeRigidBody(SLIPLegRig);
    world->removeRigidBody(SLIPFootRig);

    delete SLIPHeadRig->getMotionState();
    delete SLIPLegRig->getMotionState();
    delete SLIPFootRig->getMotionState();

    delete SLIPHeadRig;
    delete SLIPLegRig;
    delete SLIPFootRig;

    // END OF REMOVING

    // MATH

    btTransform trans;
    trans.setIdentity();
    trans.setOrigin(pos);
    trans.setRotation(qorient);
    btTransform SLIPHeadTrans = trans;

    btVector3 T2;
    T2[0] = 0.6*cb*sa;
    T2[1] = -0.6*ca;
    T2[2] = -0.6*sa*sb;

    trans.setIdentity();
    trans.setOrigin(pos+T2);
    trans.setRotation(qorient);
    btTransform SLIPLegTrans= trans;

    btVector3 T3;
    T3[0] = 1.35*cb*sa;
    T3[1] = -1.35*ca;
    T3[2] = -1.35*sa*sb;

    trans.setIdentity();
    trans.setOrigin(pos+T2);
    trans.setRotation(qorient);
    btTransform SLIPFootTrans = trans;

    // AND OF MATH

    // RECREATING SLIP

    btDefaultMotionState* SLIPHeadMS = new btDefaultMotionState(SLIPHeadTrans);
    btScalar SLIPHeadMass(30);
    btVector3 SLIPHeadInertia(0,0,0);
    ball->calculateLocalInertia(SLIPHeadMass, SLIPHeadInertia);
    btRigidBody::btRigidBodyConstructionInfo SLIPHeadCI(SLIPHeadMass, SLIPHeadMS, ball, SLIPHeadInertia);
    SLIPHeadRig = new btRigidBody(SLIPHeadCI);

    btDefaultMotionState* SLIPLegMS = new btDefaultMotionState(SLIPLegTrans);
    btScalar SLIPLegMass(1);
    btVector3 SLIPLegInertia(0,0,0);
    leg->calculateLocalInertia(SLIPLegMass, SLIPLegInertia);
    btRigidBody::btRigidBodyConstructionInfo SLIPLegCI(SLIPLegMass, SLIPLegMS, leg, SLIPLegInertia);
    SLIPLegRig = new btRigidBody(SLIPLegCI);

    btDefaultMotionState* SLIPFootMS = new btDefaultMotionState(SLIPFootTrans);
    btScalar SLIPFootMass(0.5);
    btVector3 SLIPFootInertia(0,0,0);
    foot->calculateLocalInertia(SLIPFootMass, SLIPFootInertia);
    btRigidBody::btRigidBodyConstructionInfo SLIPFootCI(SLIPFootMass, SLIPFootMS, foot, SLIPFootInertia);
    SLIPFootRig = new btRigidBody(SLIPFootCI);
    SLIPFootRig->setCcdMotionThreshold(0.05); // Anti-tunneling of small object - foot
    SLIPFootRig->setCcdSweptSphereRadius(0.04);

    btTransform headtr, legtr;
    headtr.setIdentity();
    headtr.setOrigin(btVector3(0,-0.05,0));
    legtr.setIdentity();
    legtr.setOrigin(btVector3(0,0.5,0));

    HeadLeg = new btGeneric6DofConstraint(*SLIPHeadRig, *SLIPLegRig, headtr, legtr, false);
    HeadLeg->setLinearLowerLimit(btVector3(0,0,0));
    HeadLeg->setLinearUpperLimit(btVector3(0,0,0));
    HeadLeg->setAngularLowerLimit(btVector3(0,0,0));
    HeadLeg->setAngularUpperLimit(btVector3(0,0,0));

    btTransform legtr2;
    legtr2.setIdentity();
    legtr2.setOrigin(btVector3(0, -0.5, 0));
    btTransform foottr;
    foottr.setIdentity();
    foottr.setOrigin(btVector3(0, 0.05, 0));

    LegFoot = new btGeneric6DofSpringConstraint(*SLIPLegRig, *SLIPFootRig, legtr2, foottr, false);
    LegFoot->setLinearLowerLimit(btVector3(0,-0.4,0));
    LegFoot->setLinearUpperLimit(btVector3(0,0,0));
    LegFoot->setAngularLowerLimit(btVector3(0,0,0));
    LegFoot->setAngularUpperLimit(btVector3(0,0,0));
    LegFoot->setStiffness(1, 1.5e4);
    LegFoot->setEquilibriumPoint(1, -0.2);
    LegFoot->enableSpring(1, true);

    world->addRigidBody(SLIPHeadRig);
    world->addRigidBody(SLIPLegRig);
    world->addRigidBody(SLIPFootRig);
    world->addConstraint(HeadLeg);
    world->addConstraint(LegFoot);
}
And functions used to obtain angles:

Code: Select all

double slip::getb() {
    btMatrix3x3 orient = getOrient();
    if(orient[1][1] == -1) return -atan2(orient[2][0], orient[2][2]);
    else if(orient[1][1] == 1) return atan2(orient[2][0], orient[2][2]);
    else return atan2(orient[2][1], -orient[0][1]);
}

double slip::geta() {
    btMatrix3x3 orient = getOrient();
    double beta = getb();
    double sb = sin(beta);
    double cb = cos(beta);
    return sb ? atan2(orient[2][1]/sb, orient[1][1]) : atan2(-orient[0][1]/cb, orient[1][1]);
//    return acos(orient[1][1]);
}
Function getPos() is just simple return SLIPHeadRig->getCenterOfMassPosition();, getOrient() is defined as SLIPHeadRig->getCenterOfMassTransform().getBasis();

So – how do I change this to give me desired result? What have I done wrong? Could you, please, help?
Post Reply