[Opal-commits] opal/src ServoMotor.cpp,1.18,1.19 ServoMotor.h,1.25,1.26 Vec3r.cpp,1.2,1.3 testMatrix
Status: Inactive
Brought to you by:
tylerstreeter
|
From: Olex <ole...@us...> - 2005-12-02 05:47:29
|
Update of /cvsroot/opal/opal/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv26719/src Modified Files: ServoMotor.cpp ServoMotor.h Vec3r.cpp testMatrix44r.cpp testVec3r.cpp Log Message: Fixes in ServoMotor, Vec3r to handle float point errors. Updated unit test framework. Index: ServoMotor.cpp =================================================================== RCS file: /cvsroot/opal/opal/src/ServoMotor.cpp,v retrieving revision 1.18 retrieving revision 1.19 diff -C2 -d -r1.18 -r1.19 *** ServoMotor.cpp 26 Jun 2005 16:57:35 -0000 1.18 --- ServoMotor.cpp 2 Dec 2005 05:47:21 -0000 1.19 *************** *** 1,28 **** /************************************************************************* ! * * ! * Open Physics Abstraction Layer * ! * Copyright (C) 2004-2005 * ! * Alan Fischer ala...@gm... * ! * Andres Reinot an...@re... * ! * Tyler Streeter tyl...@gm... * ! * All rights reserved. * ! * Web: opal.sourceforge.net * ! * * ! * This library is free software; you can redistribute it and/or * ! * modify it under the terms of EITHER: * ! * (1) The GNU Lesser General Public License as published by the Free * ! * Software Foundation; either version 2.1 of the License, or (at * ! * your option) any later version. The text of the GNU Lesser * ! * General Public License is included with this library in the * ! * file license-LGPL.txt. * ! * (2) The BSD-style license that is included with this library in * ! * the file license-BSD.txt. * ! * * ! * This library 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 files * ! * license-LGPL.txt and license-BSD.txt for more details. * ! * * ! *************************************************************************/ #include "ServoMotor.h" --- 1,28 ---- /************************************************************************* ! * * ! * Open Physics Abstraction Layer * ! * Copyright (C) 2004-2005 * ! * Alan Fischer ala...@gm... * ! * Andres Reinot an...@re... * ! * Tyler Streeter tyl...@gm... * ! * All rights reserved. * ! * Web: opal.sourceforge.net * ! * * ! * This library is free software; you can redistribute it and/or * ! * modify it under the terms of EITHER: * ! * (1) The GNU Lesser General Public License as published by the Free * ! * Software Foundation; either version 2.1 of the License, or (at * ! * your option) any later version. The text of the GNU Lesser * ! * General Public License is included with this library in the * ! * file license-LGPL.txt. * ! * (2) The BSD-style license that is included with this library in * ! * the file license-BSD.txt. * ! * * ! * This library 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 files * ! * license-LGPL.txt and license-BSD.txt for more details. * ! * * ! *************************************************************************/ #include "ServoMotor.h" *************** *** 30,231 **** namespace opal { ! ServoMotor::ServoMotor() ! : Motor() ! { ! // "mData" will be initialized in its own constructor. ! } ! ServoMotor::~ServoMotor() ! { ! // We must set the Joint's desired vel and max force to 0 since ! // this Motor won't apply to it anymore. This is only necessary ! // if this Motor is enabled. ! if (mData.enabled && mData.joint) ! { ! mData.joint->internal_setDesiredVel(mData.jointAxisNum, 0); ! mData.joint->internal_setMaxTorque(mData.jointAxisNum, 0); ! } ! } ! void ServoMotor::init(const ServoMotorData& data) ! { ! if (mInitCalled) ! { ! // If the Servo is already in operation, we first need to ! // set the old Joint's desired vel and max force to 0. The ! // following function call will automatically handle this ! // when set to false. ! setEnabled(false); ! } ! assert(data.joint); ! assert(data.jointAxisNum >= 0 && ! data.jointAxisNum < data.joint->getNumAxes()); ! Motor::init(); ! assert(data.joint->isRotational(data.jointAxisNum)); ! mData = data; ! setEnabled(mData.enabled); ! } ! const ServoMotorData& ServoMotor::getData()const ! { ! return mData; ! } ! MotorType ServoMotor::getType()const ! { ! return mData.getType(); ! } ! void ServoMotor::setName(const std::string& name) ! { ! mData.name = name; ! } ! const std::string& ServoMotor::getName()const ! { ! return mData.name; ! } ! bool ServoMotor::isEnabled()const ! { ! return mData.enabled; ! } ! void ServoMotor::setEnabled(bool e) ! { ! //if (!mInitCalled) ! //{ ! // return; ! //} ! mData.enabled = e; ! if (mData.joint) ! { ! if (e) ! { ! mData.joint->internal_setDesiredVel(mData.jointAxisNum, ! mData.desiredVel); ! mData.joint->internal_setMaxTorque(mData.jointAxisNum, ! mData.maxTorque); ! } ! else ! { ! mData.joint->internal_setDesiredVel(mData.jointAxisNum, 0); ! mData.joint->internal_setMaxTorque(mData.jointAxisNum, 0); ! } ! } ! } ! void ServoMotor::internal_update() ! { ! if (mData.enabled && mData.joint) ! { ! // Make sure both Solids are awake at this point. ! mData.joint->wakeSolids(); ! if (DESIRED_ANGLE_MODE == mData.mode) ! { ! // No longer support linear degrees of freedom. ! //if (true == mData.joint->isRotational(mData.jointAxisNum)) ! //{ ! 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); ! //} ! //else ! //{ ! // // This axis must be a linear degree of freedom. ! // real velocity = mData.desiredPos - ! // mData.joint->getState(mData.jointAxisNum); ! // mData.joint->internal_setDesiredVel(mData.jointAxisNum, ! // mData.restoreSpeed * velocity); ! //} ! } ! else ! { ! // Nothing to do for desired velocity mode; the Joint's ! // desired velocity should already handle this. ! } ! } ! } ! void ServoMotor::setDesiredAngle(real a) ! { ! assert(a >= mData.joint->getLowLimit(mData.jointAxisNum) ! && a <= mData.joint->getHighLimit(mData.jointAxisNum)); ! mData.desiredAngle = a; ! } ! void ServoMotor::setDesiredAngleNorm(real a) ! { ! assert(a >= 0.0 && a <= 1.0); ! real lowLimit = mData.joint->getLowLimit(mData.jointAxisNum); ! real highLimit = mData.joint->getHighLimit(mData.jointAxisNum); ! // map the pos value onto the joint limits ! mData.desiredAngle = a * (highLimit - lowLimit) + lowLimit; ! // Keep desired angle slightly away from the limit to avoid jitter. ! // TODO: fix this; this should just keep the thing away from the ! // limit when it's close, not all the time. ! mData.desiredAngle *= (real)0.99; ! } ! real ServoMotor::getDesiredAngle()const ! { ! return mData.desiredAngle; ! } ! void ServoMotor::setDesiredVel(real vel) ! { ! mData.desiredVel = vel; ! mData.joint->internal_setDesiredVel(mData.jointAxisNum, vel); ! } ! real ServoMotor::getDesiredVel()const ! { ! return mData.desiredVel; ! } ! void ServoMotor::setMaxTorque(real max) ! { ! mData.maxTorque = max; ! mData.joint->internal_setMaxTorque(mData.jointAxisNum, max); ! } ! real ServoMotor::getMaxTorque()const ! { ! return mData.maxTorque; ! } ! void ServoMotor::setRestoreSpeed(real speed) ! { ! mData.restoreSpeed = speed; ! } ! real ServoMotor::getRestoreSpeed()const ! { ! return mData.restoreSpeed; ! } ! bool ServoMotor::internal_dependsOnJoint(Joint* j) ! { ! if (j == mData.joint) ! { ! return true; ! } ! else ! { ! return false; ! } ! } } --- 30,244 ---- namespace opal { ! ServoMotor::ServoMotor() ! : Motor() ! { ! // "mData" will be initialized in its own constructor. ! } ! ServoMotor::~ServoMotor() ! { ! // We must set the Joint's desired vel and max force to 0 since ! // this Motor won't apply to it anymore. This is only necessary ! // if this Motor is enabled. ! if ( mData.enabled && mData.joint ) ! { ! mData.joint->internal_setDesiredVel( mData.jointAxisNum, 0 ); ! mData.joint->internal_setMaxTorque( mData.jointAxisNum, 0 ); ! } ! } ! void ServoMotor::init( const ServoMotorData& data ) ! { ! if ( mInitCalled ) ! { ! // If the Servo is already in operation, we first need to ! // set the old Joint's desired vel and max force to 0. The ! // following function call will automatically handle this ! // when set to false. ! setEnabled( false ); ! } ! assert( data.joint ); ! assert( data.jointAxisNum >= 0 && ! data.jointAxisNum < data.joint->getNumAxes() ); ! Motor::init(); ! assert( data.joint->isRotational( data.jointAxisNum ) ); ! mData = data; ! setEnabled( mData.enabled ); ! } ! const ServoMotorData& ServoMotor::getData() const ! { ! return mData; ! } ! MotorType ServoMotor::getType() const ! { ! return mData.getType(); ! } ! void ServoMotor::setName( const std::string& name ) ! { ! mData.name = name; ! } ! const std::string& ServoMotor::getName() const ! { ! return mData.name; ! } ! bool ServoMotor::isEnabled() const ! { ! return mData.enabled; ! } ! void ServoMotor::setEnabled( bool e ) ! { ! //if (!mInitCalled) ! //{ ! // return; ! //} ! mData.enabled = e; ! if ( mData.joint ) ! { ! if ( e ) ! { ! mData.joint->internal_setDesiredVel( mData.jointAxisNum, ! mData.desiredVel ); ! mData.joint->internal_setMaxTorque( mData.jointAxisNum, ! mData.maxTorque ); ! } ! else ! { ! mData.joint->internal_setDesiredVel( mData.jointAxisNum, 0 ); ! mData.joint->internal_setMaxTorque( mData.jointAxisNum, 0 ); ! } ! } ! } ! void ServoMotor::internal_update() ! { ! if ( mData.enabled && mData.joint ) ! { ! // Make sure both Solids are awake at this point. ! mData.joint->wakeSolids(); ! if ( DESIRED_ANGLE_MODE == mData.mode ) ! { ! // No longer support linear degrees of freedom. ! //if (true == mData.joint->isRotational(mData.jointAxisNum)) ! //{ ! 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 ); ! //} ! //else ! //{ ! // // This axis must be a linear degree of freedom. ! // real velocity = mData.desiredPos - ! // mData.joint->getState(mData.jointAxisNum); ! // mData.joint->internal_setDesiredVel(mData.jointAxisNum, ! // mData.restoreSpeed * velocity); ! //} ! } ! else ! { ! // Nothing to do for desired velocity mode; the Joint's ! // desired velocity should already handle this. ! } ! } ! } ! void ServoMotor::setDesiredAngle( real a ) ! { ! // this clamping is needed since there are sometimes float point errors ! real low = mData.joint->getLowLimit( mData.jointAxisNum ); ! if ( a < low ) ! { ! a = low; ! } ! real high = mData.joint->getHighLimit( mData.jointAxisNum ); ! if ( a > high ) ! { ! a = high; ! } ! mData.desiredAngle = a; ! } ! void ServoMotor::setDesiredAngleNorm( real a ) ! { ! if ( a < 0 ) ! a = 0; ! if ( a > 1 ) ! a = 1; ! real lowLimit = mData.joint->getLowLimit( mData.jointAxisNum ); ! real highLimit = mData.joint->getHighLimit( mData.jointAxisNum ); ! // map the pos value onto the joint limits ! mData.desiredAngle = a * ( highLimit - lowLimit ) + lowLimit; ! // Keep desired angle slightly away from the limit to avoid jitter. ! // @todo: fix this; this should just keep the thing away from the ! // limit when it's close, not all the time. ! mData.desiredAngle *= ( real ) 0.99; ! } ! real ServoMotor::getDesiredAngle() const ! { ! return mData.desiredAngle; ! } ! void ServoMotor::setDesiredVel( real vel ) ! { ! mData.desiredVel = vel; ! mData.joint->internal_setDesiredVel( mData.jointAxisNum, vel ); ! } ! real ServoMotor::getDesiredVel() const ! { ! return mData.desiredVel; ! } ! void ServoMotor::setMaxTorque( real max ) ! { ! mData.maxTorque = max; ! mData.joint->internal_setMaxTorque( mData.jointAxisNum, max ); ! } ! real ServoMotor::getMaxTorque() const ! { ! return mData.maxTorque; ! } ! void ServoMotor::setRestoreSpeed( real speed ) ! { ! mData.restoreSpeed = speed; ! } ! real ServoMotor::getRestoreSpeed() const ! { ! return mData.restoreSpeed; ! } ! ! bool ServoMotor::internal_dependsOnJoint( Joint* j ) ! { ! if ( j == mData.joint ) ! { ! return true; ! } ! else ! { ! return false; ! } ! } } Index: testMatrix44r.cpp =================================================================== RCS file: /cvsroot/opal/opal/src/testMatrix44r.cpp,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** testMatrix44r.cpp 1 Dec 2005 23:55:21 -0000 1.1 --- testMatrix44r.cpp 2 Dec 2005 05:47:21 -0000 1.2 *************** *** 41,45 **** Matrix44r m; m.setQuaternion( 0, 0, 0, 1 ); ! QT_CHECK_EQUAL( m.getEulerXYZ(), Vec3r( 0, 0, 180 ) ); } --- 41,45 ---- Matrix44r m; m.setQuaternion( 0, 0, 0, 1 ); ! QT_CHECK_EQUAL( m.getEulerXYZ(), Vec3r( 0, 0, -180 ) ); } *************** *** 48,52 **** Matrix44r m; m.setQuaternion( Quaternion( 0, 0, 0, 1 ) ); ! QT_CHECK_EQUAL( m.getEulerXYZ(), Vec3r( 0, 0, 180 ) ); } } --- 48,52 ---- Matrix44r m; m.setQuaternion( Quaternion( 0, 0, 0, 1 ) ); ! QT_CHECK_EQUAL( m.getEulerXYZ(), Vec3r( 0, 0, -180 ) ); } } Index: ServoMotor.h =================================================================== RCS file: /cvsroot/opal/opal/src/ServoMotor.h,v retrieving revision 1.25 retrieving revision 1.26 diff -C2 -d -r1.25 -r1.26 *** ServoMotor.h 23 Mar 2005 04:04:30 -0000 1.25 --- ServoMotor.h 2 Dec 2005 05:47:21 -0000 1.26 *************** *** 73,80 **** --- 73,82 ---- /// Sets the desired angle to a value between the Joint axis' /// limits. + /// Clamps given angle into allowed range for the joint. virtual void OPAL_CALL setDesiredAngle(real a); /// Sets the desired angle to a value between 0.0 and 1.0 which /// will be mapped to the Joint axis' limits. + /// Clamps given angle into [0,1] range. virtual void OPAL_CALL setDesiredAngleNorm(real a); Index: testVec3r.cpp =================================================================== RCS file: /cvsroot/opal/opal/src/testVec3r.cpp,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** testVec3r.cpp 1 Dec 2005 23:55:21 -0000 1.2 --- testVec3r.cpp 2 Dec 2005 05:47:21 -0000 1.3 *************** *** 41,47 **** { Vec3r v; ! QT_CHECK_EQUAL( v.x, 0 ); ! QT_CHECK_EQUAL( v.y, 0 ); ! QT_CHECK_EQUAL( v.z, 0 ); } --- 41,47 ---- { Vec3r v; ! QT_CHECK_CLOSE( v.x, 0 ); ! QT_CHECK_CLOSE( v.y, 0 ); ! QT_CHECK_CLOSE( v.z, 0 ); } *************** *** 50,56 **** Vec3r copy( 1, 2, 3 ); Vec3r v( copy ); ! QT_CHECK_EQUAL( v.x, 1 ); ! QT_CHECK_EQUAL( v.y, 2 ); ! QT_CHECK_EQUAL( v.z, 3 ); } --- 50,56 ---- Vec3r copy( 1, 2, 3 ); Vec3r v( copy ); ! QT_CHECK_CLOSE( v.x, 1 ); ! QT_CHECK_CLOSE( v.y, 2 ); ! QT_CHECK_CLOSE( v.z, 3 ); } *************** *** 58,64 **** { Vec3r v( 1, 2, 3 ); ! QT_CHECK_EQUAL( v.x, 1 ); ! QT_CHECK_EQUAL( v.y, 2 ); ! QT_CHECK_EQUAL( v.z, 3 ); } --- 58,64 ---- { Vec3r v( 1, 2, 3 ); ! QT_CHECK_CLOSE( v.x, 1 ); ! QT_CHECK_CLOSE( v.y, 2 ); ! QT_CHECK_CLOSE( v.z, 3 ); } *************** *** 66,72 **** { Vec3r v = Vec3r( 1, 2, 3 ); ! QT_CHECK_EQUAL( v.x, 1 ); ! QT_CHECK_EQUAL( v.y, 2 ); ! QT_CHECK_EQUAL( v.z, 3 ); } --- 66,72 ---- { Vec3r v = Vec3r( 1, 2, 3 ); ! QT_CHECK_CLOSE( v.x, 1 ); ! QT_CHECK_CLOSE( v.y, 2 ); ! QT_CHECK_CLOSE( v.z, 3 ); } *************** *** 75,81 **** real d[] = {1, 2, 3}; Vec3r v( d ); ! QT_CHECK_EQUAL( v.x, 1 ); ! QT_CHECK_EQUAL( v.y, 2 ); ! QT_CHECK_EQUAL( v.z, 3 ); } --- 75,81 ---- real d[] = {1, 2, 3}; Vec3r v( d ); ! QT_CHECK_CLOSE( v.x, 1 ); ! QT_CHECK_CLOSE( v.y, 2 ); ! QT_CHECK_CLOSE( v.z, 3 ); } *************** *** 84,90 **** Vec3r v; v.set( 1, 2, 3 ); ! QT_CHECK_EQUAL( v.x, 1 ); ! QT_CHECK_EQUAL( v.y, 2 ); ! QT_CHECK_EQUAL( v.z, 3 ); } --- 84,90 ---- Vec3r v; v.set( 1, 2, 3 ); ! QT_CHECK_CLOSE( v.x, 1 ); ! QT_CHECK_CLOSE( v.y, 2 ); ! QT_CHECK_CLOSE( v.z, 3 ); } *************** *** 94,100 **** Vec3r v; v.set( d ); ! QT_CHECK_EQUAL( v.x, 1 ); ! QT_CHECK_EQUAL( v.y, 2 ); ! QT_CHECK_EQUAL( v.z, 3 ); } --- 94,100 ---- Vec3r v; v.set( d ); ! QT_CHECK_CLOSE( v.x, 1 ); ! QT_CHECK_CLOSE( v.y, 2 ); ! QT_CHECK_CLOSE( v.z, 3 ); } *************** *** 103,109 **** Vec3r v( 1, 2, 3 ); ! QT_CHECK_EQUAL( v[ 0 ], 1 ); ! QT_CHECK_EQUAL( v[ 1 ], 2 ); ! QT_CHECK_EQUAL( v[ 2 ], 3 ); } --- 103,109 ---- Vec3r v( 1, 2, 3 ); ! QT_CHECK_CLOSE( v[ 0 ], 1 ); ! QT_CHECK_CLOSE( v[ 1 ], 2 ); ! QT_CHECK_CLOSE( v[ 2 ], 3 ); } *************** *** 111,115 **** { Vec3r v( 1, 2, 3 ); ! QT_CHECK_EQUAL( v.lengthSquared(), 1 + 4 + 9 ); } --- 111,115 ---- { Vec3r v( 1, 2, 3 ); ! QT_CHECK_CLOSE( v.lengthSquared(), 1 + 4 + 9 ); } *************** *** 117,121 **** { Vec3r v( -1, -2, -3 ); ! QT_CHECK_EQUAL( v.lengthSquared(), 1 + 4 + 9 ); } --- 117,121 ---- { Vec3r v( -1, -2, -3 ); ! QT_CHECK_CLOSE( v.lengthSquared(), 1 + 4 + 9 ); } *************** *** 123,127 **** { Vec3r v( 1, 2, 3 ); ! QT_CHECK_EQUAL( v.length(), sqrt( 1 + 4 + 9 ) ); } --- 123,127 ---- { Vec3r v( 1, 2, 3 ); ! QT_CHECK_CLOSE( v.length(), sqrt( 1 + 4 + 9 ) ); } *************** *** 129,133 **** { Vec3r v( -1, -2, -3 ); ! QT_CHECK_EQUAL( v.length(), sqrt( 1 + 4 + 9 ) ); } --- 129,133 ---- { Vec3r v( -1, -2, -3 ); ! QT_CHECK_CLOSE( v.length(), sqrt( 1 + 4 + 9 ) ); } Index: Vec3r.cpp =================================================================== RCS file: /cvsroot/opal/opal/src/Vec3r.cpp,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** Vec3r.cpp 1 Dec 2005 02:04:54 -0000 1.2 --- Vec3r.cpp 2 Dec 2005 05:47:21 -0000 1.3 *************** *** 28,32 **** // class header ! #include "Vec3r.h" //#pragma implementation --- 28,32 ---- // class header ! #include "Vec3r.h" //#pragma implementation *************** *** 83,94 **** { case 0: ! return x; case 1: ! return y; case 2: ! return z; default: ! assert( i < 3 ); ! return z; } } --- 83,94 ---- { case 0: ! return x; case 1: ! return y; case 2: ! return z; default: ! assert( i < 3 ); ! return z; } } *************** *** 99,110 **** { case 0: ! return x; case 1: ! return y; case 2: ! return z; default: ! assert( i < 3 ); ! return z; } } --- 99,110 ---- { case 0: ! return x; case 1: ! return y; case 2: ! return z; default: ! assert( i < 3 ); ! return z; } } *************** *** 177,181 **** bool Vec3r::operator==( const Vec3r & v ) { ! return ( x == v.x && y == v.y && z == v.z ); } --- 177,181 ---- bool Vec3r::operator==( const Vec3r & v ) { ! return ( areEqual( x, v.x ) && areEqual( y , v.y ) && areEqual( z , v.z ) ); } *************** *** 189,193 **** bool Vec3r::operator!=( const Vec3r & v ) { ! return ( x != v.x || y != v.y || z != v.z ); } --- 189,193 ---- bool Vec3r::operator!=( const Vec3r & v ) { ! return ( !areEqual( x, v.x ) || !areEqual( y , v.y ) || !areEqual( z , v.z ) ); } |