worldsim/src/phyuniversal.cpp

00001 /********************************************************************************
00002  *  WorldSim -- library for robot simulations                                   *
00003  *  Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it>                *
00004  *                                                                              *
00005  *  This program is free software; you can redistribute it and/or modify        *
00006  *  it under the terms of the GNU General Public License as published by        *
00007  *  the Free Software Foundation; either version 2 of the License, or           *
00008  *  (at your option) any later version.                                         *
00009  *                                                                              *
00010  *  This program is distributed in the hope that it will be useful,             *
00011  *  but WITHOUT ANY WARRANTY; without even the implied warranty of              *
00012  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               *
00013  *  GNU General Public License for more details.                                *
00014  *                                                                              *
00015  *  You should have received a copy of the GNU General Public License           *
00016  *  along with this program; if not, write to the Free Software                 *
00017  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA  *
00018  ********************************************************************************/
00019 
00020 #include "phyuniversal.h"
00021 #include "private/phyjointprivate.h"
00022 #include "private/phyobjectprivate.h"
00023 #include "private/worldprivate.h"
00024 
00025 namespace farsa {
00026 
00027 PhyUniversal::PhyUniversal( const wVector& firstAxis, const wVector& secondAxis, const wVector& centre, PhyObject* parent, PhyObject* child, bool cp )
00028     : PhyJoint( parent, child ) {
00029     firstDof = new PhyDOF( this, firstAxis, centre, false );
00030     dofsv << firstDof;
00031     secondDof = new PhyDOF( this, secondAxis, centre, false );
00032     dofsv << secondDof;
00033     dofv = 2;
00034     //--- calculate the localMatrixParent
00035     //--- supposing that firstAxis and secondAxis are ortogonals
00036     localMatrixParent.x_ax = secondAxis;
00037     localMatrixParent.y_ax = firstAxis * secondAxis;
00038     localMatrixParent.z_ax = firstAxis;
00039     localMatrixParent.w_pos = centre;
00040     localMatrixParent.sanitifize();
00041     
00042     //--- calculate the local matrix respect to child object
00043     if ( parent ) {
00044         globalMatrixParent = localMatrixParent * parent->matrix();
00045     } else {
00046         globalMatrixParent = localMatrixParent;
00047     }
00048     localMatrixChild = globalMatrixParent * child->matrix().inverse();
00049     if ( parent ) {
00050         globalMatrixParent = localMatrixParent * parent->matrix();
00051     } else {
00052         globalMatrixParent = localMatrixParent;
00053     }
00054 
00055     firstDof->setAxis( globalMatrixParent.z_ax );
00056     firstDof->setXAxis( globalMatrixParent.x_ax );
00057     firstDof->setYAxis( globalMatrixParent.y_ax );
00058     firstDof->setCentre( globalMatrixParent.w_pos );
00059 
00060     globalMatrixChild = localMatrixChild * child->matrix();
00061     secondDof->setAxis( globalMatrixParent.x_ax );
00062     secondDof->setXAxis( globalMatrixParent.z_ax );
00063     secondDof->setYAxis( globalMatrixParent.y_ax );
00064     secondDof->setCentre( globalMatrixParent.w_pos );
00065     
00066     if ( !localMatrixParent.sanityCheck() ) {
00067         qWarning( "Sanity Check Failed" );
00068     }
00069     if ( !localMatrixChild.sanityCheck() ) {
00070         qWarning( "Sanity Check Failed" );
00071     }
00072     
00073     if ( cp ) {
00074         createPrivateJoint();
00075     }
00076 }
00077 
00078 wVector PhyUniversal::centre() {
00079     //--- calculate global matrix if necessary
00080     if ( parent() ) {
00081         globalMatrixParent = localMatrixParent * parent()->matrix();
00082     }
00083     return globalMatrixParent.w_pos;
00084 };
00085 
00086 void PhyUniversal::createPrivateJoint() {
00087 #ifdef WORLDSIM_USE_NEWTON
00088     priv->joint = NewtonConstraintCreateUserJoint( worldpriv->world, 6, PhyJointPrivate::userBilateralHandler, 0, priv->child, priv->parent );
00089     NewtonJointSetUserData( priv->joint, this );
00090 #endif
00091 }
00092 
00093 void PhyUniversal::updateJointInfo() {
00094     //--- calculate the global matrices
00095     //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
00096     //--- there is no need to re-assign it because it was already done in constructor
00097     if ( parent() ) {
00098         globalMatrixParent = localMatrixParent * parent()->matrix();
00099     }
00100     globalMatrixChild = localMatrixChild * child()->matrix();
00101 
00102     if ( !globalMatrixParent.sanityCheck() ) {
00103         qWarning( "Sanity Check Failed" );
00104     }
00105     if ( !globalMatrixChild.sanityCheck() ) {
00106         qWarning( "Sanity Check Failed" );
00107     }
00108 
00109     firstDof->setAxis( globalMatrixParent.z_ax );
00110     firstDof->setXAxis( globalMatrixParent.x_ax );
00111     firstDof->setYAxis( globalMatrixParent.y_ax );
00112     firstDof->setCentre( globalMatrixParent.w_pos );
00113 
00114     //--- calculate the rotation assuming the local X axis of joint frame as 0
00115     //--- and following the right-hand convention
00116     real sinAngle = (globalMatrixParent.x_ax * globalMatrixChild.x_ax) % globalMatrixChild.z_ax;
00117     real cosAngle = globalMatrixChild.x_ax % globalMatrixParent.x_ax;
00118     real angle = atan2( sinAngle, cosAngle );
00119     firstDof->setPosition( angle );
00120 
00121     //--- the secondDOF results rotate of angle respect to its local X axis
00122     wMatrix tmp = wMatrix::identity();
00123     tmp.x_ax = globalMatrixParent.z_ax;
00124     tmp.y_ax = globalMatrixParent.y_ax;
00125     tmp.z_ax = globalMatrixParent.x_ax;
00126     tmp = tmp * wMatrix( wQuaternion( tmp.x_ax, angle ), wVector(0,0,0) );
00127     tmp.sanitifize();
00128     secondDof->setAxis( tmp.z_ax );
00129     secondDof->setXAxis( tmp.x_ax );
00130     secondDof->setYAxis( tmp.y_ax );
00131     secondDof->setCentre( globalMatrixParent.w_pos );
00132 }
00133 
00134 void PhyUniversal::updateJoint( real timestep ) {
00135     //--- calculate the global matrices
00136     //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
00137     //--- there is no need to re-assign it because it was already done in constructor
00138     if ( parent() ) {
00139         globalMatrixParent = localMatrixParent * parent()->matrix();
00140     }
00141     globalMatrixChild = localMatrixChild * child()->matrix();
00142 
00143     if ( !globalMatrixParent.sanityCheck() ) {
00144         qWarning( "Sanity Check Failed" );
00145     }
00146     if ( !globalMatrixChild.sanityCheck() ) {
00147         qWarning( "Sanity Check Failed" );
00148     }
00149 
00150     firstDof->setAxis( globalMatrixParent.z_ax );
00151     firstDof->setXAxis( globalMatrixParent.x_ax );
00152     firstDof->setYAxis( globalMatrixParent.y_ax );
00153     firstDof->setCentre( globalMatrixParent.w_pos );
00154 
00155     //--- information about secondDOF will be calculated below
00156     //--- because it depends on rotation about firstDOF
00157 
00158 #ifdef WORLDSIM_USE_NEWTON
00159     //--- get the pin fixed to the first body
00160     const wVector& dir0 = globalMatrixChild.z_ax;
00161     //--- get the pin fixed to the second body
00162     const wVector& dir1 = globalMatrixParent.x_ax;
00163 
00164     //--- construct an orthogonal coordinate system with these two vectors
00165     wVector dir2 = ( dir0 * dir1 );
00166     dir2.normalize();
00167     wVector dir3( dir2 * dir0 );
00168     dir3.normalize();
00169 
00170     const wVector& p0 = globalMatrixChild.w_pos;
00171     const wVector& p1 = globalMatrixParent.w_pos;
00172 
00173     real len = 100.0;
00174     wVector q0( p0 + dir3.scale(len) );
00175     wVector q1( p1 + dir1.scale(len) );
00176 
00177     //--- Restrict the movement on the centre of the joint along all tree orthonormal direction
00178     NewtonUserJointAddLinearRow( priv->joint, &p0[0], &p1[0], &dir0[0] );
00179     NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
00180     NewtonUserJointAddLinearRow( priv->joint, &p0[0], &p1[0], &dir1[0] );
00181     NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
00182     NewtonUserJointAddLinearRow( priv->joint, &p0[0], &p1[0], &dir2[0] );
00183     NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
00184     //NewtonUserJointAddAngularRow( priv->joint, 0.0, &dir2[0] );
00185     //NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
00186     NewtonUserJointAddLinearRow( priv->joint, &q0[0], &q1[0], &dir0[0] );
00187     NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
00188 
00189     //--- force will be used to set-up max and min torque newton will use to
00190     //--- resolve the constraint and accel to the double of appliedForce
00191     //--- this means that Newton will apply exactly forse amount of torque around
00192     //--- the axis
00193     real force = fabs( firstDof->appliedForce() );
00194     real accel = 2.0*firstDof->appliedForce();
00195 
00196     //--- calculate the rotation assuming the local X axis of joint frame as 0
00197     //--- and following the right-hand convention
00198     real sinAngle = (globalMatrixParent.x_ax * globalMatrixChild.x_ax) % globalMatrixChild.z_ax;
00199     real cosAngle = globalMatrixChild.x_ax % globalMatrixParent.x_ax;
00200     real angle = atan2( sinAngle, cosAngle );
00201     real vel = (angle - firstDof->position()) / timestep;
00202     firstDof->setPosition( angle );
00203     firstDof->setVelocity( vel );
00204     if ( firstDof->isLimited() ) {
00205         real lolimit;
00206         real hilimit;
00207         firstDof->limits( lolimit, hilimit );
00208         if ( angle < lolimit ) {
00209             real relAngle = lolimit - angle; // - lolimit;
00210             NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.z_ax[0] );
00211             NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
00212             //--- this allow the joint to move back freely
00213             NewtonUserJointSetRowMinimumFriction( priv->joint, 0.0 );
00214         } else if ( angle > hilimit ) {
00215             real relAngle = hilimit - angle; // - hilimit;
00216             NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.z_ax[0] );
00217             NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
00218             //--- this allow the joint to move forth freely
00219             NewtonUserJointSetRowMaximumFriction( priv->joint, 0.0 );
00220         } else {
00221             NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.z_ax[0] );
00222             NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
00223             NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
00224             NewtonUserJointSetRowAcceleration( priv->joint, accel );
00225             NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
00226         }
00227     } else {
00228         NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.z_ax[0] );
00229         NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
00230         NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
00231         NewtonUserJointSetRowAcceleration( priv->joint, accel );
00232         NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
00233     }
00234 
00235     //--- the secondDOF results rotate of angle respect to its local X axis
00236     wMatrix tmp = wMatrix::identity();
00237     tmp.x_ax = globalMatrixParent.z_ax;
00238     tmp.y_ax = globalMatrixParent.y_ax;
00239     tmp.z_ax = globalMatrixParent.x_ax;
00240     tmp = tmp * wMatrix( wQuaternion( tmp.x_ax, angle ), wVector(0,0,0) );
00241     tmp.sanitifize();
00242     secondDof->setAxis( tmp.z_ax );
00243     secondDof->setXAxis( tmp.x_ax );
00244     secondDof->setYAxis( tmp.y_ax );
00245     secondDof->setCentre( globalMatrixParent.w_pos );
00246 //  secondDof->setAxis( globalMatrixChild.x_ax );
00247 //  secondDof->setXAxis( globalMatrixParent.z_ax );
00248 //  secondDof->setYAxis( globalMatrixChild.y_ax );
00249 //  secondDof->setCentre( globalMatrixParent.w_pos );   
00250 
00251     //--- force will be used to set-up max and min torque newton will use to
00252     //--- resolve the constraint and accel to the double of appliedForce
00253     //--- this means that Newton will apply exactly forse amount of torque around
00254     //--- the axis
00255     force = fabs( secondDof->appliedForce() );
00256     accel = 2.0*secondDof->appliedForce();
00257 
00258     //--- calculate the rotation assuming the local X axis of joint frame as 0
00259     //--- and following the right-hand convention
00260     //----------- Not sure check the second Axis rotation reference
00261     sinAngle = (globalMatrixParent.z_ax * globalMatrixChild.z_ax) % globalMatrixChild.x_ax;
00262     cosAngle = globalMatrixChild.z_ax % globalMatrixParent.z_ax;
00263     angle = atan2( sinAngle, cosAngle );
00264     vel = (angle - secondDof->position()) / timestep;
00265     secondDof->setPosition( angle );
00266     secondDof->setVelocity( vel );
00267     if ( secondDof->isLimited() ) {
00268         real lolimit;
00269         real hilimit;
00270         secondDof->limits( lolimit, hilimit );
00271         if ( angle < lolimit ) {
00272             real relAngle = lolimit - angle; // - lolimit;
00273             NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.x_ax[0] );
00274             NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
00275             //--- this allow the joint to move back freely
00276             NewtonUserJointSetRowMinimumFriction( priv->joint, 0.0 );
00277         } else if ( angle > hilimit ) {
00278             real relAngle = hilimit - angle; // - hilimit;
00279             NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.x_ax[0] );
00280             NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
00281             //--- this allow the joint to move forth freely
00282             NewtonUserJointSetRowMaximumFriction( priv->joint, 0.0 );
00283         } else {
00284             NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.x_ax[0] );
00285             NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
00286             NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
00287             NewtonUserJointSetRowAcceleration( priv->joint, accel );
00288             NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
00289         }
00290     } else {
00291         NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.x_ax[0] );
00292         NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
00293         NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
00294         NewtonUserJointSetRowAcceleration( priv->joint, accel );
00295         NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
00296     }
00297     
00298 #endif
00299 
00300 }
00301 
00302 } // end namespace farsa