From 8d55be75ba5e7f6ad42c52efcf7e3f6422314ee7 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 1 Jul 2015 23:09:51 -0400 Subject: [PATCH] improved joint warnings for Single and MultiDofJoints --- dart/dynamics/SingleDofJoint.cpp | 281 +++++++++++---------------- dart/dynamics/detail/MultiDofJoint.h | 261 ++++++++++++------------- 2 files changed, 241 insertions(+), 301 deletions(-) diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index e65499994a529..971e1452724ee 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -42,6 +42,22 @@ #include "dart/dynamics/Skeleton.h" #include "dart/dynamics/DegreeOfFreedom.h" +#define SINGLEDOFJOINT_REPORT_DIM_MISMATCH( func, arg ) \ + dterr << "[SingleDofJoint::" #func "] Size of " << #arg << "[" << arg .size()\ + << "] should be exactly 1 for Joint named [" << getName() << "].\n"; \ + assert(false); + +#define SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( func, index ) \ + dterr << "[SingleDofJoint::" #func "] The index [" << index \ + << "] is out of range for Joint named [" << getName() \ + << "] which only has a zeroth index.\n"; \ + assert(false); + +#define SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR( func ) \ + dterr << "[SingleDofJoint::" # func "] Unsupported actuator type (" \ + << mJointP.mActuatorType << ") for Joint [" << getName() << "].\n"; \ + assert(false); + namespace dart { namespace dynamics { @@ -162,9 +178,7 @@ size_t SingleDofJoint::getIndexInSkeleton(size_t _index) const { if (_index != 0) { - dterr << "[SingleDofJoint::getIndexInSkeleton] index (" << _index - << ") may only be 0\n"; - assert(false); + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getIndexInSkeleton, _index ); return 0; } @@ -176,9 +190,7 @@ size_t SingleDofJoint::getIndexInTree(size_t _index) const { if (_index != 0) { - dterr << "[SingleDofJoint::getIndexInTree] index (" << _index - << ") may only be 0\n"; - assert(false); + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getIndexInTree, _index ); return 0; } @@ -191,9 +203,7 @@ DegreeOfFreedom* SingleDofJoint::getDof(size_t _index) if (0 == _index) return mDof; - dterr << "[SingleDofJoint::getDof] Attempting to access index (" << _index - << ") of a SingleDofJoint!\n"; - assert(false); + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getDof, _index ); return nullptr; } @@ -203,9 +213,7 @@ const DegreeOfFreedom* SingleDofJoint::getDof(size_t _index) const if (0 == _index) return mDof; - dterr << "[SingleDofJoint::getDof] Attempting to access index (" << _index - << ") of a SingleDofJoint!\n"; - assert(false); + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getDof, _index ); return nullptr; } @@ -216,9 +224,10 @@ const std::string& SingleDofJoint::setDofName(size_t _index, { if (0 < _index) { - dtwarn << "[SingleDofJoint::getDofName] Attempting to set the name of DOF " - << "index " << _index << ", which is out of bounds. We will set " - << "the name of DOF index 0 instead\n"; + dterr << "[SingleDofJoint::getDofName] Attempting to set the name of DOF " + << "index " << _index << ", which is out of bounds for the Joint [" + << getName() << "]. We will set the name of DOF index 0 instead\n"; + assert(false); _index = 0; } @@ -243,9 +252,10 @@ const std::string& SingleDofJoint::setDofName(size_t _index, void SingleDofJoint::preserveDofName(size_t _index, bool _preserve) { if (0 < _index) - dtwarn << "[SingleDofJoint::preserveDofName] Attempting to preserve the " - << "name of DOF index " << _index << ", which is out of bounds. We " - << "will preserve the name of DOF index 0 instead\n"; + { + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( preserveDofName, _index ); + return; + } mSingleDofP.mPreserveDofName = _preserve; } @@ -254,9 +264,9 @@ void SingleDofJoint::preserveDofName(size_t _index, bool _preserve) bool SingleDofJoint::isDofNamePreserved(size_t _index) const { if (0 < _index) - dtwarn << "[SingleDofJoint::isDofNamePreserved] Requesting whether DOF " - << "index " << _index << " is preserved, but this is out of bounds. " - << "We will return the result of DOF index 0 instead\n"; + { + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( isDofNamePreserved, _index ); + } return mSingleDofP.mPreserveDofName; } @@ -265,8 +275,12 @@ bool SingleDofJoint::isDofNamePreserved(size_t _index) const const std::string& SingleDofJoint::getDofName(size_t _index) const { if (0 < _index) - dtwarn << "[SingleDofJoint::getDofName] Requested name of DOF index " - << _index << ", which is out of bounds. Returning name of index 0\n"; + { + dterr << "[SingleDofJoint::getDofName] Requested name of DOF index [" + << _index << "] in Joint [" << getName() << "], which is out of " + << "bounds. Returning the name of the only DOF available.\n"; + assert(false); + } return mSingleDofP.mDofName; } @@ -276,8 +290,7 @@ void SingleDofJoint::setCommand(size_t _index, double _command) { if (_index != 0) { - dterr << "[SingleDofJoint::setCommand]: index[" << _index - << "] out of range" << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setCommand, _index ); return; } @@ -289,8 +302,12 @@ void SingleDofJoint::setCommand(size_t _index, double _command) mSingleDofP.mForceUpperLimit); break; case PASSIVE: - dtwarn << "[SingleDofJoint::setCommand] Attempting to set command for " - << "PASSIVE joint." << std::endl; + if(_command != 0.0) + { + dtwarn << "[SingleDofJoint::setCommand] Attempting to set a non-zero (" + << _command << ") command for a PASSIVE joint [" << getName() + << "].\n"; + } mCommand = _command; break; case SERVO: @@ -310,8 +327,12 @@ void SingleDofJoint::setCommand(size_t _index, double _command) // TODO: This possibly makes the acceleration to exceed the limits. break; case LOCKED: - dtwarn << "[SingleDofJoint::setCommand] Attempting to set command for " - << "LOCKED joint." << std::endl; + if(_command != 0.0) + { + dtwarn << "[SingleDofJoint::setCommand] Attempting to set a non-zero (" + << _command << ") command for a LOCKED joint [" << getName() + << "].\n"; + } mCommand = _command; break; default: @@ -325,8 +346,7 @@ double SingleDofJoint::getCommand(size_t _index) const { if (_index != 0) { - dterr << "[SingleDofJoint::getCommand]: index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getCommand, _index ); return 0.0; } @@ -338,49 +358,11 @@ void SingleDofJoint::setCommands(const Eigen::VectorXd& _commands) { if (static_cast(_commands.size()) != getNumDofs()) { - dterr << "[SingleDofJoint::setCommands]: commands's size[" - << _commands.size() << "] is different with the dof [" << getNumDofs() - << "]" << std::endl; + SINGLEDOFJOINT_REPORT_DIM_MISMATCH( setCommands, _commands ); return; } - switch (mJointP.mActuatorType) - { - case FORCE: - mCommand = math::clip(_commands[0], - mSingleDofP.mForceLowerLimit, - mSingleDofP.mForceUpperLimit); - break; - case PASSIVE: - dtwarn << "[SingleDofJoint::setCommands] Attempting to set command for " - << "PASSIVE joint." << std::endl; - mCommand = _commands[0]; - break; - case SERVO: - mCommand = math::clip(_commands[0], - mSingleDofP.mVelocityLowerLimit, - mSingleDofP.mVelocityUpperLimit); - break; - case ACCELERATION: - mCommand = math::clip(_commands[0], - mSingleDofP.mAccelerationLowerLimit, - mSingleDofP.mAccelerationUpperLimit); - break; - case VELOCITY: - mCommand = math::clip(_commands[0], - mSingleDofP.mVelocityLowerLimit, - mSingleDofP.mVelocityUpperLimit); - // TODO: This possibly makes the acceleration to exceed the limits. - break; - case LOCKED: - dtwarn << "[SingleDofJoint::setCommands] Attempting to set command for " - << "LOCKED joint." << std::endl; - mCommand = _commands[0]; - break; - default: - assert(false); - break; - } + setCommand(0, _commands[0]); } //============================================================================== @@ -400,7 +382,7 @@ void SingleDofJoint::setPosition(size_t _index, double _position) { if (_index != 0) { - dterr << "setPosition index[" << _index << "] out of range" << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setPosition, _index ); return; } @@ -412,7 +394,7 @@ double SingleDofJoint::getPosition(size_t _index) const { if (_index != 0) { - dterr << "getPosition index[" << _index << "] out of range" << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getPosition, _index ); return 0.0; } @@ -424,8 +406,7 @@ void SingleDofJoint::setPositions(const Eigen::VectorXd& _positions) { if (static_cast(_positions.size()) != getNumDofs()) { - dterr << "setPositions positions's size[" << _positions.size() - << "] is different with the dof [" << getNumDofs() << "]" << std::endl; + SINGLEDOFJOINT_REPORT_DIM_MISMATCH( setPositions, _positions ); return; } @@ -449,8 +430,7 @@ void SingleDofJoint::setPositionLowerLimit(size_t _index, double _position) { if (_index != 0) { - dterr << "setPositionLowerLimit index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setPositionLowerLimit, _index ); return; } @@ -462,8 +442,7 @@ double SingleDofJoint::getPositionLowerLimit(size_t _index) const { if (_index != 0) { - dterr << "getPositionLowerLimit index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getPositionLowerLimit, _index ); return 0.0; } @@ -475,8 +454,7 @@ void SingleDofJoint::setPositionUpperLimit(size_t _index, double _position) { if (_index != 0) { - dterr << "setPositionUpperLimit index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setPositionUpperLimit, _index ); return; } @@ -488,8 +466,7 @@ double SingleDofJoint::getPositionUpperLimit(size_t _index) const { if (_index != 0) { - dterr << "getPositionUpperLimit index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getPositionUpperLimit, _index ); return 0.0; } @@ -501,7 +478,7 @@ void SingleDofJoint::setVelocity(size_t _index, double _velocity) { if (_index != 0) { - dterr << "setVelocity index[" << _index << "] out of range" << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setVelocity, _index ); return; } @@ -519,7 +496,7 @@ double SingleDofJoint::getVelocity(size_t _index) const { if (_index != 0) { - dterr << "getVelocity index[" << _index << "] out of range" << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getVelocity, _index ); return 0.0; } @@ -531,8 +508,7 @@ void SingleDofJoint::setVelocities(const Eigen::VectorXd& _velocities) { if (static_cast(_velocities.size()) != getNumDofs()) { - dterr << "setVelocities velocities's size[" << _velocities.size() - << "] is different with the dof [" << getNumDofs() << "]" << std::endl; + SINGLEDOFJOINT_REPORT_DIM_MISMATCH( setVelocities, _velocities ); return; } @@ -562,8 +538,7 @@ void SingleDofJoint::setVelocityLowerLimit(size_t _index, double _velocity) { if (_index != 0) { - dterr << "setVelocityLowerLimit index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setVelocityLowerLimit, _index ); return; } @@ -575,8 +550,7 @@ double SingleDofJoint::getVelocityLowerLimit(size_t _index) const { if (_index != 0) { - dterr << "getVelocityLowerLimit index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getVelocityLowerLimit, _index ); return 0.0; } @@ -588,8 +562,7 @@ void SingleDofJoint::setVelocityUpperLimit(size_t _index, double _velocity) { if (_index != 0) { - dterr << "setVelocityUpperLimit index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setVelocityUpperLimit, _index ); return; } @@ -601,8 +574,7 @@ double SingleDofJoint::getVelocityUpperLimit(size_t _index) const { if (_index != 0) { - dterr << "getVelocityUpperLimit index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getVelocityUpperLimit, _index ); return 0.0; } @@ -614,8 +586,7 @@ void SingleDofJoint::setAcceleration(size_t _index, double _acceleration) { if (_index != 0) { - dterr << "setAcceleration index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setAcceleration, _index ); return; } @@ -633,8 +604,7 @@ double SingleDofJoint::getAcceleration(size_t _index) const { if (_index != 0) { - dterr << "getAcceleration index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getAcceleration, _index ); return 0.0; } @@ -646,8 +616,7 @@ void SingleDofJoint::setAccelerations(const Eigen::VectorXd& _accelerations) { if (static_cast(_accelerations.size()) != getNumDofs()) { - dterr << "setAccelerations accelerations's size[" << _accelerations.size() - << "] is different with the dof [" << getNumDofs() << "]" << std::endl; + SINGLEDOFJOINT_REPORT_DIM_MISMATCH( setAccelerations, _accelerations ); return; } @@ -678,8 +647,7 @@ void SingleDofJoint::setAccelerationLowerLimit(size_t _index, { if (_index != 0) { - dterr << "setAccelerationLowerLimit index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setAccelerationLowerLimit, _index ); return; } @@ -691,8 +659,7 @@ double SingleDofJoint::getAccelerationLowerLimit(size_t _index) const { if (_index != 0) { - dterr << "getAccelerationLowerLimit index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getAccelerationLowerLimit, _index ); return 0.0; } @@ -705,8 +672,7 @@ void SingleDofJoint::setAccelerationUpperLimit(size_t _index, { if (_index != 0) { - dterr << "setAccelerationUpperLimit index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getAccelerationUpperLimit, _index ); return; } @@ -718,8 +684,7 @@ double SingleDofJoint::getAccelerationUpperLimit(size_t _index) const { if (_index != 0) { - dterr << "getAccelerationUpperLimit index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getAccelerationUpperLimit, _index ); return 0.0; } @@ -770,7 +735,7 @@ void SingleDofJoint::setForce(size_t _index, double _force) { if (_index != 0) { - dterr << "setForce index[" << _index << "] out of range" << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setForce, _index ); return; } @@ -788,7 +753,7 @@ double SingleDofJoint::getForce(size_t _index) { if (_index != 0) { - dterr << "getForce index[" << _index << "] out of range" << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getForce, _index ); return 0.0; } @@ -800,8 +765,7 @@ void SingleDofJoint::setForces(const Eigen::VectorXd& _forces) { if (static_cast(_forces.size()) != getNumDofs()) { - dterr << "setForces forces's size[" << _forces.size() - << "] is different with the dof [" << getNumDofs() << "]" << std::endl; + SINGLEDOFJOINT_REPORT_DIM_MISMATCH( setForces, _forces ); return; } @@ -837,8 +801,7 @@ void SingleDofJoint::setForceLowerLimit(size_t _index, double _force) { if (_index != 0) { - dterr << "setForceLowerLimit index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setForceLowerLimit, _index ); return; } @@ -850,7 +813,7 @@ double SingleDofJoint::getForceLowerLimit(size_t _index) const { if (_index != 0) { - dterr << "getForceMin index[" << _index << "] out of range" << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getForceLowerLimit, _index ); return 0.0; } @@ -862,8 +825,7 @@ void SingleDofJoint::setForceUpperLimit(size_t _index, double _force) { if (_index != 0) { - dterr << "setForceUpperLimit index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setForceUpperLimit, _index ); return; } @@ -875,8 +837,7 @@ double SingleDofJoint::getForceUpperLimit(size_t _index) const { if (_index != 0) { - dterr << "getForceUpperLimit index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getForceUpperLimit, _index ); return 0.0; } @@ -888,8 +849,7 @@ void SingleDofJoint::setVelocityChange(size_t _index, double _velocityChange) { if (_index != 0) { - dterr << "setVelocityChange index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setVelocityChange, _index ); return; } @@ -901,8 +861,7 @@ double SingleDofJoint::getVelocityChange(size_t _index) const { if (_index != 0) { - dterr << "getVelocityChange index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getVelocityChange, _index ); return 0.0; } @@ -920,8 +879,7 @@ void SingleDofJoint::setConstraintImpulse(size_t _index, double _impulse) { if (_index != 0) { - dterr << "setConstraintImpulse index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setConstraintImpulse, _index ); return; } @@ -933,8 +891,7 @@ double SingleDofJoint::getConstraintImpulse(size_t _index) const { if (_index != 0) { - dterr << "getConstraintImpulse index[" << _index << "] out of range" - << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getConstraintImpulse, _index ); return 0.0; } @@ -966,9 +923,10 @@ Eigen::VectorXd SingleDofJoint::getPositionDifferences( if (static_cast(_q1.size()) != getNumDofs() || static_cast(_q2.size()) != getNumDofs()) { - dterr << "SingleDofJoint::getPositionsDifference: q1's size[" << _q1.size() - << "] or q2's size[" << _q2.size() << "is different with the dof [" - << getNumDofs() << "]." << std::endl; + dterr << "[SingleDofJoint::getPositionsDifference] q1's size [" + << _q1.size() << "] and q2's size [" << _q2.size() << "] must both " + << "equal 1 for Joint [" << getName() << "].\n"; + assert(false); return Eigen::Matrix::Zero(); } @@ -988,8 +946,7 @@ void SingleDofJoint::setSpringStiffness(size_t _index, double _k) { if (_index != 0) { - dterr << "[SingleDofJoint::setSpringStiffness]: index[" << _index - << "] out of range." << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setSpringStiffness, _index ); return; } @@ -1003,8 +960,7 @@ double SingleDofJoint::getSpringStiffness(size_t _index) const { if (_index != 0) { - dterr << "[SingleDofJoint::getSpringStiffness]: index[" << _index - << "] out of range." << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getSpringStiffness, _index ); return 0.0; } @@ -1016,19 +972,18 @@ void SingleDofJoint::setRestPosition(size_t _index, double _q0) { if (_index != 0) { - dterr << "[SingleDofJoint::setRestPosition]: index[" << _index - << "] out of range." << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setRestPosition, _index ); return; } if (mSingleDofP.mPositionLowerLimit > _q0 || mSingleDofP.mPositionUpperLimit < _q0) { - dterr << "Rest position of joint[" << getName() << "], " << _q0 - << ", is out of the limit range[" - << mSingleDofP.mPositionLowerLimit << ", " - << mSingleDofP.mPositionUpperLimit << "] in index[" << _index - << "].\n"; + dtwarn << "[SingleDofJoint::setRestPosition] Value of _q0 [" << _q0 + << "] is out of the limit range [" + << mSingleDofP.mPositionLowerLimit << ", " + << mSingleDofP.mPositionUpperLimit << "] for index [" << _index + << "] of Joint [" << getName() << "].\n"; return; } @@ -1040,8 +995,7 @@ double SingleDofJoint::getRestPosition(size_t _index) const { if (_index != 0) { - dterr << "[SingleDofJoint::getRestPosition]: index[" << _index - << "] out of range." << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getRestPosition, _index ); return 0.0; } @@ -1053,8 +1007,7 @@ void SingleDofJoint::setDampingCoefficient(size_t _index, double _d) { if (_index != 0) { - dterr << "[SingleDofJoint::setDampingCoefficient]: index[" << _index - << "] out of range." << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setDampingCoefficient, _index ); return; } @@ -1068,8 +1021,7 @@ double SingleDofJoint::getDampingCoefficient(size_t _index) const { if (_index != 0) { - dterr << "[SingleDofJoint::getDampingCoefficient]: index[" << _index - << "] out of range." << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getDampingCoefficient, _index ); return 0.0; } @@ -1081,8 +1033,7 @@ void SingleDofJoint::setCoulombFriction(size_t _index, double _friction) { if (_index != 0) { - dterr << "[SingleDofJoint::setFriction]: index[" << _index - << "] out of range." << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( setCoulombFriction, _index ); return; } @@ -1096,8 +1047,7 @@ double SingleDofJoint::getCoulombFriction(size_t _index) const { if (_index != 0) { - dterr << "[SingleDofJoint::getFriction]: index[" << _index - << "] out of range." << std::endl; + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( getCoulombFriction, _index ); return 0.0; } @@ -1304,7 +1254,7 @@ void SingleDofJoint::addChildArtInertiaTo( _childArtInertia); break; default: - dterr << "Unsupported actuator type." << std::endl; + SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR( addChildArtInertiaTo ); break; } } @@ -1353,7 +1303,7 @@ void SingleDofJoint::addChildArtInertiaImplicitTo( _childArtInertia); break; default: - dterr << "Unsupported actuator type." << std::endl; + SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(addChildArtInertiaImplicitTo); break; } } @@ -1400,7 +1350,7 @@ void SingleDofJoint::updateInvProjArtInertia( updateInvProjArtInertiaKinematic(_artInertia); break; default: - dterr << "Unsupported actuator type." << std::endl; + SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateInvProjArtInertia); break; } } @@ -1445,7 +1395,8 @@ void SingleDofJoint::updateInvProjArtInertiaImplicit( updateInvProjArtInertiaImplicitKinematic(_artInertia, _timeStep); break; default: - dterr << "Unsupported actuator type." << std::endl; + SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR( + updateInvProjArtInertiaImplicit); break; } } @@ -1503,7 +1454,7 @@ void SingleDofJoint::addChildBiasForceTo( _childPartialAcc); break; default: - dterr << "Unsupported actuator type." << std::endl; + SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(addChildBiasForceTo); break; } } @@ -1572,7 +1523,7 @@ void SingleDofJoint::addChildBiasImpulseTo( _childBiasImpulse); break; default: - dterr << "Unsupported actuator type." << std::endl; + SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(addChildBiasImpulseTo); break; } } @@ -1639,7 +1590,7 @@ void SingleDofJoint::updateTotalForce(const Eigen::Vector6d& _bodyForce, updateTotalForceKinematic(_bodyForce, _timeStep); break; default: - dterr << "Unsupported actuator type." << std::endl; + SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateTotalForce); break; } } @@ -1686,7 +1637,7 @@ void SingleDofJoint::updateTotalImpulse(const Eigen::Vector6d& _bodyImpulse) updateTotalImpulseKinematic(_bodyImpulse); break; default: - dterr << "Unsupported actuator type." << std::endl; + SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateTotalImpulse); break; } } @@ -1728,7 +1679,7 @@ void SingleDofJoint::updateAcceleration(const Eigen::Matrix6d& _artInertia, updateAccelerationKinematic(_artInertia, _spatialAcc); break; default: - dterr << "Unsupported actuator type." << std::endl; + SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateAcceleration); break; } } @@ -1772,7 +1723,7 @@ void SingleDofJoint::updateVelocityChange( updateVelocityChangeKinematic(_artInertia, _velocityChange); break; default: - dterr << "Unsupported actuator type." << std::endl; + SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateVelocityChange); break; } } @@ -1846,7 +1797,7 @@ void SingleDofJoint::updateForceFD(const Eigen::Vector6d& _bodyForce, _withSpringForces); break; default: - dterr << "Unsupported actuator type." << std::endl; + SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateForceFD); break; } } @@ -1872,7 +1823,7 @@ void SingleDofJoint::updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) updateImpulseID(_bodyImpulse); break; default: - dterr << "Unsupported actuator type." << std::endl; + SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateImpulseFD); break; } } @@ -1893,7 +1844,7 @@ void SingleDofJoint::updateConstrainedTerms(double _timeStep) updateConstrainedTermsKinematic(_timeStep); break; default: - dterr << "Unsupported actuator type." << std::endl; + SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateConstrainedTerms); break; } } diff --git a/dart/dynamics/detail/MultiDofJoint.h b/dart/dynamics/detail/MultiDofJoint.h index 61fb1b1fd9224..8813485e2a6a9 100644 --- a/dart/dynamics/detail/MultiDofJoint.h +++ b/dart/dynamics/detail/MultiDofJoint.h @@ -37,6 +37,24 @@ #ifndef DART_DYNAMICS_DETAIL_MULTIDOFJOINT_H_ #define DART_DYNAMICS_DETAIL_MULTIDOFJOINT_H_ +#define MULTIDOFJOINT_REPORT_DIM_MISMATCH( func, arg ) \ + dterr << "[MultiDofJoint::" #func "] Mismatch beteween size of " \ + << #arg " [" << arg .size() << "] and the number of " \ + << "DOFs [" << getNumDofs() << "] for Joint named [" \ + << getName() << "].\n"; \ + assert(false); + +#define MULTIDOFJOINT_REPORT_OUT_OF_RANGE( func, index ) \ + dterr << "[MultiDofJoint::" << #func << "] The index [" << index \ + << "] is out of range for Joint named [" << getName() \ + << "] which has " << getNumDofs() << " DOFs.\n"; \ + assert(false); + +#define MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR( func ) \ + dterr << "[MultiDofJoint::" # func "] Unsupported actuator type (" \ + << mJointP.mActuatorType << ") for Joint [" << getName() << "].\n"; \ + assert(false); + //============================================================================== template MultiDofJoint::UniqueProperties::UniqueProperties( @@ -196,9 +214,8 @@ DegreeOfFreedom* MultiDofJoint::getDof(size_t _index) if (_index < DOF) return mDofs[_index]; - dterr << "[MultiDofJoint::getDof] Attempting to access index (" << _index - << "). The index must be less than (" << DOF << ")!\n"; - assert(false); + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getDof, _index); + return nullptr; } @@ -209,9 +226,8 @@ const DegreeOfFreedom* MultiDofJoint::getDof(size_t _index) const if (_index < DOF) return mDofs[_index]; - dterr << "[MultiDofJoint::getDof] Attempting to access index (" << _index - << "). The index must be less than (" << DOF << ")!\n"; - assert(false); + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getDof, _index); + return nullptr; } @@ -223,9 +239,10 @@ const std::string& MultiDofJoint::setDofName(size_t _index, { if(DOF <= _index) { - dtwarn << "[MultiDofJoint::setDofName] Attempting to set the name of DOF " - << "index " << _index << ", which is out of bounds. We will set " - << "the name of DOF index 0 instead\n"; + dterr << "[MultiDofJoint::setDofName] Attempting to set the name of DOF " + << "index " << _index << ", which is out of bounds for the Joint [" + << getName() << "]. We will set the name of DOF index 0 instead.\n"; + assert(false); _index = 0; } @@ -253,10 +270,8 @@ void MultiDofJoint::preserveDofName(size_t _index, bool _preserve) { if (DOF <= _index) { - dtwarn << "[MultiDofJoint::preserveDofName] Attempting to preserve the " - << "name of DOF index " << _index << ", which is out of bounds. We " - << "will preserve the name of DOF index 0 instead\n"; - _index = 0; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(preserveDofName, _index); + return; } mMultiDofP.mPreserveDofNames[_index] = _preserve; @@ -268,10 +283,7 @@ bool MultiDofJoint::isDofNamePreserved(size_t _index) const { if(DOF <= _index) { - dtwarn << "[MultiDofJoint::isDofNamePreserved] Requesting whether DOF " - << "index " << _index << " is preserved, but this is out of bounds " - << "(max " << DOF-1 << "). We will return the result of DOF index 0 " - << "instead\n"; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(isDofNamePreserved, _index); _index = 0; } @@ -284,9 +296,10 @@ const std::string& MultiDofJoint::getDofName(size_t _index) const { if(DOF <= _index) { - dterr << "[MultiDofJoint::getDofName] Requested name of DOF index " - << _index << ", but that is out of bounds (max " << DOF-1 << "). " - << "Returning name of DOF 0\n"; + dterr << "[MultiDofJoint::getDofName] Requested name of DOF index [" + << _index << "] in Joint [" << getName() << "], but that is out of " + << "bounds (max " << DOF-1 << "). Returning name of DOF 0.\n"; + assert(false); return mMultiDofP.mDofNames[0]; } @@ -306,9 +319,7 @@ size_t MultiDofJoint::getIndexInSkeleton(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "[MultiDofJoint::getIndexInSkeleton] index (" << _index - << ") out of range. Must be less than " << getNumDofs() << "!\n"; - assert(false); + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getIndexInSkeleton, _index); return 0; } @@ -321,9 +332,7 @@ size_t MultiDofJoint::getIndexInTree(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "[MultiDofJoint::getIndexInTree] index (" << _index - << ") out of range. Must be less than " << getNumDofs() << "!\n"; - assert(false); + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getIndexInTree, _index); return 0; } @@ -336,9 +345,7 @@ void MultiDofJoint::setCommand(size_t _index, double _command) { if (_index >= getNumDofs()) { - dterr << "[MultiDofJoint::setCommand]: index[" << _index << "] out of range" - << std::endl; - return; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setCommand, _index); } switch (mJointP.mActuatorType) @@ -349,8 +356,12 @@ void MultiDofJoint::setCommand(size_t _index, double _command) mMultiDofP.mForceUpperLimits[_index]); break; case PASSIVE: - dtwarn << "[MultiDofJoint::setCommand] Attempting to set command for " - << "PASSIVE joint." << std::endl; + if(0.0 != _command) + { + dtwarn << "[MultiDofJoint::setCommand] Attempting to set a non-zero (" + << _command << ") command for a PASSIVE joint [" << getName() + << "].\n"; + } mCommands[_index] = _command; break; case SERVO: @@ -370,8 +381,12 @@ void MultiDofJoint::setCommand(size_t _index, double _command) // TODO: This possibly makes the acceleration to exceed the limits. break; case LOCKED: - dtwarn << "[MultiDofJoint::setCommand] Attempting to set command for " - << "LOCKED joint." << std::endl; + if(0.0 != _command) + { + dtwarn << "[MultiDofJoint::setCommand] Attempting to set a non-zero (" + << _command << ") command for a LOCKED joint [" << getName() + << "].\n"; + } mCommands[_index] = _command; break; default: @@ -386,8 +401,7 @@ double MultiDofJoint::getCommand(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "[MultiDofJoint::getCommand]: index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getCommand, _index); return 0.0; } @@ -400,9 +414,7 @@ void MultiDofJoint::setCommands(const Eigen::VectorXd& _commands) { if (static_cast(_commands.size()) != getNumDofs()) { - dterr << "[MultiDofJoint::setCommands]: commands's size[" - << _commands.size() << "] is different with the dof [" - << getNumDofs() << "]" << std::endl; + MULTIDOFJOINT_REPORT_DIM_MISMATCH(setCommands, _commands); return; } @@ -414,8 +426,12 @@ void MultiDofJoint::setCommands(const Eigen::VectorXd& _commands) mMultiDofP.mForceUpperLimits); break; case PASSIVE: - dtwarn << "[MultiDofJoint::setCommands] Attempting to set command for " - << "PASSIVE joint." << std::endl; + if(Vector::Zero() != _commands) + { + dtwarn << "[MultiDofJoint::setCommands] Attempting to set a non-zero (" + << _commands.transpose() << ") command for a PASSIVE joint [" + << getName() << "].\n"; + } mCommands = _commands; break; case SERVO: @@ -435,8 +451,12 @@ void MultiDofJoint::setCommands(const Eigen::VectorXd& _commands) // TODO: This possibly makes the acceleration to exceed the limits. break; case LOCKED: - dtwarn << "[MultiDofJoint::setCommands] Attempting to set command for " - << "LOCKED joint." << std::endl; + if(Vector::Zero() != _commands) + { + dtwarn << "[MultiDofJoint::setCommands] Attempting to set a non-zero (" + << _commands.transpose() << ") command for a LOCKED joint [" + << getName() << "].\n"; + } mCommands = _commands; break; default: @@ -465,7 +485,7 @@ void MultiDofJoint::setPosition(size_t _index, double _position) { if (_index >= getNumDofs()) { - dterr << "setPosition index[" << _index << "] out of range" << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setPosition, _index); return; } @@ -480,7 +500,7 @@ double MultiDofJoint::getPosition(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "setPosition index[" << _index << "] out of range" << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getPosition, _index); return 0.0; } @@ -493,8 +513,7 @@ void MultiDofJoint::setPositions(const Eigen::VectorXd& _positions) { if (static_cast(_positions.size()) != getNumDofs()) { - dterr << "setPositions positions's size[" << _positions.size() - << "] is different with the dof [" << getNumDofs() << "]" << std::endl; + MULTIDOFJOINT_REPORT_DIM_MISMATCH(setPositions, _positions); return; } @@ -521,8 +540,7 @@ void MultiDofJoint::setPositionLowerLimit(size_t _index, double _position) { if (_index >= getNumDofs()) { - dterr << "setPositionLowerLimit index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setPositionLowerLimit, _index); return; } @@ -535,8 +553,7 @@ double MultiDofJoint::getPositionLowerLimit(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "getPositionLowerLimit index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getPositionLowerLimit, _index); return 0.0; } @@ -549,8 +566,7 @@ void MultiDofJoint::setPositionUpperLimit(size_t _index, double _position) { if (_index >= getNumDofs()) { - dterr << "setPositionUpperLimit index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setPositionUpperLimit, _index); return; } @@ -563,8 +579,7 @@ double MultiDofJoint::getPositionUpperLimit(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "getPositionUpperLimit index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getPositionUpperLimit, _index); return 0.0; } @@ -577,7 +592,7 @@ void MultiDofJoint::setVelocity(size_t _index, double _velocity) { if (_index >= getNumDofs()) { - dterr << "setVelocity index[" << _index << "] out of range" << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setVelocity, _index); return; } @@ -598,7 +613,7 @@ double MultiDofJoint::getVelocity(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "getVelocity index[" << _index << "] out of range" << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getVelocity, _index); return 0.0; } @@ -611,8 +626,7 @@ void MultiDofJoint::setVelocities(const Eigen::VectorXd& _velocities) { if (static_cast(_velocities.size()) != getNumDofs()) { - dterr << "setVelocities velocities's size[" << _velocities.size() - << "] is different with the dof [" << getNumDofs() << "]" << std::endl; + MULTIDOFJOINT_REPORT_DIM_MISMATCH(setVelocities, _velocities); return; } @@ -645,8 +659,7 @@ void MultiDofJoint::setVelocityLowerLimit(size_t _index, double _velocity) { if (_index >= getNumDofs()) { - dterr << "setVelocityLowerLimit index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE( setVelocityLowerLimit, _index ); return; } @@ -659,8 +672,7 @@ double MultiDofJoint::getVelocityLowerLimit(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "getVelocityLowerLimit index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE( getVelocityLowerLimit, _index); return 0.0; } @@ -673,8 +685,7 @@ void MultiDofJoint::setVelocityUpperLimit(size_t _index, double _velocity) { if (_index >= getNumDofs()) { - dterr << "setVelocityUpperLimit index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE( setVelocityUpperLimit, _index ); return; } @@ -687,8 +698,7 @@ double MultiDofJoint::getVelocityUpperLimit(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "getVelocityUpperLimit index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE( getVelocityUpperLimit, _index ); return 0.0; } @@ -701,8 +711,7 @@ void MultiDofJoint::setAcceleration(size_t _index, double _acceleration) { if (_index >= getNumDofs()) { - dterr << "setAcceleration index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE( setAcceleration, _index ); return; } @@ -723,8 +732,7 @@ double MultiDofJoint::getAcceleration(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "getAcceleration index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE( getAcceleration, _index ); return 0.0; } @@ -737,8 +745,7 @@ void MultiDofJoint::setAccelerations(const Eigen::VectorXd& _accelerations) { if (static_cast(_accelerations.size()) != getNumDofs()) { - dterr << "setAccelerations accelerations's size[" << _accelerations.size() - << "] is different with the dof [" << getNumDofs() << "]" << std::endl; + MULTIDOFJOINT_REPORT_DIM_MISMATCH( setAccelerations, _accelerations ); return; } @@ -772,8 +779,7 @@ void MultiDofJoint::setAccelerationLowerLimit(size_t _index, { if (_index >= getNumDofs()) { - dterr << "setAccelerationLowerLimit index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setAccelerationLowerLimit, _index); return; } @@ -786,8 +792,7 @@ double MultiDofJoint::getAccelerationLowerLimit(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "getAccelerationLowerLimit index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getAccelerationLowerLimit, _index); return 0.0; } @@ -801,8 +806,7 @@ void MultiDofJoint::setAccelerationUpperLimit(size_t _index, { if (_index >= getNumDofs()) { - dterr << "setAccelerationUpperLimit index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setAccelerationUpperLimit, _index) return; } @@ -815,8 +819,7 @@ double MultiDofJoint::getAccelerationUpperLimit(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "getAccelerationUpperLimit index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getAccelerationUpperLimit, _index); return 0.0; } @@ -877,7 +880,7 @@ void MultiDofJoint::setForce(size_t _index, double _force) { if (_index >= getNumDofs()) { - dterr << "setForce index[" << _index << "] out of range" << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setForce, _index); return; } @@ -896,7 +899,7 @@ double MultiDofJoint::getForce(size_t _index) { if (_index >= getNumDofs()) { - dterr << "getForce index[" << _index << "] out of range" << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getForce, _index); return 0.0; } @@ -909,8 +912,7 @@ void MultiDofJoint::setForces(const Eigen::VectorXd& _forces) { if (static_cast(_forces.size()) != getNumDofs()) { - dterr << "setForces forces's size[" << _forces.size() - << "] is different with the dof [" << getNumDofs() << "]" << std::endl; + MULTIDOFJOINT_REPORT_DIM_MISMATCH(setForces, _forces); return; } @@ -949,8 +951,7 @@ void MultiDofJoint::setForceLowerLimit(size_t _index, double _force) { if (_index >= getNumDofs()) { - dterr << "setForceLowerLimit index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setForceLowerLimit, _index); return; } @@ -963,7 +964,7 @@ double MultiDofJoint::getForceLowerLimit(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "getForceMin index[" << _index << "] out of range" << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getForceLowerLimit, _index); return 0.0; } @@ -976,8 +977,7 @@ void MultiDofJoint::setForceUpperLimit(size_t _index, double _force) { if (_index >= getNumDofs()) { - dterr << "setForceUpperLimit index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setForceUpperLimit, _index); return; } @@ -990,8 +990,7 @@ double MultiDofJoint::getForceUpperLimit(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "getForceUpperLimit index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getForceUpperLimit, _index); return 0.0; } @@ -1005,8 +1004,7 @@ void MultiDofJoint::setVelocityChange(size_t _index, { if (_index >= getNumDofs()) { - dterr << "setVelocityChange index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setVelocityChange, _index); return; } @@ -1019,8 +1017,7 @@ double MultiDofJoint::getVelocityChange(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "getVelocityChange index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getVelocityChange, _index); return 0.0; } @@ -1040,8 +1037,7 @@ void MultiDofJoint::setConstraintImpulse(size_t _index, double _impulse) { if (_index >= getNumDofs()) { - dterr << "setConstraintImpulse index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setConstraintImpulse, _index); return; } @@ -1054,8 +1050,7 @@ double MultiDofJoint::getConstraintImpulse(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "getConstraintImpulse index[" << _index << "] out of range" - << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getConstraintImpulse, _index); return 0.0; } @@ -1091,9 +1086,10 @@ Eigen::VectorXd MultiDofJoint::getPositionDifferences( if (static_cast(_q1.size()) != getNumDofs() || static_cast(_q2.size()) != getNumDofs()) { - dterr << "MultiDofJoint::getPositionsDifference: q1's size[" << _q1.size() - << "] or q2's size[" << _q2.size() << "is different with the dof [" - << getNumDofs() << "]." << std::endl; + dterr << "[MultiDofJoint::getPositionsDifference] q1's size [" << _q1.size() + << "] or q2's size [" << _q2.size() << "] must both equal the dof [" + << getNumDofs() << "] for Joint [" << getName() << "].\n"; + assert(false); return Eigen::VectorXd::Zero(getNumDofs()); } @@ -1114,8 +1110,7 @@ void MultiDofJoint::setSpringStiffness(size_t _index, double _k) { if (_index >= getNumDofs()) { - dterr << "[MultiDofJoint::setSpringStiffness()]: index[" << _index - << "] out of range." << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setSpringStiffness, _index); return; } @@ -1130,8 +1125,7 @@ double MultiDofJoint::getSpringStiffness(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "[MultiDofJoint::getSpringStiffness()]: index[" << _index - << "] out of range." << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getSpringStiffness, _index); return 0.0; } @@ -1144,19 +1138,18 @@ void MultiDofJoint::setRestPosition(size_t _index, double _q0) { if (_index >= getNumDofs()) { - dterr << "[MultiDofJoint::setRestPosition()]: index[" << _index - << "] out of range." << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setRestPosition, _index); return; } if (mMultiDofP.mPositionLowerLimits[_index] > _q0 || mMultiDofP.mPositionUpperLimits[_index] < _q0) { - dterr << "Rest position of joint[" << getName() << "], " << _q0 - << ", is out of the limit range[" - << mMultiDofP.mPositionLowerLimits[_index] << ", " - << mMultiDofP.mPositionUpperLimits[_index] << "] in index[" << _index - << "].\n"; + dtwarn << "[MultiDofJoint::setRestPosition] Value of _q0 [" << _q0 + << "], is out of the limit range [" + << mMultiDofP.mPositionLowerLimits[_index] << ", " + << mMultiDofP.mPositionUpperLimits[_index] << "] for index [" + << _index << "] of Joint [" << getName() << "].\n"; return; } @@ -1169,8 +1162,7 @@ double MultiDofJoint::getRestPosition(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "[MultiDofJoint::getRestPosition()]: index[" << _index - << "] out of range." << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getRestPosition, _index); return 0.0; } @@ -1183,8 +1175,7 @@ void MultiDofJoint::setDampingCoefficient(size_t _index, double _d) { if (_index >= getNumDofs()) { - dterr << "[MultiDofJoint::setDampingCoefficient()]: index[" << _index - << "] out of range." << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setDampingCoefficient, _index); return; } @@ -1200,8 +1191,7 @@ double MultiDofJoint::getDampingCoefficient(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "[MultiDofJoint::getDampingCoefficient()]: index[" << _index - << "] out of range." << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getDampingCoefficient, _index); return 0.0; } @@ -1214,8 +1204,7 @@ void MultiDofJoint::setCoulombFriction(size_t _index, double _friction) { if (_index >= getNumDofs()) { - dterr << "[MultiDofJoint::setFriction()]: index[" << _index - << "] out of range." << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setCoulombFriction, _index); return; } @@ -1230,8 +1219,7 @@ double MultiDofJoint::getCoulombFriction(size_t _index) const { if (_index >= getNumDofs()) { - dterr << "[MultiDofJoint::getFriction()]: index[" << _index - << "] out of range." << std::endl; + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(getCoulombFriction, _index); return 0.0; } @@ -1459,7 +1447,7 @@ void MultiDofJoint::addChildArtInertiaTo( _childArtInertia); break; default: - dterr << "Unsupported actuator type." << std::endl; + MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(addChildArtInertiaTo); break; } } @@ -1514,7 +1502,7 @@ void MultiDofJoint::addChildArtInertiaImplicitTo( _childArtInertia); break; default: - dterr << "Unsupported actuator type." << std::endl; + MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(addChildArtInertiaImplicitTo); break; } } @@ -1566,7 +1554,7 @@ void MultiDofJoint::updateInvProjArtInertia( updateInvProjArtInertiaKinematic(_artInertia); break; default: - dterr << "Unsupported actuator type." << std::endl; + MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateInvProjArtInertia); break; } } @@ -1617,7 +1605,8 @@ void MultiDofJoint::updateInvProjArtInertiaImplicit( updateInvProjArtInertiaImplicitKinematic(_artInertia, _timeStep); break; default: - dterr << "Unsupported actuator type." << std::endl; + MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR( + updateInvProjArtInertiaImplicit); break; } } @@ -1685,7 +1674,7 @@ void MultiDofJoint::addChildBiasForceTo( _childPartialAcc); break; default: - dterr << "Unsupported actuator type." << std::endl; + MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(addChildBiasForceTo); break; } } @@ -1770,7 +1759,7 @@ void MultiDofJoint::addChildBiasImpulseTo( _childBiasImpulse); break; default: - dterr << "Unsupported actuator type." << std::endl; + MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(addChildBiasImpulseTo); break; } } @@ -1841,7 +1830,7 @@ void MultiDofJoint::updateTotalForce( updateTotalForceKinematic(_bodyForce, _timeStep); break; default: - dterr << "Unsupported actuator type." << std::endl; + MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateTotalForce); break; } } @@ -1894,7 +1883,7 @@ void MultiDofJoint::updateTotalImpulse( updateTotalImpulseKinematic(_bodyImpulse); break; default: - dterr << "Unsupported actuator type." << std::endl; + MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateTotalImpulse); break; } } @@ -1943,7 +1932,7 @@ void MultiDofJoint::updateAcceleration( updateAccelerationKinematic(_artInertia, _spatialAcc); break; default: - dterr << "Unsupported actuator type." << std::endl; + MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateAcceleration); break; } } @@ -1991,7 +1980,7 @@ void MultiDofJoint::updateVelocityChange( updateVelocityChangeKinematic(_artInertia, _velocityChange); break; default: - dterr << "Unsupported actuator type." << std::endl; + MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateVelocityChange); break; } } @@ -2069,7 +2058,7 @@ void MultiDofJoint::updateForceFD(const Eigen::Vector6d& _bodyForce, _withSpringForces); break; default: - dterr << "Unsupported actuator type." << std::endl; + MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateForceFD); break; } } @@ -2097,7 +2086,7 @@ void MultiDofJoint::updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) updateImpulseID(_bodyImpulse); break; default: - dterr << "Unsupported actuator type." << std::endl; + MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateImpulseFD); break; } } @@ -2119,7 +2108,7 @@ void MultiDofJoint::updateConstrainedTerms(double _timeStep) updateConstrainedTermsKinematic(_timeStep); break; default: - dterr << "Unsupported actuator type." << std::endl; + MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(updateConstrainedTerms); break; } }