worldsim/src/phyhinge.cpp

/********************************************************************************
 *  WorldSim -- library for robot simulations                                   *
 *  Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it>                *
 *                                                                              *
 *  This program is free software; you can redistribute it and/or modify        *
 *  it under the terms of the GNU General Public License as published by        *
 *  the Free Software Foundation; either version 2 of the License, or           *
 *  (at your option) any later version.                                         *
 *                                                                              *
 *  This program is distributed in the hope that it will be useful,             *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of              *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               *
 *  GNU General Public License for more details.                                *
 *                                                                              *
 *  You should have received a copy of the GNU General Public License           *
 *  along with this program; if not, write to the Free Software                 *
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA  *
 ********************************************************************************/

#include "phyhinge.h"
#include "private/phyjointprivate.h"
#include "private/phyobjectprivate.h"
#include "private/worldprivate.h"

// This is needed because the isnan and isinf functions are not present windows
#ifdef FARSA_WIN
    #include <float.h>
    #define isnan(x) _isnan(x)
#endif

namespace farsa {

void PhyHinge::construct( const wVector& axis, const wVector& centre, real startAngle, PhyObject* parent, PhyObject* child, bool cp ) {
    dof = new PhyDOF( this, axis, centre, false );
    dofsv << dof;
    dofv = 1;
    //--- calculate the localMatrixParent
    //--- start creating an orthonormal coordinate frame using grammSchmidt procedure
    localMatrixParent = wMatrix::grammSchmidt(axis);
    //--- calculate the axis that connect the centre of joint with centre of child object
    wVector start = centre;
    wVector end = child->matrix().w_pos;
    if ( parent ) {
        start -= parent->matrix().w_pos;
    }
    wVector newx = (start-end).normalize();
    if ( isnan(newx[0]) || isnan(newx[1]) || isnan(newx[2]) ) {
        // this happens when it is not possible to calculate the axis above,
        // --- in this case nothing has to be done
    } else {
        //--- rotate the matrix in order to align X axis calculated from grammSchmidt procedure
        //    to the axis connecting centres. In this way, angle zero correspond to position at which
        //    child object was connected by this joint
        real sinAngle = (newx * localMatrixParent.x_ax) % localMatrixParent.z_ax;
        real cosAngle = newx % localMatrixParent.x_ax;
        real angle = atan2( sinAngle, cosAngle );
        wQuaternion ql( axis, angle );
        localMatrixParent = localMatrixParent * wMatrix( ql, wVector(0,0,0) );
    }
    localMatrixParent.w_pos = centre;
    localMatrixParent.sanitifize();
    
    //--- calculate the local matrix respect to child object
    if ( parent ) {
        globalMatrixParent = localMatrixParent * parent->matrix();
    } else {
        globalMatrixParent = localMatrixParent;
    }
    localMatrixChild = globalMatrixParent * child->matrix().inverse();

    localMatrixParent.w_pos = wVector(0,0,0);
    //--- it will rotate
    //    the localMatrixParent in order to align the axis
    //    with the requested startAngle
    wQuaternion q( axis, startAngle );
    localMatrixParent = localMatrixParent * wMatrix( q, wVector(0,0,0) );
    localMatrixParent.w_pos = centre;
    if ( parent ) {
        globalMatrixParent = localMatrixParent * parent->matrix();
    } else {
        globalMatrixParent = localMatrixParent;
    }

    dof->setAxis( globalMatrixParent.z_ax );
    dof->setXAxis( globalMatrixParent.x_ax );
    dof->setYAxis( globalMatrixParent.y_ax );
    dof->setCentre( globalMatrixParent.w_pos );
    
    if ( !localMatrixParent.sanityCheck() ) {
        qWarning( "Sanity Check Failed on localMatrixParent" );
    }
    if ( !localMatrixChild.sanityCheck() ) {
        qWarning( "Sanity Check Failed on localMatrixChild" );
    }
    
    if ( cp ) {
        createPrivateJoint();
    }
}

PhyHinge::PhyHinge( const wVector& axis, const wVector& centre, real startAngle, PhyObject* parent, PhyObject* child, bool cp )
    : PhyJoint( parent, child ) {
    construct( axis, centre, startAngle, parent, child, cp );
}

PhyHinge::PhyHinge( const wVector& axis, const wVector& centre, PhyObject* parent, PhyObject* child, bool cp )
    : PhyJoint( parent, child ) {
    construct( axis, centre, 0, parent, child, cp );
}

PhyHinge::PhyHinge( const wVector& axis, real startAngle, PhyObject* parent, PhyObject* child, bool cp )
    : PhyJoint( parent, child ) {
    construct( axis, wVector(0,0,0), startAngle, parent, child, cp );
}

PhyHinge::PhyHinge( const wVector& axis, PhyObject* parent, PhyObject* child, bool cp )
    : PhyJoint( parent, child ) {
    construct( axis, wVector(0,0,0), 0, parent, child, cp );
}

PhyHinge::PhyHinge( const wVector& axis, const wVector& centre, const wVector& x_axis, PhyObject* parent, PhyObject* child, bool cp )
    : PhyJoint( parent, child ) {
    dof = new PhyDOF( this, axis, centre, false );
    dofsv << dof;
    dofv = 1;
    //--- calculate the localMatrixParent
    //--- supposing that axis and x_axis are ortogonals
    localMatrixParent.x_ax = x_axis;
    localMatrixParent.y_ax = axis * x_axis;
    localMatrixParent.z_ax = axis;
    localMatrixParent.w_pos = centre;
    localMatrixParent.sanitifize();
    
    //--- calculate the local matrix respect to child object
    if ( parent ) {
        globalMatrixParent = localMatrixParent * parent->matrix();
    } else {
        globalMatrixParent = localMatrixParent;
    }
    localMatrixChild = globalMatrixParent * child->matrix().inverse();
    if ( parent ) {
        globalMatrixParent = localMatrixParent * parent->matrix();
    } else {
        globalMatrixParent = localMatrixParent;
    }

    dof->setAxis( globalMatrixParent.z_ax );
    dof->setXAxis( globalMatrixParent.x_ax );
    dof->setYAxis( globalMatrixParent.y_ax );
    dof->setCentre( globalMatrixParent.w_pos );
    
    if ( !localMatrixParent.sanityCheck() ) {
        qWarning( "Sanity Check Failed on localMatrixParent" );
    }
    if ( !localMatrixChild.sanityCheck() ) {
        qWarning( "Sanity Check Failed on localMatrixChild" );
    }
    
    if ( cp ) {
        createPrivateJoint();
    }
}

wVector PhyHinge::centre() const {
    //--- calculate global matrix if necessary
    if ( parent() ) {
        globalMatrixParent = localMatrixParent * parent()->matrix();
    }
    return globalMatrixParent.w_pos;
}

void PhyHinge::createPrivateJoint() {
#ifdef WORLDSIM_USE_NEWTON
    priv->joint = NewtonConstraintCreateUserJoint( worldpriv->world, 6, PhyJointPrivate::userBilateralHandler, 0, priv->child, priv->parent );
    NewtonJointSetUserData( priv->joint, this );
#endif
}

void PhyHinge::updateJointInfo() {
    //--- calculate the global matrices
    //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
    //--- there is no need to re-assign it because it was already done in constructor
    if ( parent() ) {
        globalMatrixParent = localMatrixParent * parent()->matrix();
    }
    globalMatrixChild = localMatrixChild * child()->matrix();

    if ( !globalMatrixParent.sanityCheck() ) {
        qWarning( "Sanity Check Failed on globalMatrixParent" );
    }
    if ( !globalMatrixChild.sanityCheck() ) {
        qWarning( "Sanity Check Failed on globalMatrixChild" );
    }

    dof->setAxis( globalMatrixParent.z_ax );
    dof->setXAxis( globalMatrixParent.x_ax );
    dof->setYAxis( globalMatrixParent.y_ax );
    dof->setCentre( globalMatrixParent.w_pos );

    PhyDOF* dof = dofsv[0];
    //--- calculate the rotation assuming the local X axis of joint frame as 0
    //--- and following the right-hand convention
    real sinAngle = (globalMatrixParent.x_ax * globalMatrixChild.x_ax) % globalMatrixChild.z_ax;
    real cosAngle = globalMatrixChild.x_ax % globalMatrixParent.x_ax;
    real angle = atan2( sinAngle, cosAngle );
    real vel;
    // Velocity is computed in two different ways depending on whether the joint is enabled or not:
    // if it is, we use Newton functions to get the angular velocities (for a better result), otherwise
    // we revert to computing the difference with the previous position divided by the timestep
    if (enabled) {
        //--- the velocity is calculated projecting the angular velocity of objects on the main axis (z_ax)
        //    This code is not general, becuase it is not appliable in the case of parent==NULL
        //    and also return different results respect to the kinematic way
        //    Then [Gianluca] comment it and replaced with the same code used in the kinematic mode
        wVector omegaParent, omegaChild;
#ifdef WORLDSIM_USE_NEWTON
        if ( parent() ) {
            NewtonBodyGetOmega( priv->parent, &omegaParent[0] );
        } else {
            omegaParent = wVector(0, 0, 0);
        }
        NewtonBodyGetOmega( priv->child, &omegaChild[0] );
#endif
        real velP = omegaParent % globalMatrixChild.z_ax;
        real velC = omegaChild % globalMatrixChild.z_ax;
        vel = velC - velP;
        //vel = (angle - dof->position()) / (world()->timeStep());
    } else {
        vel = (angle - dof->position()) / (world()->timeStep());
    }
    dof->setPosition( angle );
    dof->setVelocity( vel );
}

void PhyHinge::updateJoint( real timestep ) {
    // If the joint is disabled, we don't do anything here
    if (!enabled) {
        return;
    }

    // Updating the global matrices and the dof
    updateJointInfo();

    const real angle = dof->position();
    const real vel = dof->velocity();

#ifdef WORLDSIM_USE_NEWTON
    //--- Restrict the movement on the centre of the joint along all tree orthonormal direction
    NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixChild.x_ax[0] );
    NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
    NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixChild.y_ax[0] );
    NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
    NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixChild.z_ax[0] );
    NewtonUserJointSetRowStiffness( priv->joint, 1.0 );

    //--- In order to constraint the rotation about X and Y axis of the joint
    //--- we use LinearRow (that are stronger) getting a point far from objects along
    //--- the Z axis. Doing this if the two object rotates about X or Y axes then
    //--- the difference between qChild and qParent augments and then Newton Engine will apply
    //--- a corresponding force (that applyied outside the centre of the object will become
    //--- a torque) that will blocks any rotation about X and Y axis
    real len = 50000.0;
    wVector qChild( globalMatrixChild.w_pos + globalMatrixChild.z_ax.scale(len) );
    wVector qParent( globalMatrixParent.w_pos + globalMatrixParent.z_ax.scale(len) );
    NewtonUserJointAddLinearRow( priv->joint, &qChild[0], &qParent[0], &globalMatrixChild.x_ax[0] );
    NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
    NewtonUserJointAddLinearRow( priv->joint, &qChild[0], &qParent[0], &globalMatrixChild.y_ax[0] );
    NewtonUserJointSetRowStiffness( priv->joint, 1.0 );

    if ( dof->isLimited() ) {
        real lolimit;
        real hilimit;
        dof->limits( lolimit, hilimit );
        if ( angle < lolimit ) {
            real relAngle = lolimit - angle; // - lolimit;
            //--- the command is opposite to the movement required to go away from the limit
            //    so, it will be overrided for going away from the limit
            NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.z_ax[0] );
            NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
            NewtonUserJointSetRowSpringDamperAcceleration( priv->joint, 50000.0, 100 );
            //--- this allow the joint to move back freely
            NewtonUserJointSetRowMinimumFriction( priv->joint, 0.0 );
            return;
        } else if ( angle > hilimit ) {
            real relAngle = hilimit - angle; // - hilimit;
            //--- the command is opposite to the movement required to go away from the limit
            //    so, it will be overrided for going away from the limit            
            NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.z_ax[0] );
            NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
            NewtonUserJointSetRowSpringDamperAcceleration( priv->joint, 50000.0, 100 );
            //--- this allow the joint to move forth freely
            NewtonUserJointSetRowMaximumFriction( priv->joint, 0.0 );
            return;
        }
    }
        
    //--- if it reach this point means that the joint if far from limits
    //--- and we check for type of motion and we'll apply the corresponding entity
    real force, accel, wishangle, wishvel, sign;
    switch( dof->motion() ) {
    case PhyDOF::force:
        //--- force will be used to set-up max and min torque newton will use to
        //--- resolve the constraint and accel to the double of appliedForce
        //--- this means that Newton will apply exactly forse amount of torque around
        //--- the axis
        force = fabs( dof->appliedForce() );
        accel = 2.0*dof->appliedForce();
        NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.z_ax[0] );
        NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
        NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
        NewtonUserJointSetRowAcceleration( priv->joint, accel );
        NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
        break;
    case PhyDOF::vel:
        //--- I'm not sure that this implementation is correct
        accel = 0.8*( dof->desiredVelocity() - vel ) / ( timestep );
        wishangle = (dof->desiredVelocity()*timestep);
        NewtonUserJointAddAngularRow( priv->joint, wishangle, &globalMatrixChild.z_ax[0] );
        NewtonUserJointSetRowAcceleration( priv->joint, accel );
        NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
        break;
    case PhyDOF::off:
    case PhyDOF::pos:
        wishangle = dof->desiredPosition() - angle;
        wishvel = wishangle / timestep;
        if ( fabs(wishvel) > dof->maxVelocity() ) {
            sign = (wishvel<0) ? -1.0 : +1.0;
            wishvel = sign*dof->maxVelocity();
        }
        accel = 0.8*( wishvel - vel ) / ( timestep );
        NewtonUserJointAddAngularRow( priv->joint, wishangle, &globalMatrixChild.z_ax[0] );
        NewtonUserJointSetRowAcceleration( priv->joint, accel );
        NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
        //NewtonUserJointAddAngularRow( priv->joint, wishangle, &globalMatrixChild.z_ax[0] );
        //NewtonUserJointSetRowSpringDamperAcceleration( priv->joint, 30000.0, 2000 );
        //NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
        break;
    default:
        break;
    }
#endif

}

} // end namespace farsa