[Opal-commits] opal/src Matrix44r.h,1.34,1.35 ServoMotor.cpp,1.17,1.18
Status: Inactive
Brought to you by:
tylerstreeter
|
From: Olex <ole...@us...> - 2005-06-26 16:57:43
|
Update of /cvsroot/opal/opal/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv3634/src Modified Files: Matrix44r.h ServoMotor.cpp Log Message: Fixed ServoMotor's calculation of the angular velocity needed to bring the joint to the desired angle. Cleaned up some warning-generating code in Matrix44r::getQuaternion() Index: ServoMotor.cpp =================================================================== RCS file: /cvsroot/opal/opal/src/ServoMotor.cpp,v retrieving revision 1.17 retrieving revision 1.18 diff -C2 -d -r1.17 -r1.18 *** ServoMotor.cpp 4 Jun 2005 20:20:21 -0000 1.17 --- ServoMotor.cpp 26 Jun 2005 16:57:35 -0000 1.18 *************** *** 133,136 **** --- 133,140 ---- real velocity = mData.desiredAngle - mData.joint->getAngle(mData.jointAxisNum); + if ( velocity > 180.0 ) + velocity = -360 + velocity; + if ( velocity < -180.0 ) + velocity = 360 + velocity; mData.joint->internal_setDesiredVel(mData.jointAxisNum, mData.restoreSpeed * velocity); Index: Matrix44r.h =================================================================== RCS file: /cvsroot/opal/opal/src/Matrix44r.h,v retrieving revision 1.34 retrieving revision 1.35 diff -C2 -d -r1.34 -r1.35 *** Matrix44r.h 27 Apr 2005 02:28:18 -0000 1.34 --- Matrix44r.h 26 Jun 2005 16:57:34 -0000 1.35 *************** *** 287,292 **** if((*this)(0,0) > (*this)(1,1) && (*this)(0,0) > (*this)(2,2)) { ! real s = 2.0f * sqrtf(1.0f + (*this)(0,0) - (*this)(1,1) - ! (*this)(2,2)); q[1] = 0.25f * s; q[2] = ((*this)(0,1) + (*this)(1,0)) / s; --- 287,292 ---- if((*this)(0,0) > (*this)(1,1) && (*this)(0,0) > (*this)(2,2)) { ! real s = 2.0 * sqrt(static_cast<real>(1.0 + (*this)(0,0) - (*this)(1,1) - ! (*this)(2,2))); q[1] = 0.25f * s; q[2] = ((*this)(0,1) + (*this)(1,0)) / s; *************** *** 296,301 **** else if ((*this)(1,1) > (*this)(2,2)) { ! real s = 2.0f * sqrtf(1.0f + (*this)(1,1) - (*this)(0,0) - ! (*this)(2,2)); q[1] = ((*this)(0,1) + (*this)(1,0)) / s; q[2] = 0.25f * s; --- 296,301 ---- else if ((*this)(1,1) > (*this)(2,2)) { ! real s = 2.0 * sqrt(static_cast<real>(1.0 + (*this)(1,1) - (*this)(0,0) - ! (*this)(2,2))); q[1] = ((*this)(0,1) + (*this)(1,0)) / s; q[2] = 0.25f * s; *************** *** 305,310 **** else { ! real s = 2.0f * sqrtf(1.0f + (*this)(2,2) - (*this)(0,0) - ! (*this)(1,1)); q[1] = ((*this)(0,2) + (*this)(2,0)) / s; q[2] = ((*this)(1,2) + (*this)(2,1)) / s; --- 305,310 ---- else { ! real s = 2.0 * sqrt(static_cast<real>(1.0 + (*this)(2,2) - (*this)(0,0) - ! (*this)(1,1))); q[1] = ((*this)(0,2) + (*this)(2,0)) / s; q[2] = ((*this)(1,2) + (*this)(2,1)) / s; |