From 65b4f5b11d5c1e3614a94cb1a2ce034d7e25f1cc Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 11 Jun 2015 06:33:07 -0400 Subject: [PATCH 01/19] Remove computeForwardKinematics() that became unnecessary by #319 --- apps/atlasSimbicon/Controller.cpp | 1 - apps/atlasSimbicon/Main.cpp | 2 -- apps/bipedStand/MyWindow.cpp | 2 -- apps/hardcodedDesign/MyWindow.cpp | 1 - apps/hybridDynamics/Main.cpp | 1 - apps/jointConstraints/Main.cpp | 1 - apps/mixedChain/Main.cpp | 1 - apps/rigidChain/Main.cpp | 1 - apps/rigidLoop/Main.cpp | 1 - apps/speedTest/Main.cpp | 1 - dart/dynamics/Skeleton.cpp | 34 ------------------------------- dart/dynamics/Skeleton.h | 9 -------- dart/gui/SimWindow.cpp | 1 - dart/planning/PathShortener.cpp | 2 -- dart/planning/RRT.cpp | 1 - unittests/testDynamics.cpp | 3 --- unittests/testJoints.cpp | 2 -- unittests/testSoftDynamics.cpp | 1 - 18 files changed, 65 deletions(-) diff --git a/apps/atlasSimbicon/Controller.cpp b/apps/atlasSimbicon/Controller.cpp index 2727857ae0913..c62d64f594621 100644 --- a/apps/atlasSimbicon/Controller.cpp +++ b/apps/atlasSimbicon/Controller.cpp @@ -300,7 +300,6 @@ void Controller::unharnessRightFoot() void Controller::resetRobot() { mAtlasRobot->setState(mInitialState); - mAtlasRobot->computeForwardKinematics(true, true, false); dtmsg << "Robot is reset." << std::endl; } diff --git a/apps/atlasSimbicon/Main.cpp b/apps/atlasSimbicon/Main.cpp index a9de64718a1fc..131c259533261 100644 --- a/apps/atlasSimbicon/Main.cpp +++ b/apps/atlasSimbicon/Main.cpp @@ -70,8 +70,6 @@ int main(int argc, char* argv[]) q[0] = -0.5 * DART_PI; atlas->setPositions(q); - atlas->computeForwardKinematics(true, true, false); - // Set gravity of the world myWorld->setGravity(Vector3d(0.0, -9.81, 0.0)); diff --git a/apps/bipedStand/MyWindow.cpp b/apps/bipedStand/MyWindow.cpp index 6b1469e14e5c1..4ce6a3478cd91 100644 --- a/apps/bipedStand/MyWindow.cpp +++ b/apps/bipedStand/MyWindow.cpp @@ -151,14 +151,12 @@ void MyWindow::plotCOMX() { for (int i = 0; i < nFrame; i++) { Eigen::VectorXd pose = mWorld->getRecording()->getConfig(i, 1); mWorld->getSkeleton(1)->setPositions(pose); - mWorld->getSkeleton(1)->computeForwardKinematics(true, true, false); data[i] = mWorld->getSkeleton(1)->getCOM()[0]; } if (nFrame != 0) { Eigen::VectorXd pose = mWorld->getRecording()->getConfig(mPlayFrame, 1); mWorld->getSkeleton(1)->setPositions(pose); - mWorld->getSkeleton(1)->computeForwardKinematics(true, true, false); } plot(data); diff --git a/apps/hardcodedDesign/MyWindow.cpp b/apps/hardcodedDesign/MyWindow.cpp index 76ddf7c13051f..89401489cfbc8 100644 --- a/apps/hardcodedDesign/MyWindow.cpp +++ b/apps/hardcodedDesign/MyWindow.cpp @@ -64,7 +64,6 @@ void MyWindow::keyboard(unsigned char _key, int _x, int _y) { Eigen::VectorXd pose = skel->getPositions(); pose(dofIdx) = pose(dofIdx) + (inverse ? -dDOF : dDOF); skel->setPositions(pose); - skel->computeForwardKinematics(true, true, false); std::cout << "Updated pose DOF " << dofIdx << ": " << pose.transpose() << std::endl; glutPostRedisplay(); diff --git a/apps/hybridDynamics/Main.cpp b/apps/hybridDynamics/Main.cpp index 41cdc1a0716b2..28cdce406547e 100644 --- a/apps/hybridDynamics/Main.cpp +++ b/apps/hybridDynamics/Main.cpp @@ -62,7 +62,6 @@ int main(int argc, char* argv[]) Eigen::VectorXd initConfig(8); initConfig << -0.2, 0.15, -0.4, 0.25, 0.15, -0.4, 0.25, 0.0; skel->setPositions(genCoordIds, initConfig); - skel->computeForwardKinematics(true, true, false); dart::dynamics::Joint* joint0 = skel->getJoint(0); joint0->setActuatorType(dart::dynamics::Joint::PASSIVE); diff --git a/apps/jointConstraints/Main.cpp b/apps/jointConstraints/Main.cpp index 9443784e075df..ca7721fdfba67 100644 --- a/apps/jointConstraints/Main.cpp +++ b/apps/jointConstraints/Main.cpp @@ -68,7 +68,6 @@ int main(int argc, char* argv[]) Eigen::VectorXd initConfig(9); initConfig << -0.1, 0.2, 0.2, -0.5, 0.3, 0.2, -0.5, 0.3, -0.1; myWorld->getSkeleton(1)->setPositions(genCoordIds, initConfig); - myWorld->getSkeleton(1)->computeForwardKinematics(true, true, false); // create controller Controller* myController = new Controller(myWorld->getSkeleton(1), diff --git a/apps/mixedChain/Main.cpp b/apps/mixedChain/Main.cpp index c50a2f6d85eb4..9c8ea57815b5e 100644 --- a/apps/mixedChain/Main.cpp +++ b/apps/mixedChain/Main.cpp @@ -60,7 +60,6 @@ int main(int argc, char* argv[]) for (int i = 0; i < 3; i++) initPose[i] = dart::math::random(-0.5, 0.5); myWorld->getSkeleton(1)->setPositions(initPose); - myWorld->getSkeleton(1)->computeForwardKinematics(true, true, false); // create a window and link it to the world MyWindow window; diff --git a/apps/rigidChain/Main.cpp b/apps/rigidChain/Main.cpp index c3b27d8766765..71803e1305c6d 100644 --- a/apps/rigidChain/Main.cpp +++ b/apps/rigidChain/Main.cpp @@ -55,7 +55,6 @@ int main(int argc, char* argv[]) { for (int i = 0; i < dof; i++) initPose[i] = dart::math::random(-0.5, 0.5); myWorld->getSkeleton(0)->setPositions(initPose); - myWorld->getSkeleton(0)->computeForwardKinematics(true, true, false); // create a window and link it to the world MyWindow window; diff --git a/apps/rigidLoop/Main.cpp b/apps/rigidLoop/Main.cpp index c79acb1f2bd9a..1222d5a14a94d 100644 --- a/apps/rigidLoop/Main.cpp +++ b/apps/rigidLoop/Main.cpp @@ -66,7 +66,6 @@ int main(int argc, char* argv[]) initPose[26] = 3.14159 * 0.4; initPose[29] = 3.14159 * 0.4; myWorld->getSkeleton(0)->setPositions(initPose); - myWorld->getSkeleton(0)->computeForwardKinematics(true, true, false); // create a ball joint constraint BodyNode* bd1 = myWorld->getSkeleton(0)->getBodyNode("link 6"); diff --git a/apps/speedTest/Main.cpp b/apps/speedTest/Main.cpp index f7e779dd935ab..1a3285910b931 100644 --- a/apps/speedTest/Main.cpp +++ b/apps/speedTest/Main.cpp @@ -130,7 +130,6 @@ double testDynamicsSpeed(dart::simulation::WorldPtr world, skel->resetPositions(); skel->resetVelocities(); skel->resetAccelerations(); - skel->computeForwardKinematics(); } std::chrono::time_point start, end; diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index d2618cd32ac78..70987fc27fc1e 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -858,40 +858,6 @@ Eigen::VectorXd Skeleton::getVelocityDifferences( return _dq2 - _dq1; } -//============================================================================== -void Skeleton::computeForwardKinematics(bool _updateTransforms, - bool _updateVels, - bool _updateAccs) -{ - if (_updateTransforms) - { - for (std::vector::iterator it = mSkelCache.mBodyNodes.begin(); - it != mSkelCache.mBodyNodes.end(); ++it) - { - (*it)->updateTransform(); - } - } - - if (_updateVels) - { - for (std::vector::iterator it = mSkelCache.mBodyNodes.begin(); - it != mSkelCache.mBodyNodes.end(); ++it) - { - (*it)->updateVelocity(); - (*it)->updatePartialAcceleration(); - } - } - - if (_updateAccs) - { - for (std::vector::iterator it = mSkelCache.mBodyNodes.begin(); - it != mSkelCache.mBodyNodes.end(); ++it) - { - (*it)->updateAccelerationID(); - } - } -} - //============================================================================== static bool isValidBodyNode(const Skeleton* _skeleton, const BodyNode* _bodyNode, diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 350dadf191c6e..8e92deb523d35 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -411,15 +411,6 @@ class Skeleton : public MetaSkeleton /// Get the state of this skeleton described in generalized coordinates Eigen::VectorXd getState() const; - //---------------------------------------------------------------------------- - // Kinematics algorithms - //---------------------------------------------------------------------------- - - /// Compute forward kinematics - void computeForwardKinematics(bool _updateTransforms = true, - bool _updateVels = true, - bool _updateAccs = true); - //---------------------------------------------------------------------------- // Dynamics algorithms //---------------------------------------------------------------------------- diff --git a/dart/gui/SimWindow.cpp b/dart/gui/SimWindow.cpp index 4ae9a90ff5585..2cba0755cefc0 100644 --- a/dart/gui/SimWindow.cpp +++ b/dart/gui/SimWindow.cpp @@ -119,7 +119,6 @@ void SimWindow::draw() { // size_t start = mWorld->getIndex(i); // size_t size = mWorld->getSkeleton(i)->getNumDofs(); mWorld->getSkeleton(i)->setPositions(mWorld->getRecording()->getConfig(mPlayFrame, i)); - mWorld->getSkeleton(i)->computeForwardKinematics(true, false, false); } if (mShowMarkers) { // size_t sumDofs = mWorld->getIndex(nSkels); diff --git a/dart/planning/PathShortener.cpp b/dart/planning/PathShortener.cpp index cac4a7f10af15..a54d39d7b26dd 100644 --- a/dart/planning/PathShortener.cpp +++ b/dart/planning/PathShortener.cpp @@ -65,7 +65,6 @@ void PathShortener::shortenPath(list &path) } // TODO(JS): What kinematic values should be updated here? robot->setPositions(dofs, savedDofs); - robot->computeForwardKinematics(true, true, true); printf("End Brute Force Shortener \n"); } @@ -94,7 +93,6 @@ bool PathShortener::segmentCollisionFree(list &intermediatePoints, con list intermediatePoints1, intermediatePoints2; // TODO(JS): What kinematic values should be updated here? robot->setPositions(dofs, midpoint); - robot->computeForwardKinematics(true, true, true); if(!world->checkCollision() && segmentCollisionFree(intermediatePoints1, config1, midpoint) && segmentCollisionFree(intermediatePoints2, midpoint, config2)) { diff --git a/dart/planning/RRT.cpp b/dart/planning/RRT.cpp index d9f2fb68db711..b3c7b8630ea05 100644 --- a/dart/planning/RRT.cpp +++ b/dart/planning/RRT.cpp @@ -223,7 +223,6 @@ void RRT::tracePath(int node, std::list &path, bool reverse) { /* ********************************************************************************************* */ bool RRT::checkCollisions(const VectorXd &c) { robot->setPositions(dofs, c); - robot->computeForwardKinematics(true, false, false); return world->checkCollision(); } diff --git a/unittests/testDynamics.cpp b/unittests/testDynamics.cpp index 772ba6c970b4b..40b068280f950 100644 --- a/unittests/testDynamics.cpp +++ b/unittests/testDynamics.cpp @@ -1638,7 +1638,6 @@ void DynamicsTest::testImpulseBasedDynamics(const std::string& _fileName) joint->setPosition(l, random(lbRP, ubRP)); } } -// skel->computeForwardKinematics(); // skel->setPositions(VectorXd::Zero(dof)); // TODO(JS): Just clear what should be @@ -1801,7 +1800,6 @@ TEST_F(DynamicsTest, HybridDynamics) // Initialize the skeleton with the zero initial states skel->setPositions(q0); skel->setVelocities(dq0); - skel->computeForwardKinematics(true, true, true); EXPECT_TRUE(equals(skel->getPositions(), q0)); EXPECT_TRUE(equals(skel->getVelocities(), dq0)); @@ -1843,7 +1841,6 @@ TEST_F(DynamicsTest, HybridDynamics) // Restore the skeleton to the initial state skel->setPositions(q0); skel->setVelocities(dq0); - skel->computeForwardKinematics(true, true, true); EXPECT_TRUE(equals(skel->getPositions(), q0)); EXPECT_TRUE(equals(skel->getVelocities(), dq0)); diff --git a/unittests/testJoints.cpp b/unittests/testJoints.cpp index 7c3274edb8d0a..26f25fa1385ad 100644 --- a/unittests/testJoints.cpp +++ b/unittests/testJoints.cpp @@ -578,8 +578,6 @@ TEST_F(JOINTS, CONVENIENCE_FUNCTIONS) Eigen::Isometry3d actual_balljoint_tf = BallJoint::convertToTransform( balljoint->getPositions()); - skel->computeForwardKinematics(true, false, false); - // -- collect everything so we can cycle through the tests std::vector joints; std::vector bns; diff --git a/unittests/testSoftDynamics.cpp b/unittests/testSoftDynamics.cpp index e19b885fe5d75..d50f824a8931c 100644 --- a/unittests/testSoftDynamics.cpp +++ b/unittests/testSoftDynamics.cpp @@ -316,7 +316,6 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) for (int k = 0; k < x.size(); ++k) x[k] = random(lb, ub); softSkel->setState(x); - softSkel->computeForwardKinematics(true, true, false); //------------------------ Mass Matrix Test ---------------------------- // Get matrices From b163adb83cfbadbfe97b0d38bc2844c3abea975c Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 11 Jun 2015 06:44:50 -0400 Subject: [PATCH 02/19] Suppress unused-variable warnings from Bullet --- dart/collision/bullet/BulletTypes.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/dart/collision/bullet/BulletTypes.h b/dart/collision/bullet/BulletTypes.h index b0044d576bc70..13f37e6df93c4 100644 --- a/dart/collision/bullet/BulletTypes.h +++ b/dart/collision/bullet/BulletTypes.h @@ -43,6 +43,13 @@ namespace dart { namespace collision { +/// Dummy structure to suppress unused-variable warning from bullet +struct BulletWarningSuppression +{ + BulletWarningSuppression() : dummy(btInfinityMask) {} + const int dummy; +}; + /// @brief Convert Bullet vector3 type to Eigen vector3 type Eigen::Vector3d convertVector3(const btVector3& _vec); From 45779361602cb25ec57a4f79a25bbd443a6ea9cf Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 11 Jun 2015 07:07:46 -0400 Subject: [PATCH 03/19] Remove deprecated header generation that was for backward compatibility --- cmake/DARTMacros.cmake | 29 ----------------------------- dart/constraint/CMakeLists.txt | 3 --- dart/lcpsolver/CMakeLists.txt | 3 --- tools/issue_303_generator.bash | 32 -------------------------------- 4 files changed, 67 deletions(-) delete mode 100755 tools/issue_303_generator.bash diff --git a/cmake/DARTMacros.cmake b/cmake/DARTMacros.cmake index c982600e9a1d1..95f14a9620dac 100644 --- a/cmake/DARTMacros.cmake +++ b/cmake/DARTMacros.cmake @@ -56,32 +56,3 @@ macro(dart_add_library _name) ) endmacro() -#=============================================================================== -# Copied from https://bitbucket.org/osrf/gazebo/pull-request/638 and will be -# removed by DART 5.0 -#=============================================================================== -macro (dt_install_includes _subdir) - install(FILES ${ARGN} DESTINATION include/dart/${_subdir} COMPONENT headers) -endmacro() - -#=============================================================================== -# Deprecated header files -# Install until next gazebo version on case-sensitive filesystems -# Copied from https://bitbucket.org/osrf/gazebo/pull-request/638 and will be -# removed by DART 5.0 -#=============================================================================== -macro(dt_issue_303 _name _output_name) - if (FILESYSTEM_CASE_SENSITIVE) - if (${DART_VERSION} VERSION_GREATER 4.3) - message(WARNING "Installing deprecated ${_name}.h. This should be removed in DART 5.0.") - endif() - set(generated_file "${CMAKE_CURRENT_BINARY_DIR}/${_name}.h") - execute_process( - COMMAND bash ${PROJECT_SOURCE_DIR}/tools/issue_303_generator.bash ${_name} ${_output_name} - OUTPUT_FILE ${generated_file} - ) - string(TOLOWER ${_name} nameLower) - dt_install_includes(${nameLower} ${generated_file}) - endif() -endmacro() - diff --git a/dart/constraint/CMakeLists.txt b/dart/constraint/CMakeLists.txt index d7dc9c35e1ece..3f8e3ada0d7c3 100644 --- a/dart/constraint/CMakeLists.txt +++ b/dart/constraint/CMakeLists.txt @@ -36,6 +36,3 @@ install( #install(TARGETS dart_constraint EXPORT DARTCoreTargets DESTINATION lib) #install(TARGETS dart_constraint EXPORT DARTTargets DESTINATION lib) -# Deprecated Constraint.h -dt_issue_303("Constraint" "ConstraintBase") - diff --git a/dart/lcpsolver/CMakeLists.txt b/dart/lcpsolver/CMakeLists.txt index 366ec304e5bfe..f9422270af334 100644 --- a/dart/lcpsolver/CMakeLists.txt +++ b/dart/lcpsolver/CMakeLists.txt @@ -31,6 +31,3 @@ install( #install(TARGETS dart_lcpsolver EXPORT DARTCoreTargets DESTINATION lib) #install(TARGETS dart_lcpsolver EXPORT DARTTargets DESTINATION lib) -# Deprecated LCPSolver.h -dt_issue_303("LCPSolver" "ODELCPSolver") - diff --git a/tools/issue_303_generator.bash b/tools/issue_303_generator.bash deleted file mode 100755 index 888eb21b93474..0000000000000 --- a/tools/issue_303_generator.bash +++ /dev/null @@ -1,32 +0,0 @@ -#!/bin/bash -lower=`echo $1 | tr '[:upper:]' '[:lower:]'` -guard=DART_`echo $1 | tr '[:lower:]' '[:upper:]'`_`echo $1 | tr '[:lower:]' '[:upper:]'`_H_ -cat < Date: Thu, 11 Jun 2015 08:56:28 -0400 Subject: [PATCH 04/19] Remove deprecated API --- LICENSE | 8 +- dart/constraint/ConstraintBase.h | 4 - dart/constraint/ContactConstraint.cpp | 13 - dart/constraint/ContactConstraint.h | 8 - dart/constraint/SoftContactConstraint.cpp | 21 -- dart/constraint/SoftContactConstraint.h | 5 - dart/dynamics/BodyNode.cpp | 271 ------------------ dart/dynamics/BodyNode.h | 323 ---------------------- dart/dynamics/BoxShape.cpp | 4 - dart/dynamics/BoxShape.h | 6 - dart/dynamics/EllipsoidShape.cpp | 4 - dart/dynamics/EllipsoidShape.h | 6 - dart/dynamics/Joint.cpp | 6 - dart/dynamics/Joint.h | 22 -- dart/dynamics/PlaneShape.cpp | 12 - dart/dynamics/PlaneShape.h | 7 - dart/dynamics/PointMass.cpp | 99 ------- dart/dynamics/PointMass.h | 60 ---- dart/dynamics/Skeleton.cpp | 254 +---------------- dart/dynamics/Skeleton.h | 175 +----------- dart/dynamics/SoftBodyNode.h | 2 +- dart/lcpsolver/ODELCPSolver.h | 4 - dart/math/Helpers.h | 16 -- dart/planning/PathPlanner.h | 8 +- dart/simulation/World.cpp | 2 +- dart/simulation/World.h | 1 - dart/utils/Paths.h | 3 - dart/utils/Paths.h.in | 8 - dart/utils/SkelParser.h | 1 - unittests/testOptimizer.cpp | 4 +- 30 files changed, 16 insertions(+), 1341 deletions(-) delete mode 100644 dart/utils/Paths.h delete mode 100644 dart/utils/Paths.h.in diff --git a/LICENSE b/LICENSE index e48d4794c7b45..2a2f1e0d5f15b 100644 --- a/LICENSE +++ b/LICENSE @@ -1,10 +1,10 @@ -Copyright (c) 2011, Georgia Tech Research Corporation +Copyright (c) 2011-2015, Georgia Tech Research Corporation All rights reserved. Georgia Tech Graphics Lab and Humanoid Robotics Lab -Directed by: - - Prof. C. Karen Liu - - Prof. Mike Stilman + +Directed by Prof. C. Karen Liu and Prof. Mike Stilman + This file is provided under the following "BSD-style" License: Redistribution and use in source and binary forms, with or diff --git a/dart/constraint/ConstraintBase.h b/dart/constraint/ConstraintBase.h index 971ed54aa9612..adcca77d5aad2 100644 --- a/dart/constraint/ConstraintBase.h +++ b/dart/constraint/ConstraintBase.h @@ -39,7 +39,6 @@ #include -#include "dart/common/Deprecated.h" #include "dart/dynamics/Ptr.h" namespace dart { @@ -137,9 +136,6 @@ class ConstraintBase size_t mDim; }; -DEPRECATED(4.3) -typedef ConstraintBase Constraint; - } // namespace constraint } // namespace dart diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 6f8c2f470c85d..4a4823c283028 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -64,19 +64,6 @@ double ContactConstraint::mErrorReductionParameter = DART_ERP; double ContactConstraint::mMaxErrorReductionVelocity = DART_MAX_ERV; double ContactConstraint::mConstraintForceMixing = DART_CFM; -//============================================================================== -ContactConstraint::ContactConstraint(const collision::Contact& _contact) - : ConstraintBase(), - mFirstFrictionalDirection(Eigen::Vector3d::UnitZ()), - mIsFrictionOn(true), - mAppliedImpulseIndex(-1), - mIsBounceOn(false), - mActive(false) -{ - dterr << "This constructor is deprecated. Please don't use me anymore." - << std::endl; -} - //============================================================================== ContactConstraint::ContactConstraint(collision::Contact& _contact, double _timeStep) diff --git a/dart/constraint/ContactConstraint.h b/dart/constraint/ContactConstraint.h index 405e83d730414..8f84ff8279459 100644 --- a/dart/constraint/ContactConstraint.h +++ b/dart/constraint/ContactConstraint.h @@ -39,7 +39,6 @@ #include "dart/constraint/ConstraintBase.h" -#include "dart/common/Deprecated.h" #include "dart/math/MathTypes.h" #include "dart/collision/CollisionDetector.h" @@ -56,13 +55,6 @@ namespace constraint { class ContactConstraint : public ConstraintBase { public: - /// Constructor - /// - /// Do not use me anymore. This constructor is not possible to store contact - /// force in _contact - DEPRECATED(4.2) - explicit ContactConstraint(const collision::Contact& _contact); - /// Constructor ContactConstraint(collision::Contact& _contact, double _timeStep); diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp index b44bd6c256193..e24dda5c55197 100644 --- a/dart/constraint/SoftContactConstraint.cpp +++ b/dart/constraint/SoftContactConstraint.cpp @@ -68,27 +68,6 @@ double SoftContactConstraint::mErrorReductionParameter = DART_ERP; double SoftContactConstraint::mMaxErrorReductionVelocity = DART_MAX_ERV; double SoftContactConstraint::mConstraintForceMixing = DART_CFM; -//============================================================================== -SoftContactConstraint::SoftContactConstraint( - const collision::Contact& _contact) - : ConstraintBase(), - mBodyNode1(_contact.bodyNode1.lock()), - mBodyNode2(_contact.bodyNode2.lock()), - mSoftBodyNode1(dynamic_cast(mBodyNode1)), - mSoftBodyNode2(dynamic_cast(mBodyNode2)), - mPointMass1(NULL), - mPointMass2(NULL), - mSoftCollInfo(static_cast(_contact.userData)), - mFirstFrictionalDirection(Eigen::Vector3d::UnitZ()), - mIsFrictionOn(true), - mAppliedImpulseIndex(-1), - mIsBounceOn(false), - mActive(false) -{ - dterr << "This constructor is deprecated. Please don't use me anymore." - << std::endl; -} - //============================================================================== SoftContactConstraint::SoftContactConstraint( collision::Contact& _contact, double _timeStep) diff --git a/dart/constraint/SoftContactConstraint.h b/dart/constraint/SoftContactConstraint.h index 7ee7d5579675c..cd4ef33a94bbd 100644 --- a/dart/constraint/SoftContactConstraint.h +++ b/dart/constraint/SoftContactConstraint.h @@ -39,7 +39,6 @@ #include "dart/constraint/ConstraintBase.h" -#include "dart/common/Deprecated.h" #include "dart/math/MathTypes.h" #include "dart/collision/CollisionDetector.h" @@ -62,10 +61,6 @@ namespace constraint { class SoftContactConstraint : public ConstraintBase { public: - /// Constructor - DEPRECATED(4.2) - explicit SoftContactConstraint(const collision::Contact& _contact); - /// Constructor SoftContactConstraint(collision::Contact& _contact, double _timeStep); diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 689f101296b23..68ea7068e80f0 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -386,26 +386,6 @@ const Eigen::Vector3d& BodyNode::getLocalCOM() const return mBodyP.mInertia.getLocalCOM(); } -//============================================================================== -Eigen::Vector3d BodyNode::getWorldCOM() const -{ - return getWorldTransform() * getLocalCOM(); -} - -//============================================================================== -Eigen::Vector3d BodyNode::getWorldCOMVelocity() const -{ - return getSpatialVelocity(getLocalCOM(), - Frame::World(), Frame::World()).tail<3>(); -} - -//============================================================================== -Eigen::Vector3d BodyNode::getWorldCOMAcceleration() const -{ - return getSpatialAcceleration(getLocalCOM(), - Frame::World(), Frame::World()).tail<3>(); -} - //============================================================================== Eigen::Vector3d BodyNode::getCOM(const Frame* _withRespectTo) const { @@ -844,51 +824,6 @@ const Eigen::Vector6d& BodyNode::getRelativeSpatialVelocity() const return mParentJoint->getLocalSpatialVelocity(); } -//============================================================================== -const Eigen::Vector6d& BodyNode::getBodyVelocity() const -{ - return getSpatialVelocity(); -} - -//============================================================================== -Eigen::Vector3d BodyNode::getBodyLinearVelocity() const -{ - return getSpatialVelocity().tail<3>(); -} - -//============================================================================== -Eigen::Vector3d BodyNode::getBodyLinearVelocity( - const Eigen::Vector3d& _offset) const -{ - const Eigen::Vector6d& V = getSpatialVelocity(); - return V.tail<3>() + V.head<3>().cross(_offset); -} - -//============================================================================== -Eigen::Vector3d BodyNode::getBodyAngularVelocity() const -{ - return getSpatialVelocity().head<3>(); -} - -//============================================================================== -Eigen::Vector3d BodyNode::getWorldLinearVelocity() const -{ - return getLinearVelocity(); -} - -//============================================================================== -Eigen::Vector3d BodyNode::getWorldLinearVelocity( - const Eigen::Vector3d& _offset) const -{ - return getLinearVelocity(_offset); -} - -//============================================================================== -Eigen::Vector3d BodyNode::getWorldAngularVelocity() const -{ - return getAngularVelocity(); -} - //============================================================================== const Eigen::Vector6d& BodyNode::getRelativeSpatialAcceleration() const { @@ -1131,138 +1066,6 @@ math::AngularJacobian BodyNode::getAngularJacobianDeriv( * J_d.topRows<3>(); } -//============================================================================== -const Eigen::Vector6d& BodyNode::getBodyAcceleration() const -{ - return getSpatialAcceleration(); -} - -//============================================================================== -Eigen::Vector3d BodyNode::getBodyLinearAcceleration() const -{ - return getSpatialAcceleration().tail<3>(); -} - -//============================================================================== -Eigen::Vector3d BodyNode::getBodyLinearAcceleration( - const Eigen::Vector3d& _offset) const -{ - return getSpatialAcceleration(_offset).tail<3>(); -} - -//============================================================================== -Eigen::Vector3d BodyNode::getBodyAngularAcceleration() const -{ - return getSpatialAcceleration().head<3>(); -} - -//============================================================================== -Eigen::Vector3d BodyNode::getWorldLinearAcceleration() const -{ - return getSpatialAcceleration(Frame::World(), Frame::World()).tail<3>(); -} - -//============================================================================== -Eigen::Vector3d BodyNode::getWorldLinearAcceleration( - const Eigen::Vector3d& _offset) const -{ - return getSpatialAcceleration(_offset, Frame::World(), Frame::World()).tail<3>(); -} - -//============================================================================== -Eigen::Vector3d BodyNode::getWorldAngularAcceleration() const -{ - return getSpatialAcceleration(Frame::World(), Frame::World()).head<3>(); -} - -//============================================================================== -const math::Jacobian& BodyNode::getBodyJacobian() -{ - return getJacobian(); -} - -//============================================================================== -math::LinearJacobian BodyNode::getBodyLinearJacobian() -{ - return getJacobian().bottomRows<3>(); -} - -//============================================================================== -math::LinearJacobian BodyNode::getBodyLinearJacobian( - const Eigen::Vector3d& _offset) -{ - return getJacobian(_offset).bottomRows<3>(); -} - -//============================================================================== -math::AngularJacobian BodyNode::getBodyAngularJacobian() -{ - return getJacobian().topRows<3>(); -} - -//============================================================================== -math::LinearJacobian BodyNode::getWorldLinearJacobian() -{ - return getJacobian(Frame::World()).bottomRows<3>(); -} - -//============================================================================== -math::LinearJacobian BodyNode::getWorldLinearJacobian( - const Eigen::Vector3d& _offset) -{ - return getJacobian(_offset, Frame::World()).bottomRows<3>(); -} - -//============================================================================== -math::AngularJacobian BodyNode::getWorldAngularJacobian() -{ - return getJacobian(Frame::World()).topRows<3>(); -} - -//============================================================================== -const math::Jacobian& BodyNode::getBodyJacobianDeriv() -{ - return getJacobianSpatialDeriv(); -} - -//============================================================================== -math::LinearJacobian BodyNode::getBodyLinearJacobianDeriv() -{ - return getJacobianSpatialDeriv().bottomRows<3>(); -} - -//============================================================================== -math::LinearJacobian BodyNode::getBodyLinearJacobianDeriv( - const Eigen::Vector3d& _offset) -{ - return getJacobianSpatialDeriv(_offset).bottomRows<3>(); -} - -//============================================================================== -math::AngularJacobian BodyNode::getBodyAngularJacobianDeriv() -{ - return getJacobianSpatialDeriv().topRows<3>(); -} - -//============================================================================== -math::LinearJacobian BodyNode::getWorldLinearJacobianDeriv() -{ - return getJacobianSpatialDeriv(Frame::World()).bottomRows<3>(); -} - -//============================================================================== -math::LinearJacobian BodyNode::getWorldLinearJacobianDeriv( - const Eigen::Vector3d& _offset) -{ - return getJacobianSpatialDeriv(_offset, Frame::World()).bottomRows<3>(); -} - -//============================================================================== -math::AngularJacobian BodyNode::getWorldAngularJacobianDeriv() -{ - return getJacobianSpatialDeriv(Frame::World()).topRows<3>(); -} - //============================================================================== const Eigen::Vector6d& BodyNode::getBodyVelocityChange() const { @@ -1420,8 +1223,6 @@ void BodyNode::init(const SkeletonPtr& _skeleton) mLockedSkeleton->mSkeleton = mSkeleton; } - mParentJoint->init(_skeleton); - //-------------------------------------------------------------------------- // Fill the list of generalized coordinates this node depends on, and sort // it. @@ -1652,12 +1453,6 @@ void BodyNode::updatePartialAcceleration() const mIsPartialAccelerationDirty = false; } -//============================================================================== -void BodyNode::updateAcceleration() -{ - updateAccelerationID(); -} - //============================================================================== void BodyNode::updateAccelerationID() { @@ -1667,13 +1462,6 @@ void BodyNode::updateAccelerationID() assert(!math::isNan(mAcceleration)); } -//============================================================================== -void BodyNode::updateBodyWrench(const Eigen::Vector3d& _gravity, - bool _withExternalForces) -{ - updateTransmittedForceID(_gravity, _withExternalForces); -} - //============================================================================== void BodyNode::updateTransmittedForceID(const Eigen::Vector3d& _gravity, bool _withExternalForces) @@ -1716,12 +1504,6 @@ void BodyNode::updateTransmittedForceID(const Eigen::Vector3d& _gravity, assert(!math::isNan(mF)); } -//============================================================================== -void BodyNode::updateGeneralizedForce(bool _withDampingForces) -{ - updateJointForceID(0.001, _withDampingForces, false); -} - //============================================================================== void BodyNode::updateArtInertia(double _timeStep) const { @@ -1815,24 +1597,6 @@ void BodyNode::updateBiasImpulse() mParentJoint->updateTotalImpulse(mBiasImpulse); } -//============================================================================== -void BodyNode::updateJointAndBodyAcceleration() -{ - updateAccelerationFD(); -} - -//============================================================================== -void BodyNode::updateJointVelocityChange() -{ - updateVelocityChangeFD(); -} - -//============================================================================== -void BodyNode::updateTransmittedWrench() -{ - updateTransmittedForceFD(); -} - //============================================================================== void BodyNode::updateTransmittedForceFD() { @@ -1842,12 +1606,6 @@ void BodyNode::updateTransmittedForceFD() assert(!math::isNan(mF)); } -//============================================================================== -void BodyNode::updateBodyImpForceFwdDyn() -{ - updateTransmittedImpulse(); -} - //============================================================================== void BodyNode::updateTransmittedImpulse() { @@ -2065,12 +1823,6 @@ Eigen::Vector3d BodyNode::getAngularMomentum(const Eigen::Vector3d& _pivot) return math::dAdT(T, mI * getSpatialVelocity()).head<3>(); } -//============================================================================== -bool BodyNode::isImpulseReponsible() const -{ - return isReactive(); -} - //============================================================================== bool BodyNode::isReactive() const { @@ -2098,29 +1850,6 @@ bool BodyNode::isReactive() const } } -//============================================================================== -void BodyNode::updateConstrainedJointAndBodyAcceleration(double /*_timeStep*/) -{ - // 1. dq = dq + del_dq - // mParentJoint->updateVelocityWithVelocityChange(); - - // 2. ddq = ddq + del_dq / dt - // mParentJoint->updateAccelerationWithVelocityChange(_timeStep); - - // 3. tau = tau + imp / dt - // mParentJoint->updateForceWithImpulse(_timeStep); -} - -//============================================================================== -void BodyNode::updateConstrainedTransmittedForce(double _timeStep) -{ - /// - mAcceleration += mDelV / _timeStep; - - /// - mF += mImpF / _timeStep; -} - //============================================================================== void BodyNode::aggregateCoriolisForceVector(Eigen::VectorXd& _C) { diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index c2024f8b08c95..99c44f71caca4 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -45,7 +45,6 @@ #include #include "dart/config.h" -#include "dart/common/Deprecated.h" #include "dart/common/Signal.h" #include "dart/math/Geometry.h" @@ -218,26 +217,6 @@ class BodyNode : public Frame, public SkeletonRefCountingBase /// Return center of mass expressed in body frame const Eigen::Vector3d& getLocalCOM() const; - /// Return center of mass expressed in world frame - /// - /// Deprecated in 4.4. Please use getCOM() - DEPRECATED(4.4) - Eigen::Vector3d getWorldCOM() const; - - /// Return velocity of center of mass expressed in world frame - /// - /// Deprecated in 4.4. Please use getCOMLinearVelocity() or - /// getCOMSpatialVelocity() - DEPRECATED(4.4) - Eigen::Vector3d getWorldCOMVelocity() const; - - /// Return acceleration of center of mass expressed in world frame - /// - /// Deprecated in 4.4. Please use getCOMLinearAcceleration() or - /// getCOMSpatialAcceleration() - DEPRECATED(4.4) - Eigen::Vector3d getWorldCOMAcceleration() const; - /// Return the center of mass with respect to an arbitrary Frame Eigen::Vector3d getCOM(const Frame* _withRespectTo = Frame::World()) const; @@ -670,248 +649,6 @@ class BodyNode : public Frame, public SkeletonRefCountingBase math::AngularJacobian getAngularJacobianDeriv( const Frame* _inCoordinatesOf = Frame::World()) const; - //---------------------------------------------------------------------------- - // Deprecated velocity, acceleration, and Jacobian functions - //---------------------------------------------------------------------------- - - /// Return the spatial velocity at the origin of the bodynode expressed in - /// the body-fixed frame - /// - /// Deprecated in 4.4. Please use getSpatialVelocity() instead. - DEPRECATED(4.4) - const Eigen::Vector6d& getBodyVelocity() const; - - /// Return the linear velocity of the origin of the bodynode expressed in - /// body-fixed frame - /// - /// Deprecated as of 4.4 - /// \sa getSpatialVelocity(), getLinearVelocity() - DEPRECATED(4.4) - Eigen::Vector3d getBodyLinearVelocity() const; - - /// Return the linear velocity of a point on the bodynode expressed in - /// body-fixed frame - /// \param[in] _offset Offset of the point from the origin of the bodynode - /// frame. The offset is expressed in the body-fixed frame - /// - /// Deprecated as of 4.4 - /// \sa getSpatialVelocity(), getLinearVelocity() - DEPRECATED(4.4) - Eigen::Vector3d getBodyLinearVelocity(const Eigen::Vector3d& _offset) const; - - /// Return the angular velocity expressed in body-fixed frame - /// - /// Deprecated as of 4.4 - /// \sa getSpatialVelocity(), getAngularVelocity() - DEPRECATED(4.4) - Eigen::Vector3d getBodyAngularVelocity() const; - - /// Return the linear velocity of the origin of the bodynode expressed in - /// world frame - /// - /// Deprecated as of 4.4 - /// \sa getSpatialVelocity(), getLinearVelocity() - DEPRECATED(4.4) - Eigen::Vector3d getWorldLinearVelocity() const; - - /// Return the linear velocity of a point on the bodynode expressed in world - /// frame - /// \param[in] _offset Offset of the point from the origin of the bodynode - /// frame. The offset is expressed in the body-fixed frame - /// - /// Deprecated as of 4.4 - /// \sa getSpatialVelocity(), getLinearVelocity() - DEPRECATED(4.4) - Eigen::Vector3d getWorldLinearVelocity(const Eigen::Vector3d& _offset) const; - - /// Return the angular velocity expressed in world frame - /// - /// Deprecated as of 4.4 - /// \sa getSpatialVelocity(), getAngularVelocity() - DEPRECATED(4.4) - Eigen::Vector3d getWorldAngularVelocity() const; - - /// Return generalized acceleration at the origin of this body node where the - /// acceleration is expressed in this body node frame - /// - /// Deprecated as of 4.4 - /// \sa getSpatialAcceleration() - DEPRECATED(4.4) - const Eigen::Vector6d& getBodyAcceleration() const; - - /// Return the linear acceleration of the origin of the bodynode expressed in - /// body-fixed frame - /// - /// Deprecated as of 4.4 - /// \sa getSpatialAcceleration(), getLinearAcceleration() - DEPRECATED(4.4) - Eigen::Vector3d getBodyLinearAcceleration() const; - - /// Return the linear acceleration of a point on the bodynode expressed in - /// body-fixed frame - /// \param[in] _offset Offset of the point from the origin of the bodynode - /// frame. The offset is expressed in the body-fixed frame - /// - /// Deprecated as of 4.4 - /// \sa getSpatialAcceleration(), getLinearAcceleration() - DEPRECATED(4.4) - Eigen::Vector3d getBodyLinearAcceleration( - const Eigen::Vector3d& _offset) const; - - /// Return the angular acceleration expressed in body-fixed frame - /// - /// Deprecated as of 4.4 - /// \sa getSpatialAcceleration(), getAngularAcceleration() - DEPRECATED(4.4) - Eigen::Vector3d getBodyAngularAcceleration() const; - - /// Return the linear acceleration of the origin of the bodynode expressed in - /// world frame - /// - /// Deprecated as of 4.4 - /// \sa getSpatialAcceleration(), getLinearAcceleration() - DEPRECATED(4.4) - Eigen::Vector3d getWorldLinearAcceleration() const; - - /// Return the linear acceleration of a point on the bodynode expressed in - /// world frame - /// \param[in] _offset Offset of the point from the origin of the bodynode - /// frame. The offset is expressed in the body-fixed frame - /// - /// Deprecated as of 4.4 - /// \sa getSpatialAcceleration(), getLinearAcceleration() - DEPRECATED(4.4) - Eigen::Vector3d getWorldLinearAcceleration( - const Eigen::Vector3d& _offset) const; - - /// Return the angular acceleration expressed in world frame - /// - /// Deprecated as of 4.4 - /// \sa getSpatialAcceleration(), getAngularAcceleration() - DEPRECATED(4.4) - Eigen::Vector3d getWorldAngularAcceleration() const; - - /// Return generalized Jacobian at the origin of this body node where the - /// Jacobian is expressed in this body node frame - /// - /// Deprecated as of 4.4 - /// \sa getJacobian() - DEPRECATED(4.4) - const math::Jacobian& getBodyJacobian(); - - /// Return the linear Jacobian of the origin of the bodynode expressed in - /// body-fixed frame - /// - /// Deprecated as of 4.4 - /// \sa getJacobian(), getLinearJacobian() - DEPRECATED(4.4) - math::LinearJacobian getBodyLinearJacobian(); - - /// Return the linear Jacobian of a point on the bodynode expressed in - /// body-fixed frame - /// \param[in] _offset Offset of the point from the origin of the bodynode - /// frame. The offset is expressed in the body-fixed frame - /// - /// Deprecated as of 4.4 - /// \sa getJacobian(), getLinearJacobian() - DEPRECATED(4.4) - math::LinearJacobian getBodyLinearJacobian(const Eigen::Vector3d& _offset); - - /// Return the angular Jacobian expressed in body-fixed frame - /// - /// Deprecated as of 4.4 - /// \sa getJacobian(), getAngularJacobian() - DEPRECATED(4.4) - math::AngularJacobian getBodyAngularJacobian(); - - /// Return the linear Jacobian of the origin of the bodynode expressed in - /// world frame - /// - /// Deprecated as of 4.4 - /// \sa getJacobian(), getLinearJacobian() - DEPRECATED(4.4) - math::LinearJacobian getWorldLinearJacobian(); - - /// Return the linear Jacobian of a point on the bodynode expressed in world - /// frame - /// \param[in] _offset Offset of the point from the origin of the bodynode - /// frame. The offset is expressed in the body-fixed frame - /// - /// Deprecated as of 4.4 - /// \sa getJacobian(), getLinearJacobian() - DEPRECATED(4.4) - math::LinearJacobian getWorldLinearJacobian(const Eigen::Vector3d& _offset); - - /// Return the angular Jacobian expressed in world frame - /// - /// Deprecated as of 4.4 - /// \sa getJacobian(), getAngularJacobian() - DEPRECATED(4.4) - math::AngularJacobian getWorldAngularJacobian(); - - /// Return time derivative of generalized Jacobian at the origin of this body - /// node where the Jacobian is expressed in this body node frame - /// - /// Deprecated as of 4.4. Replaced by getJacobianSpatialDeriv() - /// \sa getJacobianSpatialDeriv(), getJacobianClassicDeriv() - DEPRECATED(4.4) - const math::Jacobian& getBodyJacobianDeriv(); - - /// Return the time derivative of the linear Jacobian of the origin of the - /// bodynode expressed in body-fixed frame - /// - /// Deprecated as of 4.4 - /// \sa getJacobianSpatialDeriv(), getLinearJacobianClassicDeriv() - DEPRECATED(4.4) - math::LinearJacobian getBodyLinearJacobianDeriv(); - - /// Return the time derivative of the linear Jacobian of a point on the - /// bodynode expressed in body-fixed frame - /// \param[in] _offset Offset of the point from the origin of the bodynode - /// frame. The offset is expressed in the body-fixed frame - /// - /// Deprecated as of 4.4 - /// \sa getJacobianSpatialDeriv(), getLinearJacobianClassicDeriv() - DEPRECATED(4.4) - math::LinearJacobian getBodyLinearJacobianDeriv( - const Eigen::Vector3d& _offset); - - /// Return the time derivative of the angular Jacobian expressed in body-fixed - /// frame - /// - /// Deprecated as of 4.4 - /// \sa getJacobianSpatialDeriv(), getAngularJacobianClassicDeriv() - DEPRECATED(4.4) - math::AngularJacobian getBodyAngularJacobianDeriv(); - - /// Return the time derivative of the linear Jacobian of the origin of the - /// bodynode expressed in world frame - /// - /// Deprecated as of 4.4 - /// \sa getJacobianSpatialDeriv(), getLinearJacobianClassicDeriv() - DEPRECATED(4.4) - math::LinearJacobian getWorldLinearJacobianDeriv(); - - /// Return the time derivative of the linear Jacobian of a point on the - /// bodynode expressed in world frame - /// \param[in] _offset Offset of the point from the origin of the bodynode - /// frame. The offset is expressed in the body-fixed frame - /// - /// Deprecated as of 4.4 - /// \sa getJacobianSpatialDeriv(), getLinearJacobianClassicDeriv() - DEPRECATED(4.4) - math::LinearJacobian getWorldLinearJacobianDeriv( - const Eigen::Vector3d& _offset); - - /// Return the angular Jacobian expressed in world frame - /// - /// Deprecated as of 4.4 - /// \sa getJacobianSpatialDeriv(), getAngularJacobianClassicDeriv() - DEPRECATED(4.4) - math::AngularJacobian getWorldAngularJacobianDeriv(); - - // --- End of Deprecated functions ------------------------------------------- - /// Return the velocity change due to the constraint impulse const Eigen::Vector6d& getBodyVelocityChange() const; @@ -981,10 +718,6 @@ class BodyNode : public Frame, public SkeletonRefCountingBase // - Following functions are managed by constraint solver. //---------------------------------------------------------------------------- - /// Deprecated in 4.2. Please use isReactive(). - DEPRECATED(4.2) - bool isImpulseReponsible() const; - /// Return true if the body can react to force or constraint impulse. /// /// A body node is reactive if the skeleton is mobile and the number of @@ -1171,62 +904,6 @@ class BodyNode : public Frame, public SkeletonRefCountingBase /// dynamics. virtual void updateConstrainedTerms(double _timeStep); - //- DEPRECATED --------------------------------------------------------------- - - /// Update spatial body acceleration with the partial spatial body - /// acceleration. - /// \warning Please use updateAccelerationID(). - DEPRECATED(4.3) - virtual void updateAcceleration(); - - /// Update the joint acceleration and body acceleration - /// \warning Please use updateAccelerationFD(). - DEPRECATED(4.3) - virtual void updateJointAndBodyAcceleration(); - - /// Update joint velocity change for impulse-based forward dynamics algorithm - /// \warning Please use updateVelocityChangeFD(). - DEPRECATED(4.3) - virtual void updateJointVelocityChange(); - - /// Update the spatial body force transmitted to this BodyNode from the - /// parent body through the connecting joint. The spatial body force is - /// expressed in this BodyNode's frame. - /// \warning Please use updateTransmittedForceFD(). - DEPRECATED(4.3) - virtual void updateTransmittedWrench(); - - /// Update spatial body force. Inverse dynamics routine. - /// - /// The spatial body force is transmitted to this BodyNode from the parent - /// body through the connecting joint. It is expressed in this BodyNode's - /// frame. - /// - /// \warning Please use updateTransmittedForceID(). - DEPRECATED(4.3) - virtual void updateBodyWrench(const Eigen::Vector3d& _gravity, - bool _withExternalForces = false); - - /// updateBodyImpForceFwdDyn - /// \warning Please use updateTransmittedImpulse(). - DEPRECATED(4.3) - virtual void updateBodyImpForceFwdDyn(); - - /// Update the joint force - /// \warning Please use updateJointForceID(). - DEPRECATED(4.3) - virtual void updateGeneralizedForce(bool _withDampingForces = false); - - /// updateConstrainedJointAndBodyAcceleration - /// \warning Deprecated. Please do not use. - DEPRECATED(4.3) - virtual void updateConstrainedJointAndBodyAcceleration(double _timeStep); - - /// updateConstrainedTransmittedForce - /// \warning Deprecated. Please do not use. - DEPRECATED(4.3) - virtual void updateConstrainedTransmittedForce(double _timeStep); - /// \} //---------------------------------------------------------------------------- diff --git a/dart/dynamics/BoxShape.cpp b/dart/dynamics/BoxShape.cpp index 043d6ff0e3ea8..eda3fdd7560f9 100644 --- a/dart/dynamics/BoxShape.cpp +++ b/dart/dynamics/BoxShape.cpp @@ -56,10 +56,6 @@ BoxShape::BoxShape(const Eigen::Vector3d& _size) BoxShape::~BoxShape() { } -void BoxShape::setDim(const Eigen::Vector3d& _size) { - setSize(_size); -} - void BoxShape::setSize(const Eigen::Vector3d& _size) { assert(_size[0] > 0.0); assert(_size[1] > 0.0); diff --git a/dart/dynamics/BoxShape.h b/dart/dynamics/BoxShape.h index e5e67a30bb23b..a9c86fe30390b 100644 --- a/dart/dynamics/BoxShape.h +++ b/dart/dynamics/BoxShape.h @@ -38,7 +38,6 @@ #ifndef DART_DYNAMICS_BOXSHAPE_H_ #define DART_DYNAMICS_BOXSHAPE_H_ -#include "dart/common/Deprecated.h" #include "dart/dynamics/Shape.h" namespace dart { @@ -52,11 +51,6 @@ class BoxShape : public Shape { /// \brief Destructor. virtual ~BoxShape(); - /// \brief Set size of this box. - /// \warning Don't use me any more - DEPRECATED(4.0) - void setDim(const Eigen::Vector3d& _size); - /// \brief Set size of this box. void setSize(const Eigen::Vector3d& _size); diff --git a/dart/dynamics/EllipsoidShape.cpp b/dart/dynamics/EllipsoidShape.cpp index 84fb007104119..da2ddfd69af94 100644 --- a/dart/dynamics/EllipsoidShape.cpp +++ b/dart/dynamics/EllipsoidShape.cpp @@ -51,10 +51,6 @@ EllipsoidShape::EllipsoidShape(const Eigen::Vector3d& _size) EllipsoidShape::~EllipsoidShape() { } -void EllipsoidShape::setDim(const Eigen::Vector3d& _size) { - setSize(_size); -} - void EllipsoidShape::setSize(const Eigen::Vector3d& _size) { assert(_size[0] > 0.0); assert(_size[1] > 0.0); diff --git a/dart/dynamics/EllipsoidShape.h b/dart/dynamics/EllipsoidShape.h index 4d155c3016b11..b065621864f59 100644 --- a/dart/dynamics/EllipsoidShape.h +++ b/dart/dynamics/EllipsoidShape.h @@ -38,7 +38,6 @@ #ifndef DART_DYNAMICS_ELLIPSOIDSHAPE_H_ #define DART_DYNAMICS_ELLIPSOIDSHAPE_H_ -#include "dart/common/Deprecated.h" #include "dart/dynamics/Shape.h" namespace dart { @@ -52,11 +51,6 @@ class EllipsoidShape : public Shape { /// \brief Destructor. virtual ~EllipsoidShape(); - /// \brief Set size of this box. - /// \warning Don't use me any more - DEPRECATED(4.0) - void setDim(const Eigen::Vector3d& _size); - /// \brief Set size of this box. void setSize(const Eigen::Vector3d& _size); diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index 2c841dc3b2cd1..378e2e2739393 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -358,12 +358,6 @@ Joint::Joint(const Properties& _properties) // Do nothing } -//============================================================================== -void Joint::init(const SkeletonPtr&) -{ - // Currently unused -} - //============================================================================== DegreeOfFreedom* Joint::createDofPointer(size_t _indexInJoint) { diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index f5ffaa2e23a04..c07e506068d31 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -43,7 +43,6 @@ #include #include "dart/common/Subject.h" -#include "dart/common/Deprecated.h" #include "dart/math/MathTypes.h" #include "dart/dynamics/Ptr.h" @@ -634,13 +633,6 @@ class Joint : public virtual common::Subject /// Called by the Skeleton class virtual void registerDofs() = 0; - /// Initialize this joint. This function is called by BodyNode::init() - /// - /// Note: This function is being deprecated due to lack of use. If this is - /// something you need for a custom Joint type, please inform us on Github. - DEPRECATED(5.0) - virtual void init(const SkeletonPtr& _skel); - /// \brief Create a DegreeOfFreedom pointer. /// \param[in] _name DegreeOfFreedom's name. /// \param[in] _indexInJoint DegreeOfFreedom's index within the joint. Note @@ -788,20 +780,6 @@ class Joint : public virtual common::Subject /// Update constrained terms for forward dynamics virtual void updateConstrainedTerms(double _timeStep) = 0; - //- DEPRECATED --------------------------------------------------------------- - - /// updateVelocityWithVelocityChange - DEPRECATED(4.3) - virtual void updateVelocityWithVelocityChange() {} - - /// updateAccelerationWithVelocityChange - DEPRECATED(4.3) - virtual void updateAccelerationWithVelocityChange(double _timeStep) {} - - /// updateForceWithImpulse - DEPRECATED(4.3) - virtual void updateForceWithImpulse(double _timeStep) {} - /// \} //---------------------------------------------------------------------------- diff --git a/dart/dynamics/PlaneShape.cpp b/dart/dynamics/PlaneShape.cpp index 3dfdc79afe17a..9dbed04c474be 100644 --- a/dart/dynamics/PlaneShape.cpp +++ b/dart/dynamics/PlaneShape.cpp @@ -94,18 +94,6 @@ const Eigen::Vector3d& PlaneShape::getNormal() const return mNormal; } -//============================================================================== -void PlaneShape::setPoint(const Eigen::Vector3d& _point) -{ - mOffset = mNormal.dot(_point); -} - -//============================================================================== -Eigen::Vector3d PlaneShape::getPoint() const -{ - return mNormal * mOffset; -} - //============================================================================== void PlaneShape::setOffset(double _offset) { diff --git a/dart/dynamics/PlaneShape.h b/dart/dynamics/PlaneShape.h index be8c265280594..204f982eb54e4 100644 --- a/dart/dynamics/PlaneShape.h +++ b/dart/dynamics/PlaneShape.h @@ -37,7 +37,6 @@ #ifndef DART_DYNAMICS_PLANESHAPE_H_ #define DART_DYNAMICS_PLANESHAPE_H_ -#include "dart/common/Deprecated.h" #include "dart/dynamics/Shape.h" namespace dart { @@ -87,12 +86,6 @@ class PlaneShape : public Shape /// Compute signed distance between the plane and the given point double computeSignedDistance(const Eigen::Vector3d& _point) const; - DEPRECATED(4.3) - void setPoint(const Eigen::Vector3d& _point); - - DEPRECATED(4.3) - Eigen::Vector3d getPoint() const; - private: // Documentation inherited. void computeVolume(); diff --git a/dart/dynamics/PointMass.cpp b/dart/dynamics/PointMass.cpp index f33fcc9d281d9..064da04f84a22 100644 --- a/dart/dynamics/PointMass.cpp +++ b/dart/dynamics/PointMass.cpp @@ -459,24 +459,6 @@ void PointMass::integrateVelocities(double _dt) setVelocities( getVelocities() + getAccelerations() * _dt ); } -//============================================================================== -void PointMass::updateVelocityWithVelocityChange() -{ - setVelocities( getVelocities() + mVelocityChanges ); -} - -//============================================================================== -void PointMass::updateAccelerationWithVelocityChange(double _timeStep) -{ - setAccelerations( getAccelerations() + mVelocityChanges / _timeStep ); -} - -//============================================================================== -void PointMass::updateForceWithImpulse(double _timeStep) -{ - mForces.noalias() += mConstraintImpulses / _timeStep; -} - //============================================================================== void PointMass::addExtForce(const Eigen::Vector3d& _force, bool _isForceLocal) { @@ -714,12 +696,6 @@ void PointMass::updatePartialAcceleration() const assert(!math::isNan(mEta)); } -//============================================================================== -void PointMass::updateAcceleration() -{ - updateAccelerationID(); -} - //============================================================================== void PointMass::updateAccelerationID() const { @@ -730,13 +706,6 @@ void PointMass::updateAccelerationID() const assert(!math::isNan(mA)); } -//============================================================================== -void PointMass::updateBodyForce(const Eigen::Vector3d& _gravity, - bool _withExternalForces) -{ - updateTransmittedForceID(_gravity, _withExternalForces); -} - //============================================================================== void PointMass::updateTransmittedForceID(const Eigen::Vector3d& _gravity, bool _withExternalForces) @@ -753,12 +722,6 @@ void PointMass::updateTransmittedForceID(const Eigen::Vector3d& _gravity, assert(!math::isNan(mF)); } -//============================================================================== -void PointMass::updateArticulatedInertia(double _dt) -{ - updateArtInertiaFD(_dt); -} - //============================================================================== void PointMass::updateArtInertiaFD(double _timeStep) const { @@ -784,12 +747,6 @@ void PointMass::updateArtInertiaFD(double _timeStep) const assert(!math::isNan(mImplicitPi)); } -//============================================================================== -void PointMass::updateGeneralizedForce(bool /*_withDampingForces*/) -{ - updateJointForceID(false, false, false); -} - //============================================================================== void PointMass::updateJointForceID(double /*_timeStep*/, double /*_withDampingForces*/, @@ -839,12 +796,6 @@ void PointMass::updateBiasForceFD(double _dt, const Eigen::Vector3d& _gravity) assert(!math::isNan(mBeta)); } -//============================================================================== -void PointMass::updateJointAndBodyAcceleration() -{ - updateAccelerationFD(); -} - //============================================================================== void PointMass::updateAccelerationFD() { @@ -881,12 +832,6 @@ void PointMass::updateMassMatrix() assert(!math::isNan(mM_dV)); } -//============================================================================== -void PointMass::updateBiasImpulse() -{ - updateBiasImpulseFD(); -} - //============================================================================== void PointMass::updateBiasImpulseFD() { @@ -902,12 +847,6 @@ void PointMass::updateBiasImpulseFD() assert(!math::isNan(mImpBeta)); } -//============================================================================== -void PointMass::updateJointVelocityChange() -{ - updateVelocityChangeFD(); -} - //============================================================================== void PointMass::updateVelocityChangeFD() { @@ -934,20 +873,6 @@ void PointMass::updateVelocityChangeFD() assert(!math::isNan(mDelV)); } -//============================================================================== -void PointMass::updateBodyVelocityChange() -{ - mDelV = mParentSoftBodyNode->getBodyVelocityChange().head<3>().cross(getLocalPosition()) - + mParentSoftBodyNode->getBodyVelocityChange().tail<3>() - + mVelocityChanges; - assert(!math::isNan(mDelV)); -} - -//============================================================================== -void PointMass::updateBodyImpForceFwdDyn() -{ - updateTransmittedImpulse(); -} //============================================================================== void PointMass::updateTransmittedImpulse() @@ -957,30 +882,6 @@ void PointMass::updateTransmittedImpulse() assert(!math::isNan(mImpF)); } -//============================================================================== -void PointMass::updateConstrainedJointAndBodyAcceleration(double _timeStep) -{ -// // 1. dq = dq + del_dq -// updateVelocityWithVelocityChange(); - -// // 2. ddq = ddq + del_dq / dt -// updateAccelerationWithVelocityChange(_timeStep); - -// // 3. tau = tau + imp / dt -// updateForceWithImpulse(_timeStep); -} - -//============================================================================== -void PointMass::updateConstrainedTransmittedForce(double _timeStep) -{ - /// -// mA += mDelV / _timeStep; - setAccelerations( getAccelerations() + mDelV / _timeStep ); - - /// - mF += _timeStep * mImpF; -} - //============================================================================== void PointMass::updateConstrainedTermsFD(double _timeStep) { diff --git a/dart/dynamics/PointMass.h b/dart/dynamics/PointMass.h index 1c908e03c4266..4fa19ebc2b1ed 100644 --- a/dart/dynamics/PointMass.h +++ b/dart/dynamics/PointMass.h @@ -40,7 +40,6 @@ #include #include #include "dart/dynamics/Entity.h" -#include "dart/common/Deprecated.h" namespace dart { namespace renderer { @@ -441,65 +440,6 @@ class PointMass : public common::Subject /// dynamics routine. void updateConstrainedTermsFD(double _timeStep); - //- DEPRECATED --------------------------------------------------------------- - - /// \warning Please use updateAccelerationID(). - DEPRECATED(4.3) - void updateAcceleration(); - - /// \warning Please use updateTransmittedForceID(). - DEPRECATED(4.3) - void updateBodyForce(const Eigen::Vector3d& _gravity, - bool _withExternalForces = false); - - /// \warning Please use updateArtInertiaFD(). - DEPRECATED(4.3) - void updateArticulatedInertia(double _dt); - - /// \warning Please use updateJointForceID(). - DEPRECATED(4.3) - void updateGeneralizedForce(bool _withDampingForces = false); - - /// \warning Please use updateAccelerationFD(). - DEPRECATED(4.3) - void updateJointAndBodyAcceleration(); - - /// \brief Update impulsive bias force for impulse-based forward dynamics - /// algorithm. - /// \warning Please use updateBiasImpulseFD(). - DEPRECATED(4.3) - void updateBiasImpulse(); - - /// \brief Update joint velocity change for impulse-based forward dynamics - /// algorithm. - /// \warning Please use updateVelocityChangeFD(). - DEPRECATED(4.3) - void updateJointVelocityChange(); - - /// \brief Update body velocity change for impulse-based forward dynamics - /// algorithm. - DEPRECATED(4.3) - void updateBodyVelocityChange(); - - /// \warning Please use updateTransmittedImpulse(). - DEPRECATED(4.3) - void updateBodyImpForceFwdDyn(); - - DEPRECATED(4.3) - void updateConstrainedJointAndBodyAcceleration(double _timeStep); - - DEPRECATED(4.3) - void updateConstrainedTransmittedForce(double _timeStep); - - DEPRECATED(4.3) - void updateVelocityWithVelocityChange(); - - DEPRECATED(4.3) - void updateAccelerationWithVelocityChange(double _timeStep); - - DEPRECATED(4.3) - void updateForceWithImpulse(double _timeStep); - /// \} //---------------------------------------------------------------------------- diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 70987fc27fc1e..0718e5142af4f 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -322,25 +322,6 @@ const Eigen::Vector3d& Skeleton::getGravity() const return mSkeletonP.mGravity; } -//============================================================================== -void Skeleton::addBodyNode(BodyNode* _body) -{ - assert(_body && _body->getParentJoint()); - - mSkelCache.mBodyNodes.push_back(_body); - addEntryToBodyNodeNameMgr(_body); - addMarkersOfBodyNode(_body); - _body->mSkeleton = mPtr; - registerJoint(_body->getParentJoint()); - - SoftBodyNode* softBodyNode = dynamic_cast(_body); - if (softBodyNode) - { - mSoftBodyNodes.push_back(softBodyNode); - addEntryToSoftBodyNodeNameMgr(softBodyNode); - } -} - //============================================================================== size_t Skeleton::getNumBodyNodes() const { @@ -647,106 +628,6 @@ const Marker* Skeleton::getMarker(const std::string& _name) const return const_cast(this)->getMarker(_name); } -//============================================================================== -void Skeleton::init(double _timeStep, const Eigen::Vector3d& _gravity) -{ - // Set timestep and gravity - setTimeStep(_timeStep); - setGravity(_gravity); - - // Get root bodynodes that don't have parent bodynode - std::vector rootBodyNodes; - for (size_t i = 0; i < mSkelCache.mBodyNodes.size(); ++i) - { - if (mSkelCache.mBodyNodes[i]->getParentBodyNode() == nullptr) - rootBodyNodes.push_back(mSkelCache.mBodyNodes[i]); - } - - // Rearrange the list of body nodes with BFS (Breadth First Search) - std::queue queue; - mSkelCache.mBodyNodes.clear(); - mSkelCache.mDofs.clear(); - mTreeCache.clear(); - mNameMgrForBodyNodes.clear(); - mNameMgrForJoints.clear(); - mSoftBodyNodes.clear(); - mNameMgrForSoftBodyNodes.clear(); - mNameMgrForDofs.clear(); - mNameMgrForMarkers.clear(); - for (size_t i = 0; i < rootBodyNodes.size(); ++i) - { - queue.push(rootBodyNodes[i]); - - while (!queue.empty()) - { - BodyNode* itBodyNode = queue.front(); - queue.pop(); - registerBodyNode(itBodyNode); - for (size_t j = 0; j < itBodyNode->getNumChildBodyNodes(); ++j) - queue.push(itBodyNode->getChildBodyNode(j)); - } - } - - /////////////////////////////////////////////////////////////////////////// - - // Clear external/internal force - clearExternalForces(); - resetGeneralizedForces(); -} - -//============================================================================== -void Skeleton::setPositionSegment(const std::vector& _indices, - const Eigen::VectorXd& _positions) -{ - setPositions(_indices, _positions); -} - -//============================================================================== -Eigen::VectorXd Skeleton::getPositionSegment( - const std::vector& _indices) const -{ - return getPositions(_indices); -} - -//============================================================================== -void Skeleton::setVelocitySegment(const std::vector& _indices, - const Eigen::VectorXd& _velocities) -{ - setVelocities(_indices, _velocities); -} - -//============================================================================== -Eigen::VectorXd Skeleton::getVelocitySegment(const std::vector& _id) const -{ - return getVelocities(_id); -} - -//============================================================================== -void Skeleton::setAccelerationSegment(const std::vector& _indices, - const Eigen::VectorXd& _accelerations) -{ - setAccelerations(_indices, _accelerations); -} - -//============================================================================== -Eigen::VectorXd Skeleton::getAccelerationSegment( - const std::vector& _indices) const -{ - return getAccelerations(_indices); -} - -//============================================================================== -void Skeleton::setConstraintImpulses(const Eigen::VectorXd& _impulses) -{ - setJointConstraintImpulses(_impulses); -} - -//============================================================================== -Eigen::VectorXd Skeleton::getConstraintImpulses() const -{ - return getJointConstraintImpulses(); -} - //============================================================================== void Skeleton::setState(const Eigen::VectorXd& _state) { @@ -1358,30 +1239,6 @@ const Eigen::VectorXd& Skeleton::getConstraintForces() const return computeConstraintForces(mSkelCache); } -//============================================================================== -const Eigen::VectorXd& Skeleton::getCoriolisForceVector() const -{ - return getCoriolisForces(); -} - -//============================================================================== -const Eigen::VectorXd& Skeleton::getGravityForceVector() const -{ - return getGravityForces(); -} - -//============================================================================== -const Eigen::VectorXd& Skeleton::getCombinedVector() const -{ - return getCoriolisAndGravityForces(); -} - -//============================================================================== -const Eigen::VectorXd& Skeleton::getExternalForceVector() const -{ - return getExternalForces(); -} - //============================================================================== //const Eigen::VectorXd& Skeleton::getDampingForceVector() { // if (mIsDampingForceVectorDirty) @@ -1389,12 +1246,6 @@ const Eigen::VectorXd& Skeleton::getExternalForceVector() const // return mFd; //} -//============================================================================== -const Eigen::VectorXd& Skeleton::getConstraintForceVector() -{ - return getConstraintForces(); -} - //============================================================================== void Skeleton::draw(renderer::RenderInterface* _ri, const Eigen::Vector4d& _color, bool _useDefaultColor) const @@ -2323,12 +2174,6 @@ void Skeleton::updateInvAugMassMatrix() const mSkelCache.mDirty.mInvAugMassMatrix = false; } -//============================================================================== -void Skeleton::updateCoriolisForceVector() -{ - updateCoriolisForces(); -} - //============================================================================== void Skeleton::updateCoriolisForces(size_t _treeIdx) const { @@ -2386,12 +2231,6 @@ void Skeleton::updateCoriolisForces() const mSkelCache.mDirty.mCoriolisForces = false; } -//============================================================================== -void Skeleton::updateGravityForceVector() -{ - updateGravityForces(); -} - //============================================================================== void Skeleton::updateGravityForces(size_t _treeIdx) const { @@ -2443,12 +2282,6 @@ void Skeleton::updateGravityForces() const mSkelCache.mDirty.mGravityForces = false; } -//============================================================================== -void Skeleton::updateCombinedVector() -{ - updateCoriolisAndGravityForces(); -} - //============================================================================== void Skeleton::updateCoriolisAndGravityForces(size_t _treeIdx) const { @@ -2506,12 +2339,6 @@ void Skeleton::updateCoriolisAndGravityForces() const mSkelCache.mDirty.mCoriolisAndGravityForces = false; } -//============================================================================== -void Skeleton::updateExternalForceVector() -{ - updateExternalForces(); -} - //============================================================================== void Skeleton::updateExternalForces(size_t _treeIdx) const { @@ -2621,26 +2448,6 @@ const Eigen::VectorXd& Skeleton::computeConstraintForces(DataCache& cache) const //============================================================================== void Skeleton::computeForwardDynamics() -{ - computeForwardDynamicsRecursionPartB(); -} - -//============================================================================== -void Skeleton::computeForwardDynamicsRecursionPartA() -{ - // Update body transformations, velocities, and partial acceleration due to - // parent joint's velocity - for (std::vector::iterator it = mSkelCache.mBodyNodes.begin(); - it != mSkelCache.mBodyNodes.end(); ++it) - { - (*it)->updateTransform(); - (*it)->updateVelocity(); - (*it)->updatePartialAcceleration(); - } -} - -//============================================================================== -void Skeleton::computeForwardDynamicsRecursionPartB() { // Note: Articulated Inertias will be updated automatically when // getArtInertiaImplicit() is called in BodyNode::updateBiasForce() @@ -2660,29 +2467,8 @@ void Skeleton::computeForwardDynamicsRecursionPartB() //============================================================================== void Skeleton::computeInverseDynamics(bool _withExternalForces, - bool _withDampingForces) -{ - // - computeInverseDynamicsRecursionB(_withExternalForces, _withDampingForces); -} - -//============================================================================== -void Skeleton::computeInverseDynamicsRecursionA() -{ - for (std::vector::iterator it = mSkelCache.mBodyNodes.begin(); - it != mSkelCache.mBodyNodes.end(); ++it) - { - (*it)->updateTransform(); - (*it)->updateVelocity(); - (*it)->updatePartialAcceleration(); - (*it)->updateAccelerationID(); - } -} - -//============================================================================== -void Skeleton::computeInverseDynamicsRecursionB(bool _withExternalForces, - bool _withDampingForces, - bool _withSpringForces) + bool _withDampingForces, + bool _withSpringForces) { // Skip immobile or 0-dof skeleton if (getNumDofs() == 0) @@ -2919,12 +2705,6 @@ void Skeleton::computeImpulseForwardDynamics() } } -//============================================================================== -void Skeleton::setConstraintForceVector(const Eigen::VectorXd& _Fc) -{ - mSkelCache.mFc = _Fc; -} - //============================================================================== double Skeleton::getKineticEnergy() const { @@ -3096,36 +2876,6 @@ math::LinearJacobian Skeleton::getCOMLinearJacobianDeriv( &BodyNode::getLinearJacobianDeriv>(this, _inCoordinatesOf); } -//============================================================================== -Eigen::Vector3d Skeleton::getWorldCOM() -{ - return getCOM(Frame::World()); -} - -//============================================================================== -Eigen::Vector3d Skeleton::getWorldCOMVelocity() -{ - return getCOMLinearVelocity(); -} - -//============================================================================== -Eigen::Vector3d Skeleton::getWorldCOMAcceleration() -{ - return getCOMLinearAcceleration(); -} - -//============================================================================== -Eigen::MatrixXd Skeleton::getWorldCOMJacobian() -{ - return getCOMLinearJacobian(); -} - -//============================================================================== -Eigen::MatrixXd Skeleton::getWorldCOMJacobianTimeDeriv() -{ - return getCOMLinearJacobianDeriv(); -} - //============================================================================== Skeleton::DirtyFlags::DirtyFlags() : mArticulatedInertia(true), diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 8e92deb523d35..c924746952d37 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -38,7 +38,6 @@ #ifndef DART_DYNAMICS_SKELETON_H_ #define DART_DYNAMICS_SKELETON_H_ -#include "dart/common/Deprecated.h" #include "dart/common/NameManager.h" #include "dart/dynamics/MetaSkeleton.h" #include "dart/dynamics/Ptr.h" @@ -188,10 +187,6 @@ class Skeleton : public MetaSkeleton const typename NodeType::Properties& _bodyProperties = typename NodeType::Properties()); - /// Add a body node - DEPRECATED(4.5) - void addBodyNode(BodyNode* _body); - // Documentation inherited size_t getNumBodyNodes() const override; @@ -307,77 +302,6 @@ class Skeleton : public MetaSkeleton /// \} - //---------------------------------------------------------------------------- - /// \{ \name Deprecated - //---------------------------------------------------------------------------- - - /// Initialize this skeleton for kinematics and dynamics - DEPRECATED(4.5) - void init(double _timeStep = 0.001, - const Eigen::Vector3d& _gravity = Eigen::Vector3d(0.0, 0.0, -9.81)); - - /// Set the configuration of this skeleton described in generalized - /// coordinates. The order of input configuration is determined by _id. - /// - /// DEPRECATED: Use setPositionSegment(const std::vector&, - /// const Eigen::VectorXd&) instead - DEPRECATED(4.5) - void setPositionSegment(const std::vector& _indices, - const Eigen::VectorXd& _positions); - - /// Get the configuration of this skeleton described in generalized - /// coordinates. The returned order of configuration is determined by _id. - /// - /// DEPRECATED: Use getPositions(const std::vector&) instead - DEPRECATED(4.5) - Eigen::VectorXd getPositionSegment(const std::vector& _indices) const; - - /// Set the generalized velocities of a segment of this Skeleton. The order of - /// input is determined by _id - /// - /// DEPRECATED: Use setVelocities(const std::vector&, - /// const Eigen::VectorXd&) instead - DEPRECATED(4.5) - void setVelocitySegment(const std::vector& _indices, - const Eigen::VectorXd& _velocities); - - /// Get the generalized velocities of a segment of this Skeleton. The returned - /// order of the velocities is determined by _id. - /// - /// DEPRECATED: use getVelocities(const std::vector&) instead - DEPRECATED(4.5) - Eigen::VectorXd getVelocitySegment(const std::vector& _id) const; - - /// Set the generalized accelerations of a segment of this Skeleton. The order - /// of input is determined by _id - /// - /// DEPRECATED: Use setAccelerations(const std::vector&, - /// const Eigen::VectorXd&) instead - DEPRECATED(4.5) - void setAccelerationSegment(const std::vector& _id, - const Eigen::VectorXd& _accelerations); - - /// Get the generalized accelerations of a segment of this Skeleton. The - /// returned order of the accelerations is determined by _id - /// - /// DEPRECATED: Use getAccelerations(const std::vector&) instead - DEPRECATED(4.5) - Eigen::VectorXd getAccelerationSegment(const std::vector& _indices) const; - - /// \} - - //---------------------------------------------------------------------------- - // Constraint impulse - //---------------------------------------------------------------------------- - - /// - DEPRECATED(4.2) - void setConstraintImpulses(const Eigen::VectorXd& _impulses); - - /// - DEPRECATED(4.2) - Eigen::VectorXd getConstraintImpulses() const; - //---------------------------------------------------------------------------- // Integration and finite difference //---------------------------------------------------------------------------- @@ -420,7 +344,8 @@ class Skeleton : public MetaSkeleton /// Compute inverse dynamics void computeInverseDynamics(bool _withExternalForces = false, - bool _withDampingForces = false); + bool _withDampingForces = false, + bool _withSpringForces = false); //---------------------------------------------------------------------------- // Impulse-based dynamics algorithms @@ -430,10 +355,6 @@ class Skeleton : public MetaSkeleton /// (b) generalized constraints on Joint void clearConstraintImpulses(); - /// Set constraint force vector. - DEPRECATED(4.2) - void setConstraintForceVector(const Eigen::VectorXd& _Fc); - /// Update bias impulses void updateBiasImpulse(BodyNode* _bodyNode); @@ -601,44 +522,24 @@ class Skeleton : public MetaSkeleton // Documentation inherited const Eigen::MatrixXd& getInvAugMassMatrix() const override; - /// Get Coriolis force vector of the skeleton. - /// \remarks Please use getCoriolisForces() instead. - DEPRECATED(4.2) - const Eigen::VectorXd& getCoriolisForceVector() const; - /// Get the Coriolis force vector of a tree in this Skeleton const Eigen::VectorXd& getCoriolisForces(size_t _treeIdx) const; // Documentation inherited const Eigen::VectorXd& getCoriolisForces() const override; - /// Get gravity force vector of the skeleton. - /// \remarks Please use getGravityForces() instead. - DEPRECATED(4.2) - const Eigen::VectorXd& getGravityForceVector() const; - /// Get the gravity forces for a tree in this Skeleton const Eigen::VectorXd& getGravityForces(size_t _treeIdx) const; // Documentation inherited const Eigen::VectorXd& getGravityForces() const override; - /// Get combined vector of Coriolis force and gravity force of the skeleton. - /// \remarks Please use getCoriolisAndGravityForces() instead. - DEPRECATED(4.2) - const Eigen::VectorXd& getCombinedVector() const; - /// Get the combined vector of Coriolis force and gravity force of a tree const Eigen::VectorXd& getCoriolisAndGravityForces(size_t _treeIdx) const; // Documentation inherited const Eigen::VectorXd& getCoriolisAndGravityForces() const override; - /// Get external force vector of the skeleton. - /// \remarks Please use getExternalForces() instead. - DEPRECATED(4.2) - const Eigen::VectorXd& getExternalForceVector() const; - /// Get the external force vector of a tree in the Skeleton const Eigen::VectorXd& getExternalForces(size_t _treeIdx) const; @@ -648,11 +549,6 @@ class Skeleton : public MetaSkeleton /// Get damping force of the skeleton. // const Eigen::VectorXd& getDampingForceVector(); - /// Get constraint force vector. - /// \remarks Please use getConstraintForces() instead. - DEPRECATED(4.2) - const Eigen::VectorXd& getConstraintForceVector(); - /// Get constraint force vector for a tree const Eigen::VectorXd& getConstraintForces(size_t _treeIdx) const; @@ -733,32 +629,6 @@ class Skeleton : public MetaSkeleton math::LinearJacobian getCOMLinearJacobianDeriv( const Frame* _inCoordinatesOf = Frame::World()) const override; - /// Get skeleton's COM w.r.t. world frame. - /// - /// Deprecated in 4.4. Please use getCOM() instead - DEPRECATED(4.4) - Eigen::Vector3d getWorldCOM(); - - /// Get skeleton's COM velocity w.r.t. world frame. - /// - /// Deprecated in 4.4. Please use getCOMLinearVelocity() instead - DEPRECATED(4.4) - Eigen::Vector3d getWorldCOMVelocity(); - - /// Get skeleton's COM acceleration w.r.t. world frame. - /// - /// Deprecated in 4.4. Please use getCOMAcceleration() instead - DEPRECATED(4.4) - Eigen::Vector3d getWorldCOMAcceleration(); - - /// Get skeleton's COM Jacobian w.r.t. world frame. - DEPRECATED(4.4) - Eigen::MatrixXd getWorldCOMJacobian(); - - /// Get skeleton's COM Jacobian time derivative w.r.t. world frame. - DEPRECATED(4.4) - Eigen::MatrixXd getWorldCOMJacobianTimeDeriv(); - /// \} //---------------------------------------------------------------------------- @@ -775,27 +645,6 @@ class Skeleton : public MetaSkeleton const Eigen::Vector4d& _color = Eigen::Vector4d::Ones(), bool _useDefaultColor = true) const; -public: - /// Compute recursion part A of forward dynamics - /// - /// Deprecated as of 4.4. Auto-updating makes this function irrelevant - DEPRECATED(4.4) - void computeForwardDynamicsRecursionPartA(); - - /// Compute recursion part B of forward dynamics - void computeForwardDynamicsRecursionPartB(); - - /// Compute recursion part A of inverse dynamics - /// - /// Deprecated as of 4.4. Auto-updating makes this function irrelevant - DEPRECATED(4.4) - void computeInverseDynamicsRecursionA(); - - /// Compute recursion part B of inverse dynamics - void computeInverseDynamicsRecursionB(bool _withExternalForces = false, - bool _withDampingForces = false, - bool _withSpringForces = false); - //---------------------------------------------------------------------------- // Friendship //---------------------------------------------------------------------------- @@ -917,44 +766,24 @@ class Skeleton : public MetaSkeleton /// Update inverse of augmented mass matrix of the skeleton. void updateInvAugMassMatrix() const; - /// Update Coriolis force vector of the skeleton. - /// \remarks Please use updateCoriolisForces() instead. - DEPRECATED(4.2) - virtual void updateCoriolisForceVector(); - /// Update Coriolis force vector for a tree in the Skeleton void updateCoriolisForces(size_t _treeIdx) const; /// Update Coriolis force vector of the skeleton. void updateCoriolisForces() const; - /// Update gravity force vector of the skeleton. - /// \remarks Please use updateGravityForces() instead. - DEPRECATED(4.2) - virtual void updateGravityForceVector(); - /// Update the gravity force vector of a tree void updateGravityForces(size_t _treeIdx) const; /// Update gravity force vector of the skeleton. void updateGravityForces() const; - /// Update combined vector of the skeleton. - /// \remarks Please use updateCoriolisAndGravityForces() instead. - DEPRECATED(4.2) - virtual void updateCombinedVector(); - /// Update the combined vector for a tree in this Skeleton void updateCoriolisAndGravityForces(size_t _treeIdx) const; /// Update combined vector of the skeleton. void updateCoriolisAndGravityForces() const; - /// update external force vector to generalized forces. - /// \remarks Please use updateExternalForces() instead. - DEPRECATED(4.2) - virtual void updateExternalForceVector(); - /// Update external force vector to generalized forces for a tree void updateExternalForces(size_t _treeIdx) const; diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index 0125efe3b2954..3e2689debd0cd 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -125,7 +125,7 @@ class SoftBodyNode : public BodyNode /// Remove any unwarranted SoftBodyShapes /// Note: This will be deprecated once VisualizationNodes and CollisionNodes - /// are implemented + /// are implemented. Please see #394. void removeSoftBodyShapes(); diff --git a/dart/lcpsolver/ODELCPSolver.h b/dart/lcpsolver/ODELCPSolver.h index 808dc1abe526b..b1a617d75084c 100644 --- a/dart/lcpsolver/ODELCPSolver.h +++ b/dart/lcpsolver/ODELCPSolver.h @@ -38,7 +38,6 @@ #define DART_LCPSOLVER_ODELCPSOLVER_H_ #include -#include "dart/common/Deprecated.h" namespace dart { namespace lcpsolver { @@ -82,9 +81,6 @@ class ODELCPSolver { const Eigen::VectorXd& _x); }; -DEPRECATED(4.3) -typedef ODELCPSolver LCPSolver; - } // namespace lcpsolver } // namespace dart diff --git a/dart/math/Helpers.h b/dart/math/Helpers.h index 1ef3ac739df00..c62a14eb61859 100644 --- a/dart/math/Helpers.h +++ b/dart/math/Helpers.h @@ -53,7 +53,6 @@ #include // Local Headers #include "dart/common/Console.h" -#include "dart/common/Deprecated.h" #include "dart/math/MathTypes.h" namespace dart { @@ -239,21 +238,6 @@ Eigen::Matrix randomVector(double _limit) return randomVector(-fabs(_limit), fabs(_limit)); } -DEPRECATED(4.3) -inline int castUIntToInt(size_t _x) -{ -// if (_x <= INT_MAX) -// return static_cast(_x); - -// if (_x >= INT_MIN) -// return static_cast(_x - INT_MIN) + INT_MIN; - -// dterr << "x is out of range." << std::endl; - -// throw _x; // Or whatever else you like - return static_cast(_x); -} - } // namespace math namespace Color diff --git a/dart/planning/PathPlanner.h b/dart/planning/PathPlanner.h index ec5d0f0c19943..b8d490a7ca8de 100644 --- a/dart/planning/PathPlanner.h +++ b/dart/planning/PathPlanner.h @@ -91,7 +91,7 @@ bool PathPlanner::planPath(dynamics::Skeleton* robot, const std::vector &start, const std::vector &goal, std::list &path) { - Eigen::VectorXd savedConfiguration = robot->getPositionSegment(dofs); + Eigen::VectorXd savedConfiguration = robot->getPositions(dofs); // ==================================================================== // Check for collisions in the start and goal configurations @@ -99,7 +99,7 @@ bool PathPlanner::planPath(dynamics::Skeleton* robot, const std::vector feasibleStart; for(unsigned int i = 0; i < start.size(); i++) { - robot->setPositionSegment(dofs, start[i]); + robot->setPositions(dofs, start[i]); if(!world->checkCollision()) feasibleStart.push_back(start[i]); } @@ -112,7 +112,7 @@ bool PathPlanner::planPath(dynamics::Skeleton* robot, const std::vector feasibleGoal; for(unsigned int i = 0; i < goal.size(); i++) { - robot->setPositionSegment(dofs, goal[i]); + robot->setPositions(dofs, goal[i]); if(!world->checkCollision()) feasibleGoal.push_back(goal[i]); } @@ -135,7 +135,7 @@ bool PathPlanner::planPath(dynamics::Skeleton* robot, const std::vectorsetPositionSegment(dofs, savedConfiguration); + robot->setPositions(dofs, savedConfiguration); return result; } diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 764e17f5e1310..0cad3034b27f9 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -159,7 +159,7 @@ void World::step(bool _resetCommand) if (!skel->isMobile()) continue; - skel->computeForwardDynamicsRecursionPartB(); + skel->computeForwardDynamics(); skel->integrateVelocities(mTimeStep); } diff --git a/dart/simulation/World.h b/dart/simulation/World.h index 91c16721936cb..abfa09958044e 100644 --- a/dart/simulation/World.h +++ b/dart/simulation/World.h @@ -49,7 +49,6 @@ #include -#include "dart/common/Deprecated.h" #include "dart/common/Timer.h" #include "dart/common/NameManager.h" #include "dart/common/Subject.h" diff --git a/dart/utils/Paths.h b/dart/utils/Paths.h deleted file mode 100644 index 97058a9aa0925..0000000000000 --- a/dart/utils/Paths.h +++ /dev/null @@ -1,3 +0,0 @@ -// This file is deprecated by DART 4.3 and will be removed in DART 5.0 - -#include "dart/config.h" diff --git a/dart/utils/Paths.h.in b/dart/utils/Paths.h.in deleted file mode 100644 index 9d801d46fdad9..0000000000000 --- a/dart/utils/Paths.h.in +++ /dev/null @@ -1,8 +0,0 @@ -// This file is automatically generated from CMake -#ifndef DART_COMMON_PATHS_H -#define DART_COMMON_PATHS_H - -#define DART_ROOT_PATH "@CMAKE_SOURCE_DIR@/" -#define DART_DATA_PATH "@CMAKE_SOURCE_DIR@/data/" - -#endif // #ifndef DART_COMMON_PATHS_H diff --git a/dart/utils/SkelParser.h b/dart/utils/SkelParser.h index e99cf6fb8fa45..efa509f4be299 100644 --- a/dart/utils/SkelParser.h +++ b/dart/utils/SkelParser.h @@ -45,7 +45,6 @@ // http://www.grinninglizard.com/tinyxml2/index.html #include -#include "dart/common/Deprecated.h" #include "dart/utils/Parser.h" #include "dart/dynamics/Skeleton.h" #include "dart/dynamics/BodyNode.h" diff --git a/unittests/testOptimizer.cpp b/unittests/testOptimizer.cpp index 0f098a90cc9b7..514ee71d186ab 100644 --- a/unittests/testOptimizer.cpp +++ b/unittests/testOptimizer.cpp @@ -140,7 +140,7 @@ TEST(Optimizer, BasicNlopt) Eigen::VectorXd optX = prob.getOptimalSolution(); EXPECT_NEAR(minF, 0.544330847, 1e-6); - EXPECT_EQ(optX.size(), prob.getDimension()); + EXPECT_EQ(static_cast(optX.size()), prob.getDimension()); EXPECT_NEAR(optX[0], 0.333334, 1e-6); EXPECT_NEAR(optX[1], 0.296296, 1e-6); } @@ -173,7 +173,7 @@ TEST(Optimizer, BasicIpopt) Eigen::VectorXd optX = prob.getOptimalSolution(); EXPECT_NEAR(minF, 0.544330847, 1e-6); - EXPECT_EQ(optX.size(), prob.getDimension()); + EXPECT_EQ(static_cast(optX.size()), prob.getDimension()); EXPECT_NEAR(optX[0], 0.333334, 1e-6); EXPECT_NEAR(optX[1], 0.296296, 1e-6); } From 0ea3304c71e926c1a5d58dc044bf1da923fa2eb8 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 11 Jun 2015 11:41:31 -0400 Subject: [PATCH 05/19] Don't include Paths.h which is removed --- dart/utils/SkelParser.cpp | 1 - dart/utils/sdf/SdfParser.cpp | 1 - unittests/testConstraint.cpp | 1 - unittests/testDynamics.cpp | 1 - unittests/testFrames.cpp | 2 +- unittests/testJoints.cpp | 1 - unittests/testParser.cpp | 1 - unittests/testSoftDynamics.cpp | 1 - 8 files changed, 1 insertion(+), 8 deletions(-) diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 9b97ab6bc16fe..5f87bb8203cd2 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -72,7 +72,6 @@ #include "dart/dynamics/Marker.h" #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" -#include "dart/utils/Paths.h" namespace dart { namespace utils { diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 6801ec86f5c2f..a5456edbaa90b 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -20,7 +20,6 @@ #include "dart/dynamics/Skeleton.h" #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" -#include "dart/utils/Paths.h" #include "dart/utils/sdf/SdfParser.h" namespace dart { diff --git a/unittests/testConstraint.cpp b/unittests/testConstraint.cpp index f0723ff28bfc3..22a83282b4671 100644 --- a/unittests/testConstraint.cpp +++ b/unittests/testConstraint.cpp @@ -50,7 +50,6 @@ #include "dart/dynamics/Skeleton.h" #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" -#include "dart/utils/Paths.h" //============================================================================== class ConstraintTest : public ::testing::Test diff --git a/unittests/testDynamics.cpp b/unittests/testDynamics.cpp index 40b068280f950..3fdf5e1cc943c 100644 --- a/unittests/testDynamics.cpp +++ b/unittests/testDynamics.cpp @@ -50,7 +50,6 @@ #include "dart/dynamics/SimpleFrame.h" #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" -#include "dart/utils/Paths.h" using namespace Eigen; using namespace dart; diff --git a/unittests/testFrames.cpp b/unittests/testFrames.cpp index a47e10ca2b362..2d6a95bac5134 100644 --- a/unittests/testFrames.cpp +++ b/unittests/testFrames.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2014, Georgia Tech Research Corporation + * Copyright (c) 2014-2015, Georgia Tech Research Corporation * All rights reserved. * * Author(s): Michael X. Grey diff --git a/unittests/testJoints.cpp b/unittests/testJoints.cpp index 26f25fa1385ad..b5fbab6caa7bb 100644 --- a/unittests/testJoints.cpp +++ b/unittests/testJoints.cpp @@ -54,7 +54,6 @@ #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/Skeleton.h" #include "dart/simulation/World.h" -#include "dart/utils/Paths.h" #include "dart/utils/SkelParser.h" using namespace dart; diff --git a/unittests/testParser.cpp b/unittests/testParser.cpp index 00fd6dcbcda8e..405dad2f7ce79 100644 --- a/unittests/testParser.cpp +++ b/unittests/testParser.cpp @@ -42,7 +42,6 @@ #include "dart/dynamics/RevoluteJoint.h" #include "dart/dynamics/PlanarJoint.h" #include "dart/dynamics/Skeleton.h" -#include "dart/utils/Paths.h" #include "dart/simulation/World.h" #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" diff --git a/unittests/testSoftDynamics.cpp b/unittests/testSoftDynamics.cpp index d50f824a8931c..226a81cab06e7 100644 --- a/unittests/testSoftDynamics.cpp +++ b/unittests/testSoftDynamics.cpp @@ -49,7 +49,6 @@ #include "dart/dynamics/SoftBodyNode.h" #include "dart/dynamics/PointMass.h" #include "dart/simulation/World.h" -#include "dart/utils/Paths.h" #include "dart/utils/SkelParser.h" using namespace std; From 6a3b88994b8eca1f59ceebed816a3ef285ba9eee Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 11 Jun 2015 12:56:16 -0400 Subject: [PATCH 06/19] Move templated and non-exposing implementations a detail deirectory --- dart/common/CMakeLists.txt | 13 +- dart/common/Console.cpp | 2 +- dart/common/Console.h | 1 - dart/common/NameManager.h | 217 ++---------------- dart/common/Signal.cpp | 144 ++++++++++++ dart/common/Signal.h | 295 ++++-------------------- dart/common/detail/ConnectionBody.cpp | 76 +++++++ dart/common/detail/ConnectionBody.h | 146 ++++++++++++ dart/common/detail/NameManager.h | 312 ++++++++++++++++++++++++++ dart/common/detail/Signal.h | 279 +++++++++++++++++++++++ dart/common/detail/sub_ptr.h | 126 +++++++++++ dart/common/sub_ptr.h | 44 ++-- 12 files changed, 1174 insertions(+), 481 deletions(-) create mode 100644 dart/common/Signal.cpp create mode 100644 dart/common/detail/ConnectionBody.cpp create mode 100644 dart/common/detail/ConnectionBody.h create mode 100644 dart/common/detail/NameManager.h create mode 100644 dart/common/detail/Signal.h create mode 100644 dart/common/detail/sub_ptr.h diff --git a/dart/common/CMakeLists.txt b/dart/common/CMakeLists.txt index 144e0309b03f8..616e3ee7b9c42 100644 --- a/dart/common/CMakeLists.txt +++ b/dart/common/CMakeLists.txt @@ -1,9 +1,11 @@ # Search all header and source files file(GLOB srcs "*.cpp") file(GLOB hdrs "*.h") +file(GLOB detail_srcs "detail/*.cpp") +file(GLOB detail_hdrs "detail/*.h") -set(dart_common_hdrs ${hdrs} PARENT_SCOPE) -set(dart_common_srcs ${srcs} PARENT_SCOPE) +set(dart_common_hdrs "${hdrs};${detail_hdrs}" PARENT_SCOPE) +set(dart_common_srcs "${srcs};${detail_srcs}" PARENT_SCOPE) # Library #dart_add_library(dart_common ${srcs} ${hdrs}) @@ -28,6 +30,13 @@ install( DESTINATION include/dart/common COMPONENT headers ) + +install( + FILES ${detail_hdrs} + DESTINATION include/dart/common/detail + COMPONENT headers +) + #install(TARGETS dart_common EXPORT DARTCoreTargets DESTINATION lib) #install(TARGETS dart_common EXPORT DARTTargets DESTINATION lib) diff --git a/dart/common/Console.cpp b/dart/common/Console.cpp index 84bbabbf6c41f..21d46b622e666 100644 --- a/dart/common/Console.cpp +++ b/dart/common/Console.cpp @@ -37,7 +37,7 @@ #include "dart/common/Console.h" -#include +#include namespace dart { namespace common { diff --git a/dart/common/Console.h b/dart/common/Console.h index 42ff7881f2821..ce67953838169 100644 --- a/dart/common/Console.h +++ b/dart/common/Console.h @@ -39,7 +39,6 @@ #define DART_COMMON_CONSOLE_H_ #include -#include /// \brief Output a message #define dtmsg (dart::common::colorMsg("Msg", 32)) diff --git a/dart/common/NameManager.h b/dart/common/NameManager.h index 6bdcf6156293b..3f7750bb240e1 100644 --- a/dart/common/NameManager.h +++ b/dart/common/NameManager.h @@ -39,9 +39,6 @@ #define DART_COMMON_NAMEMANAGER_H_ #include -#include -#include -#include "dart/common/Console.h" namespace dart { namespace common { @@ -70,14 +67,10 @@ class NameManager public: /// Constructor NameManager(const std::string& _managerName = "default", - const std::string& _defaultName = "default") - : mManagerName(_managerName), - mDefaultName(_defaultName), - mNameBeforeNumber(true), - mPrefix(""), mInfix("("), mAffix(")") {} + const std::string& _defaultName = "default"); /// Destructor - virtual ~NameManager() {} + virtual ~NameManager(); /// Set a new pattern for name generation. /// @@ -90,192 +83,49 @@ class NameManager /// /// returns false if the pattern was invalid (i.e. did not contain b /// oth %s and %d) - bool setPattern(const std::string& _newPattern) - { - size_t name_start = _newPattern.find("%s"); - size_t number_start = _newPattern.find("%d"); - - if(name_start == std::string::npos || number_start == std::string::npos) - return false; - - if(name_start < number_start) - mNameBeforeNumber = true; - else - mNameBeforeNumber = false; - - size_t prefix_end = std::min(name_start, number_start); - size_t infix_end = std::max(name_start, number_start); - - mPrefix = _newPattern.substr(0, prefix_end); - mInfix = _newPattern.substr(prefix_end+2, infix_end-prefix_end-2); - mAffix = _newPattern.substr(infix_end+2); - - return true; - } + bool setPattern(const std::string& _newPattern); /// Issue new unique combined name of given base name and number suffix - std::string issueNewName(const std::string& _name) const - { - if(!hasName(_name)) - return _name; - - int count = 1; - std::string newName; - do - { - std::stringstream ss; - if(mNameBeforeNumber) - ss << mPrefix << _name << mInfix << count++ << mAffix; - else - ss << mPrefix << count++ << mInfix << _name << mAffix; - newName = ss.str(); - } while (hasName(newName)); - - dtmsg << "[NameManager::issueNewName] (" << mManagerName << ") The name [" - << _name << "] is a duplicate, so it has been renamed to [" - << newName << "]\n"; - - return newName; - } + std::string issueNewName(const std::string& _name) const; /// Call issueNewName() and add the result to the map - std::string issueNewNameAndAdd(const std::string& _name, const T& _obj) - { - const std::string& checkEmpty = _name.empty()? mDefaultName : _name; - const std::string& newName = issueNewName(checkEmpty); - addName(newName, _obj); - - return newName; - } + std::string issueNewNameAndAdd(const std::string& _name, const T& _obj); /// Add an object to the map - bool addName(const std::string& _name, const T& _obj) - { - if (_name.empty()) - { - dtwarn << "[NameManager::addName] (" << mManagerName - << ") Empty name is not allowed!\n"; - return false; - } - - if (hasName(_name)) - { - dtwarn << "[NameManager::addName] (" << mManagerName << ") The name [" - << _name << "] already exists!\n"; - return false; - } - - mMap.insert(std::pair(_name, _obj)); - mReverseMap.insert(std::pair(_obj, _name)); - - assert(mReverseMap.size() == mMap.size()); - - return true; - } + bool addName(const std::string& _name, const T& _obj); /// Remove an object from the Manager based on its name - bool removeName(const std::string& _name) - { - assert(mReverseMap.size() == mMap.size()); - - typename std::map::iterator it = mMap.find(_name); - - if (it == mMap.end()) - return false; - - typename std::map::iterator rit = - mReverseMap.find(it->second); - - if (rit != mReverseMap.end()) - mReverseMap.erase(rit); - - mMap.erase(it); - - return true; - } + bool removeName(const std::string& _name); /// Remove an object from the Manager based on reverse lookup - bool removeObject(const T& _obj) - { - assert(mReverseMap.size() == mMap.size()); - - typename std::map::iterator rit = mReverseMap.find(_obj); - - if (rit == mReverseMap.end()) - return false; - - typename std::map::iterator it = mMap.find(rit->second); - if (it != mMap.end()) - mMap.erase(it); - - mReverseMap.erase(rit); - - return true; - } + bool removeObject(const T& _obj); /// Remove _name using the forward lookup and _obj using the reverse lookup. /// This will allow you to add _obj under the name _name without any conflicts - void removeEntries(const std::string& _name, const T& _obj) - { - removeObject(_obj); - removeName(_name, false); - } + void removeEntries(const std::string& _name, const T& _obj); /// Clear all the objects - void clear() - { - mMap.clear(); - mReverseMap.clear(); - } + void clear(); /// Return true if the name is contained - bool hasName(const std::string& _name) const - { - return (mMap.find(_name) != mMap.end()); - } + bool hasName(const std::string& _name) const; /// Return true if the object is contained - bool hasObject(const T& _obj) const - { - return (mReverseMap.find(_obj) != mReverseMap.end()); - } + bool hasObject(const T& _obj) const; /// Get the number of the objects currently stored by the NameManager - size_t getCount() const - { - return mMap.size(); - } + size_t getCount() const; /// Get object by given name /// \param[in] _name /// Name of the requested object /// \return /// The object if it exists, or nullptr if it does not exist - T getObject(const std::string& _name) const - { - typename std::map::const_iterator result = - mMap.find(_name); - - if (result != mMap.end()) - return result->second; - else - return nullptr; - } + T getObject(const std::string& _name) const; /// Use a reverse lookup to get the name that the manager has _obj listed /// under. Returns an empty string if it is not in the list. - std::string getName(const T& _obj) const - { - assert(mReverseMap.size() == mMap.size()); - - typename std::map::const_iterator result = - mReverseMap.find(_obj); - - if (result != mReverseMap.end()) - return result->second; - else - return ""; - } + std::string getName(const T& _obj) const; /// Change the name of a currently held object. This will do nothing if the /// object is already using _newName or if the object is not held by this @@ -284,46 +134,21 @@ class NameManager /// If the object is held, its new name is returned (which might /// be different than _newName if there was a duplicate naming conflict). If /// the object is not held, an empty string will be returned. - std::string changeObjectName(const T& _obj, const std::string& _newName) - { - assert(mReverseMap.size() == mMap.size()); - - typename std::map::iterator rit = mReverseMap.find(_obj); - if(rit == mReverseMap.end()) - return _newName; - - if(rit->second == _newName) - return rit->second; - - removeName(rit->second); - return issueNewNameAndAdd(_newName, _obj); - } + std::string changeObjectName(const T& _obj, const std::string& _newName); /// Set the name that will be provided to objects passed in with an empty /// string for a name - void setDefaultName(const std::string& _defaultName) - { - mDefaultName = _defaultName; - } + void setDefaultName(const std::string& _defaultName); /// Get the name that will be provided to objects passed in with an empty /// string for a name - const std::string& getDefaultName() const - { - return mDefaultName; - } + const std::string& getDefaultName() const; /// Set the name of this NameManager so that it can be printed in error reports - void setManagerName(const std::string& _managerName) - { - mManagerName = _managerName; - } + void setManagerName(const std::string& _managerName); /// Get the name of this NameManager - const std::string& getManagerName() const - { - return mManagerName; - } + const std::string& getManagerName() const; protected: /// Name of this NameManager. This is used to report errors. @@ -356,4 +181,6 @@ class NameManager } // namespace common } // namespace dart +#include "dart/common/detail/NameManager.h" + #endif // DART_COMMON_NAMEMANAGER_H_ diff --git a/dart/common/Signal.cpp b/dart/common/Signal.cpp new file mode 100644 index 0000000000000..3931ce565696e --- /dev/null +++ b/dart/common/Signal.cpp @@ -0,0 +1,144 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/common/Signal.h" + +namespace dart { +namespace common { + +//============================================================================== +Connection::Connection() +{ + // Do nothing +} + +//============================================================================== +Connection::Connection(const Connection& _other) + : mWeakConnectionBody(_other.mWeakConnectionBody) +{ + // Do nothing +} + +//============================================================================== +Connection::Connection(Connection&& _other) + : mWeakConnectionBody(std::move(_other.mWeakConnectionBody)) +{ + // Do nothing +} + +//============================================================================== +Connection& Connection::operator=(const Connection& _other) +{ + mWeakConnectionBody = _other.mWeakConnectionBody; + return *this; +} + +//============================================================================== +Connection& Connection::operator=(Connection&& _other) +{ + mWeakConnectionBody = std::move(_other.mWeakConnectionBody); + return *this; +} + +//============================================================================== +Connection::Connection( + const std::weak_ptr& _connectionBody) + : mWeakConnectionBody(_connectionBody) +{ + // Do nothing +} + +//============================================================================== +Connection::Connection( + std::weak_ptr&& _connectionBody) + : mWeakConnectionBody(std::move(_connectionBody)) +{ + // Do nothing +} + +//============================================================================== +Connection::~Connection() +{ + // Do nothing +} + +//============================================================================== +bool Connection::isConnected() const +{ + std::shared_ptr + connectionBody(mWeakConnectionBody.lock()); + + if (nullptr == connectionBody) + return false; + + return connectionBody->isConnected(); +} + +//============================================================================== +void Connection::disconnect() const +{ + std::shared_ptr + connectionBody(mWeakConnectionBody.lock()); + + if (nullptr == connectionBody) + return; + + connectionBody->disconnect(); +} + +//============================================================================== +ScopedConnection::ScopedConnection(const Connection& _other) + : Connection(_other) +{ + // Do nothing +} + +//============================================================================== +ScopedConnection::ScopedConnection(Connection&& _other) + : Connection(std::move(_other)) +{ + // Do nothing +} + +//============================================================================== +ScopedConnection::~ScopedConnection() +{ + disconnect(); +} + +} // namespace common +} // namespace dart + diff --git a/dart/common/Signal.h b/dart/common/Signal.h index 4178040e4b4a9..05c4b82d08729 100644 --- a/dart/common/Signal.h +++ b/dart/common/Signal.h @@ -37,149 +37,51 @@ #ifndef DART_COMMON_SIGNAL_H_ #define DART_COMMON_SIGNAL_H_ -#include #include #include #include -#include -#include + +#include "dart/common/detail/ConnectionBody.h" namespace dart { namespace common { -namespace signal { -namespace detail { - -/// class ConnectionBodyBase -class ConnectionBodyBase -{ -public: - /// Constructor - ConnectionBodyBase() : mIsConnected(true) {} - - /// Destructor - virtual ~ConnectionBodyBase() {} - - /// Disconnect - void disconnect() { mIsConnected = false; } - - /// Get true if this connection body is connected to the signal - bool isConnected() const { return mIsConnected; } - -protected: - /// Connection flag - bool mIsConnected; -}; - -/// class ConnectionBody -template -class ConnectionBody : public ConnectionBodyBase -{ -public: - /// Constructor given slot - ConnectionBody(const SlotType& _slot) : ConnectionBodyBase(), mSlot(_slot) {} - - /// Move constructor given slot - ConnectionBody(SlotType&& _slot) - : ConnectionBodyBase(), mSlot(std::forward(_slot)) {} - - /// Destructor - virtual ~ConnectionBody() {} - - /// Get slot - const SlotType& getSlot() { return mSlot; } - -private: - /// Slot - SlotType mSlot; -}; - -/// DefaultCombiner -- return the last result -template -struct DefaultCombiner -{ - typedef T result_type; - - template - static T process(InputIterator first, InputIterator last) - { - // If there are no slots to call, just return the - // default-constructed value - if (first == last) - return T(); - - return *(--last); - } -}; - -} // namespace detail -} // namespace signal - /// class Connection class Connection { public: /// Default constructor - Connection() {} + Connection(); /// Copy constructor - Connection(const Connection& _other) - : mWeakConnectionBody(_other.mWeakConnectionBody) {} + Connection(const Connection& _other); /// Move constructor - Connection(Connection&& _other) - : mWeakConnectionBody(std::move(_other.mWeakConnectionBody)) {} + Connection(Connection&& _other); /// Assignment operator - Connection& operator=(const Connection& _other) - { - mWeakConnectionBody = _other.mWeakConnectionBody; - return *this; - } + Connection& operator=(const Connection& _other); /// Move assignment operator - Connection& operator=(Connection&& _other) - { - mWeakConnectionBody = std::move(_other.mWeakConnectionBody); - return *this; - } - - /// Constructor given connection body - Connection( - const std::weak_ptr& _connectionBody) - : mWeakConnectionBody(_connectionBody) {} - - /// Move constructor given connection body - Connection( - std::weak_ptr&& _connectionBody) - : mWeakConnectionBody(std::move(_connectionBody)) {} + Connection& operator=(Connection&& _other); /// Destructor - virtual ~Connection() {} + virtual ~Connection(); /// Get true if the slot is connected - bool isConnected() const - { - std::shared_ptr - connectionBody(mWeakConnectionBody.lock()); - - if (nullptr == connectionBody) - return false; - - return connectionBody->isConnected(); - } + bool isConnected() const; /// Disconnect the connection - void disconnect() const - { - std::shared_ptr - connectionBody(mWeakConnectionBody.lock()); + void disconnect() const; - if (nullptr == connectionBody) - return; +protected: + /// Constructor given connection body + Connection( + const std::weak_ptr& _connectionBody); - connectionBody->disconnect(); - } + /// Move constructor given connection body + Connection( + std::weak_ptr&& _connectionBody); private: /// Weak pointer to connection body in the signal @@ -191,13 +93,13 @@ class ScopedConnection : public Connection { public: /// Default constructor - ScopedConnection(const Connection& _other) : Connection(_other) {} + ScopedConnection(const Connection& _other); /// Move constructor - ScopedConnection(Connection&& _other) : Connection(std::move(_other)) {} + ScopedConnection(Connection&& _other); /// Destructor - virtual ~ScopedConnection() { disconnect(); } + virtual ~ScopedConnection(); }; template std::owner_less>>; /// Constructor - Signal() {} + Signal(); /// Destructor - virtual ~Signal() { disconnectAll(); } + virtual ~Signal(); /// Connect a slot to the signal - Connection connect(const SlotType& _slot) - { - auto newConnectionBody = std::make_shared(_slot); - mConnectionBodies.insert(newConnectionBody); - - return Connection(std::move(newConnectionBody)); - } + Connection connect(const SlotType& _slot); /// Connect a slot to the signal - Connection connect(SlotType&& _slot) - { - auto newConnectionBody - = std::make_shared(std::forward(_slot)); - mConnectionBodies.insert(newConnectionBody); - - return Connection(std::move(newConnectionBody)); - } + Connection connect(SlotType&& _slot); /// Disconnect given connection - void disconnect(const Connection& _connection) const - { - _connection.disconnect(); - } + void disconnect(const Connection& _connection) const; /// Disconnect all the connections - void disconnectAll() { mConnectionBodies.clear(); } + void disconnectAll(); /// Cleanup all the disconnected connections - void clenaupConnections() - { - // Counts all the connected conection bodies - for (const auto& connectionBody : mConnectionBodies) - { - if (!connectionBody->isConnected()) - mConnectionBodies.erase(connectionBody); - } - } + void clenaupConnections(); /// Get the number of connections - size_t getNumConnections() const - { - size_t numConnections = 0; - - // Counts all the connected conection bodies - for (const auto& connectionBody : mConnectionBodies) - { - if (connectionBody->isConnected()) - ++numConnections; - } - - return numConnections; - } + size_t getNumConnections() const; /// Raise the signal template - ResultType raise(ArgTypes&&... _args) - { - std::vector res(mConnectionBodies.size()); - auto resIt = res.begin(); - - for (auto itr = mConnectionBodies.begin(); itr != mConnectionBodies.end(); ) - { - if ((*itr)->isConnected()) - { - *(resIt++) = (*itr)->getSlot()(std::forward(_args)...); - ++itr; - } - else - { - mConnectionBodies.erase(itr++); - } - } - - return Combiner::process(res.begin(), resIt); - } + ResultType raise(ArgTypes&&... _args); /// Raise the signal template - ResultType operator()(ArgTypes&&... _args) - { - return raise(std::forward(_args)...); - } + ResultType operator()(ArgTypes&&... _args); private: /// Connection set @@ -327,89 +171,36 @@ class Signal std::owner_less>>; /// Constructor - Signal() {} + Signal(); /// Destructor - virtual ~Signal() { disconnectAll(); } + virtual ~Signal(); /// Connect a slot to the signal - Connection connect(const SlotType& _slot) - { - auto newConnectionBody = std::make_shared(_slot); - mConnectionBodies.insert(newConnectionBody); - - return Connection(std::move(newConnectionBody)); - } + Connection connect(const SlotType& _slot); /// Connect a slot to the signal - Connection connect(SlotType&& _slot) - { - auto newConnectionBody - = std::make_shared(std::forward(_slot)); - mConnectionBodies.insert(newConnectionBody); - - return Connection(std::move(newConnectionBody)); - } + Connection connect(SlotType&& _slot); /// Disconnect given connection - void disconnect(const Connection& _connection) const - { - _connection.disconnect(); - } + void disconnect(const Connection& _connection) const; /// Disconnect all the connections - void disconnectAll() { mConnectionBodies.clear(); } + void disconnectAll(); /// Cleanup all the disconnected connections - void clenaupConnections() - { - // Counts all the connected conection bodies - for (const auto& connectionBody : mConnectionBodies) - { - if (!connectionBody->isConnected()) - mConnectionBodies.erase(connectionBody); - } - } + void clenaupConnections(); /// Get the number of connections - size_t getNumConnections() const - { - size_t numConnections = 0; - - // Counts all the connected conection bodies - for (const auto& connectionBody : mConnectionBodies) - { - if (connectionBody->isConnected()) - ++numConnections; - } - - return numConnections; - } + size_t getNumConnections() const; /// Raise the signal template - void raise(ArgTypes&&... _args) - { - for (auto itr = mConnectionBodies.begin(); itr != mConnectionBodies.end(); ) - { - if ((*itr)->isConnected()) - { - (*itr)->getSlot()(std::forward(_args)...); - ++itr; - } - else - { - mConnectionBodies.erase(itr++); - } - } - } + void raise(ArgTypes&&... _args); /// Raise the signal template - void operator()(ArgTypes&&... _args) - { - raise(std::forward(_args)...); - } + void operator()(ArgTypes&&... _args); private: /// Connection set @@ -427,10 +218,10 @@ class SlotRegister using SignalType = typename T::SignalType; /// Constructor given signal - SlotRegister(typename T::SignalType& _signal) : mSignal(_signal) {} + SlotRegister(typename T::SignalType& _signal); /// Connect a slot to the signal - Connection connect(const SlotType& _slot) { return mSignal.connect(_slot); } + Connection connect(const SlotType& _slot); private: /// Signal @@ -440,5 +231,7 @@ class SlotRegister } // namespace common } // namespace dart +#include "dart/common/detail/Signal.h" + #endif // DART_COMMON_SIGNAL_H_ diff --git a/dart/common/detail/ConnectionBody.cpp b/dart/common/detail/ConnectionBody.cpp new file mode 100644 index 0000000000000..4d57f79012051 --- /dev/null +++ b/dart/common/detail/ConnectionBody.cpp @@ -0,0 +1,76 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/common/detail/ConnectionBody.h" + +namespace dart { +namespace common { + +namespace signal { +namespace detail { + +//============================================================================== +ConnectionBodyBase::ConnectionBodyBase() + : mIsConnected(true) +{ + // Do nothing +} + +//============================================================================== +ConnectionBodyBase::~ConnectionBodyBase() +{ + // Do nothing +} + +//============================================================================== +void ConnectionBodyBase::disconnect() +{ + mIsConnected = false; +} + +//============================================================================== +bool ConnectionBodyBase::isConnected() const +{ + return mIsConnected; +} + +} // namespace detail +} // namespace signal + +} // namespace common +} // namespace dart + + diff --git a/dart/common/detail/ConnectionBody.h b/dart/common/detail/ConnectionBody.h new file mode 100644 index 0000000000000..5ee8b27ab6431 --- /dev/null +++ b/dart/common/detail/ConnectionBody.h @@ -0,0 +1,146 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_DETAIL_CONNECTIONBODY_H_ +#define DART_COMMON_DETAIL_CONNECTIONBODY_H_ + +#include + +namespace dart { +namespace common { + +namespace signal { +namespace detail { + +/// class ConnectionBodyBase +class ConnectionBodyBase +{ +public: + /// Constructor + ConnectionBodyBase(); + + /// Destructor + virtual ~ConnectionBodyBase(); + + /// Disconnect + void disconnect(); + + /// Get true if this connection body is connected to the signal + bool isConnected() const; + +protected: + /// Connection flag + bool mIsConnected; +}; + +/// class ConnectionBody +template +class ConnectionBody : public ConnectionBodyBase +{ +public: + /// Constructor given slot + ConnectionBody(const SlotType& _slot); + + /// Move constructor given slot + ConnectionBody(SlotType&& _slot); + + /// Destructor + virtual ~ConnectionBody(); + + /// Get slot + const SlotType& getSlot(); + +private: + /// Slot + SlotType mSlot; +}; + +//============================================================================== +template +ConnectionBody::ConnectionBody(const SlotType& _slot) + : ConnectionBodyBase(), mSlot(_slot) +{ + // Do nothing +} + +//============================================================================== +template +ConnectionBody::ConnectionBody(SlotType&& _slot) + : ConnectionBodyBase(), mSlot(std::forward(_slot)) +{ + // Do nothing +} + +//============================================================================== +template +ConnectionBody::~ConnectionBody() +{ + // Do nothing +} + +//============================================================================== +template +const SlotType& ConnectionBody::getSlot() +{ + return mSlot; +} + +/// DefaultCombiner -- return the last result +template +struct DefaultCombiner +{ + typedef T result_type; + + template + static T process(InputIterator first, InputIterator last) + { + // If there are no slots to call, just return the + // default-constructed value + if (first == last) + return T(); + + return *(--last); + } +}; + +} // namespace detail +} // namespace signal + +} // namespace common +} // namespace dart + +#endif // DART_COMMON_DETAIL_CONNECTIONBODY_H_ + diff --git a/dart/common/detail/NameManager.h b/dart/common/detail/NameManager.h new file mode 100644 index 0000000000000..164b274bdd4be --- /dev/null +++ b/dart/common/detail/NameManager.h @@ -0,0 +1,312 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee , + * Michael Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_DETAIL_NAMEMANAGER_H_ +#define DART_COMMON_DETAIL_NAMEMANAGER_H_ + +#include +#include +#include "dart/common/Console.h" + +namespace dart { +namespace common { + +//============================================================================== +template +NameManager::NameManager(const std::string& _managerName, + const std::string& _defaultName) + : mManagerName(_managerName), + mDefaultName(_defaultName), + mNameBeforeNumber(true), + mPrefix(""), mInfix("("), mAffix(")") +{ + // Do nothing +} + +//============================================================================== +template +NameManager::~NameManager() +{ + // Do nothing +} + +//============================================================================== +template +bool NameManager::setPattern(const std::string& _newPattern) +{ + size_t name_start = _newPattern.find("%s"); + size_t number_start = _newPattern.find("%d"); + + if(name_start == std::string::npos || number_start == std::string::npos) + return false; + + if(name_start < number_start) + mNameBeforeNumber = true; + else + mNameBeforeNumber = false; + + size_t prefix_end = std::min(name_start, number_start); + size_t infix_end = std::max(name_start, number_start); + + mPrefix = _newPattern.substr(0, prefix_end); + mInfix = _newPattern.substr(prefix_end+2, infix_end-prefix_end-2); + mAffix = _newPattern.substr(infix_end+2); + + return true; +} + +//============================================================================== +template +std::string NameManager::issueNewName(const std::string& _name) const +{ + if(!hasName(_name)) + return _name; + + int count = 1; + std::string newName; + do + { + std::stringstream ss; + if(mNameBeforeNumber) + ss << mPrefix << _name << mInfix << count++ << mAffix; + else + ss << mPrefix << count++ << mInfix << _name << mAffix; + newName = ss.str(); + } while (hasName(newName)); + + dtmsg << "[NameManager::issueNewName] (" << mManagerName << ") The name [" + << _name << "] is a duplicate, so it has been renamed to [" + << newName << "]\n"; + + return newName; +} + +//============================================================================== +template +std::string NameManager::issueNewNameAndAdd(const std::string& _name, + const T& _obj) +{ + const std::string& checkEmpty = _name.empty()? mDefaultName : _name; + const std::string& newName = issueNewName(checkEmpty); + addName(newName, _obj); + + return newName; +} + +//============================================================================== +template +bool NameManager::addName(const std::string& _name, const T& _obj) +{ + if (_name.empty()) + { + dtwarn << "[NameManager::addName] (" << mManagerName + << ") Empty name is not allowed!\n"; + return false; + } + + if (hasName(_name)) + { + dtwarn << "[NameManager::addName] (" << mManagerName << ") The name [" + << _name << "] already exists!\n"; + return false; + } + + mMap.insert(std::pair(_name, _obj)); + mReverseMap.insert(std::pair(_obj, _name)); + + assert(mReverseMap.size() == mMap.size()); + + return true; +} + +//============================================================================== +template +bool NameManager::removeName(const std::string& _name) +{ + assert(mReverseMap.size() == mMap.size()); + + typename std::map::iterator it = mMap.find(_name); + + if (it == mMap.end()) + return false; + + typename std::map::iterator rit = + mReverseMap.find(it->second); + + if (rit != mReverseMap.end()) + mReverseMap.erase(rit); + + mMap.erase(it); + + return true; +} + +//============================================================================== +template +bool NameManager::removeObject(const T& _obj) +{ + assert(mReverseMap.size() == mMap.size()); + + typename std::map::iterator rit = mReverseMap.find(_obj); + + if (rit == mReverseMap.end()) + return false; + + typename std::map::iterator it = mMap.find(rit->second); + if (it != mMap.end()) + mMap.erase(it); + + mReverseMap.erase(rit); + + return true; +} + +//============================================================================== +template +void NameManager::removeEntries(const std::string& _name, const T& _obj) +{ + removeObject(_obj); + removeName(_name, false); +} + +//============================================================================== +template +void NameManager::clear() +{ + mMap.clear(); + mReverseMap.clear(); +} + +//============================================================================== +template +bool NameManager::hasName(const std::string& _name) const +{ + return (mMap.find(_name) != mMap.end()); +} + +//============================================================================== +template +bool NameManager::hasObject(const T& _obj) const +{ + return (mReverseMap.find(_obj) != mReverseMap.end()); +} + +//============================================================================== +template +size_t NameManager::getCount() const +{ + return mMap.size(); +} + +//============================================================================== +template +T NameManager::getObject(const std::string& _name) const +{ + typename std::map::const_iterator result = + mMap.find(_name); + + if (result != mMap.end()) + return result->second; + else + return nullptr; +} + +//============================================================================== +template +std::string NameManager::getName(const T& _obj) const +{ + assert(mReverseMap.size() == mMap.size()); + + typename std::map::const_iterator result = + mReverseMap.find(_obj); + + if (result != mReverseMap.end()) + return result->second; + else + return ""; +} + +//============================================================================== +template +std::string NameManager::changeObjectName(const T& _obj, + const std::string& _newName) +{ + assert(mReverseMap.size() == mMap.size()); + + typename std::map::iterator rit = mReverseMap.find(_obj); + if(rit == mReverseMap.end()) + return _newName; + + if(rit->second == _newName) + return rit->second; + + removeName(rit->second); + return issueNewNameAndAdd(_newName, _obj); +} + +//============================================================================== +template +void NameManager::setDefaultName(const std::string& _defaultName) +{ + mDefaultName = _defaultName; +} + +//============================================================================== +template +const std::string& NameManager::getDefaultName() const +{ + return mDefaultName; +} + +//============================================================================== +template +void NameManager::setManagerName(const std::string& _managerName) +{ + mManagerName = _managerName; +} + +//============================================================================== +template +const std::string& NameManager::getManagerName() const +{ + return mManagerName; +} + +} // namespace common +} // namespace dart + +#endif // DART_COMMON_DETAIL_NAMEMANAGER_H_ diff --git a/dart/common/detail/Signal.h b/dart/common/detail/Signal.h new file mode 100644 index 0000000000000..9359c9158c6da --- /dev/null +++ b/dart/common/detail/Signal.h @@ -0,0 +1,279 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_DETAIL_SIGNAL_H_ +#define DART_COMMON_DETAIL_SIGNAL_H_ + +#include + +namespace dart { +namespace common { + +//============================================================================== +template class Combiner> +Signal<_Res (_ArgTypes...), Combiner>::Signal() +{ + // Do nothing +} + +//============================================================================== +template class Combiner> +Signal<_Res (_ArgTypes...), Combiner>::~Signal() +{ + disconnectAll(); +} + +//============================================================================== +template class Combiner> +Connection Signal<_Res (_ArgTypes...), Combiner>::connect(const SlotType& _slot) +{ + auto newConnectionBody = std::make_shared(_slot); + mConnectionBodies.insert(newConnectionBody); + + return Connection(std::move(newConnectionBody)); +} + +//============================================================================== +template class Combiner> +Connection Signal<_Res (_ArgTypes...), Combiner>::connect(SlotType&& _slot) +{ + auto newConnectionBody + = std::make_shared(std::forward(_slot)); + mConnectionBodies.insert(newConnectionBody); + + return Connection(std::move(newConnectionBody)); +} + +//============================================================================== +template class Combiner> +void Signal<_Res (_ArgTypes...), Combiner>::disconnect( + const Connection& _connection) const +{ + _connection.disconnect(); +} + +//============================================================================== +template class Combiner> +void Signal<_Res (_ArgTypes...), Combiner>::disconnectAll() +{ + mConnectionBodies.clear(); +} + +//============================================================================== +template class Combiner> +void Signal<_Res (_ArgTypes...), Combiner>::clenaupConnections() +{ + // Counts all the connected conection bodies + for (const auto& connectionBody : mConnectionBodies) + { + if (!connectionBody->isConnected()) + mConnectionBodies.erase(connectionBody); + } +} + +//============================================================================== +template class Combiner> +size_t Signal<_Res (_ArgTypes...), Combiner>::getNumConnections() const +{ + size_t numConnections = 0; + + // Counts all the connected conection bodies + for (const auto& connectionBody : mConnectionBodies) + { + if (connectionBody->isConnected()) + ++numConnections; + } + + return numConnections; +} + +//============================================================================== +template class Combiner> +template +_Res Signal<_Res (_ArgTypes...), Combiner>::raise(ArgTypes&&... _args) +{ + std::vector res(mConnectionBodies.size()); + auto resIt = res.begin(); + + for (auto itr = mConnectionBodies.begin(); itr != mConnectionBodies.end(); ) + { + if ((*itr)->isConnected()) + { + *(resIt++) = (*itr)->getSlot()(std::forward(_args)...); + ++itr; + } + else + { + mConnectionBodies.erase(itr++); + } + } + + return Combiner::process(res.begin(), resIt); +} + +//============================================================================== +template class Combiner> +template +_Res Signal<_Res (_ArgTypes...), Combiner>::operator()(ArgTypes&&... _args) +{ + return raise(std::forward(_args)...); +} + +//============================================================================== +template +Signal::Signal() +{ + // Do nothing +} + +//============================================================================== +template +Signal::~Signal() +{ + disconnectAll(); +} + +//============================================================================== +template +Connection Signal::connect(const SlotType& _slot) +{ + auto newConnectionBody = std::make_shared(_slot); + mConnectionBodies.insert(newConnectionBody); + + return Connection(std::move(newConnectionBody)); +} + +//============================================================================== +template +Connection Signal::connect(SlotType&& _slot) +{ + auto newConnectionBody + = std::make_shared(std::forward(_slot)); + mConnectionBodies.insert(newConnectionBody); + + return Connection(std::move(newConnectionBody)); +} + +//============================================================================== +template +void Signal::disconnect( + const Connection& _connection) const +{ + _connection.disconnect(); +} + +//============================================================================== +template +void Signal::disconnectAll() +{ + mConnectionBodies.clear(); +} + +//============================================================================== +template +void Signal::clenaupConnections() +{ + // Counts all the connected conection bodies + for (const auto& connectionBody : mConnectionBodies) + { + if (!connectionBody->isConnected()) + mConnectionBodies.erase(connectionBody); + } +} + +//============================================================================== +template +size_t Signal::getNumConnections() const +{ + size_t numConnections = 0; + + // Counts all the connected conection bodies + for (const auto& connectionBody : mConnectionBodies) + { + if (connectionBody->isConnected()) + ++numConnections; + } + + return numConnections; +} + +//============================================================================== +template +template +void Signal::raise(ArgTypes&&... _args) +{ + for (auto itr = mConnectionBodies.begin(); itr != mConnectionBodies.end(); ) + { + if ((*itr)->isConnected()) + { + (*itr)->getSlot()(std::forward(_args)...); + ++itr; + } + else + { + mConnectionBodies.erase(itr++); + } + } +} + +//============================================================================== +template +template +void Signal::operator()(ArgTypes&&... _args) +{ + raise(std::forward(_args)...); +} + +//============================================================================== +template +SlotRegister::SlotRegister(typename T::SignalType& _signal) + : mSignal(_signal) +{ + // Do nothing +} + +//============================================================================== +template +Connection SlotRegister::connect(const SlotType& _slot) +{ + return mSignal.connect(_slot); +} + +} // namespace common +} // namespace dart + +#endif // DART_COMMON_DETAIL_SIGNAL_H_ + diff --git a/dart/common/detail/sub_ptr.h b/dart/common/detail/sub_ptr.h new file mode 100644 index 0000000000000..6040dcd3d49c5 --- /dev/null +++ b/dart/common/detail/sub_ptr.h @@ -0,0 +1,126 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_DETAIL_SUB_PTR_H_ +#define DART_COMMON_DETAIL_SUB_PTR_H_ + +//============================================================================== +template +sub_ptr::sub_ptr() + : mSubject(nullptr) +{ + // Do nothing +} + +//============================================================================== +template +sub_ptr::sub_ptr(T* _ptr) : mSubject(nullptr) +{ + set(_ptr); +} + +//============================================================================== +template +sub_ptr& sub_ptr::operator =(const sub_ptr& _sp) +{ + set(_sp.get()); + return *this; +} + +//============================================================================== +template +sub_ptr& sub_ptr::operator =(T* _ptr) +{ + set(_ptr); + return *this; +} + +//============================================================================== +template +sub_ptr::operator T*() const +{ + return mSubject; +} + +//============================================================================== +template +T& sub_ptr::operator*() const +{ + return *mSubject; +} + +//============================================================================== +template +T* sub_ptr::operator->() const +{ + return mSubject; +} + +//============================================================================== +template +T* sub_ptr::get() const +{ + return mSubject; +} + +//============================================================================== +template +void sub_ptr::set(T* _ptr) +{ + if(mSubject == _ptr) + return; + + removeSubject(mSubject); + mSubject = _ptr; + addSubject(mSubject); +} + +//============================================================================== +template +bool sub_ptr::valid() +{ + return mSubject != nullptr; +} + +//============================================================================== +template +void sub_ptr::handleDestructionNotification(const Subject* _subject) +{ + if(_subject == mSubject) + mSubject = nullptr; +} + +#endif // DART_COMMON_DETAIL_SUB_PTR_H_ diff --git a/dart/common/sub_ptr.h b/dart/common/sub_ptr.h index 43480f15aa770..cba7186898230 100644 --- a/dart/common/sub_ptr.h +++ b/dart/common/sub_ptr.h @@ -51,65 +51,47 @@ class sub_ptr : public Observer { public: /// Default constructor - sub_ptr() : mSubject(nullptr) { } + sub_ptr(); /// Alternative constructor. _ptr must be a valid pointer when passed to this /// constructor. - sub_ptr(T* _ptr) : mSubject(nullptr) { set(_ptr); } + sub_ptr(T* _ptr); /// Change the Subject of this sub_ptr - sub_ptr& operator = (const sub_ptr& _sp) - { - set(_sp.get()); - return *this; - } + sub_ptr& operator = (const sub_ptr& _sp); /// Change the Subject of this sub_ptr - sub_ptr& operator = (T* _ptr) - { - set(_ptr); - return *this; - } + sub_ptr& operator = (T* _ptr); /// Implicit conversion to pointer type - operator T*() const { return mSubject; } + operator T*() const; /// Dereferencing operator - T& operator*() const { return *mSubject; } + T& operator*() const; /// Dereferencing operation - T* operator->() const { return mSubject; } + T* operator->() const; /// Get the Subject of this sub_ptr - T* get() const { return mSubject; } + T* get() const; /// Set the subject of this sub_ptr - void set(T* _ptr) - { - if(mSubject == _ptr) - return; - - removeSubject(mSubject); - mSubject = _ptr; - addSubject(mSubject); - } + void set(T* _ptr); /// True if and only if this sub_ptr still points to a valid Subject - bool valid() { return mSubject != nullptr; } + bool valid(); protected: virtual void handleDestructionNotification( - const Subject* _subject) override - { - if(_subject == mSubject) - mSubject = nullptr; - } + const Subject* _subject) override; /// Store the Subject pointer T* mSubject; }; +#include "dart/common/detail/sub_ptr.h" + } // namespace common // Make an alias for sub_ptr in the dart namespace for convenience From 3e7565870bb5c135b49ee6ba931ecb03c49e7448 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 11 Jun 2015 18:05:37 -0400 Subject: [PATCH 07/19] Suppress unused-variable warnings from Bullet in better way --- dart/collision/bullet/BulletTypes.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/dart/collision/bullet/BulletTypes.h b/dart/collision/bullet/BulletTypes.h index 13f37e6df93c4..47f324fce01eb 100644 --- a/dart/collision/bullet/BulletTypes.h +++ b/dart/collision/bullet/BulletTypes.h @@ -43,12 +43,13 @@ namespace dart { namespace collision { +#if BT_BULLET_VERSION < 283 /// Dummy structure to suppress unused-variable warning from bullet -struct BulletWarningSuppression +inline int btGetInfinityMask() { - BulletWarningSuppression() : dummy(btInfinityMask) {} - const int dummy; -}; + return btInfinityMask; +} +#endif /// @brief Convert Bullet vector3 type to Eigen vector3 type Eigen::Vector3d convertVector3(const btVector3& _vec); From 36777be73da5975ea0c5a31431fb0a22acdfb4bb Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 11 Jun 2015 18:40:25 -0400 Subject: [PATCH 08/19] Fix build error introduced by the previous commit --- dart/common/NameManager.h | 1 + dart/common/Signal.h | 3 +++ 2 files changed, 4 insertions(+) diff --git a/dart/common/NameManager.h b/dart/common/NameManager.h index 3f7750bb240e1..de7cea302c995 100644 --- a/dart/common/NameManager.h +++ b/dart/common/NameManager.h @@ -39,6 +39,7 @@ #define DART_COMMON_NAMEMANAGER_H_ #include +#include namespace dart { namespace common { diff --git a/dart/common/Signal.h b/dart/common/Signal.h index 05c4b82d08729..ed7015200fd46 100644 --- a/dart/common/Signal.h +++ b/dart/common/Signal.h @@ -74,6 +74,9 @@ class Connection /// Disconnect the connection void disconnect() const; + template class Combiner> + friend class Signal; + protected: /// Constructor given connection body Connection( From 6dc95afc9e219ae52f6d549931db98d6cd4ee51b Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 11 Jun 2015 18:41:36 -0400 Subject: [PATCH 09/19] Remove unnecesarry header inclusion in apps --- apps/bipedStand/Controller.cpp | 1 - apps/operationalSpaceControl/Controller.cpp | 1 - apps/speedTest/Main.cpp | 9 +-------- 3 files changed, 1 insertion(+), 10 deletions(-) diff --git a/apps/bipedStand/Controller.cpp b/apps/bipedStand/Controller.cpp index 2d294a2121ec4..b44651aff16b2 100644 --- a/apps/bipedStand/Controller.cpp +++ b/apps/bipedStand/Controller.cpp @@ -36,7 +36,6 @@ */ #include "apps/bipedStand/Controller.h" -#include "dart/dynamics/Group.h" void createIndexing(std::vector&) { diff --git a/apps/operationalSpaceControl/Controller.cpp b/apps/operationalSpaceControl/Controller.cpp index e1e47a270ffc3..7baa8ace43279 100644 --- a/apps/operationalSpaceControl/Controller.cpp +++ b/apps/operationalSpaceControl/Controller.cpp @@ -35,7 +35,6 @@ */ #include "apps/operationalSpaceControl/Controller.h" -#include "dart/math/MathTypes.h" //============================================================================== Controller::Controller(dart::dynamics::SkeletonPtr _robot, diff --git a/apps/speedTest/Main.cpp b/apps/speedTest/Main.cpp index 1a3285910b931..14a9e595e8829 100644 --- a/apps/speedTest/Main.cpp +++ b/apps/speedTest/Main.cpp @@ -38,14 +38,7 @@ #include #include -#include "dart/dynamics/Skeleton.h" -#include "dart/dynamics/DegreeOfFreedom.h" -#include "dart/dynamics/BodyNode.h" -#include "dart/dynamics/Joint.h" -#include "dart/simulation/World.h" -#include "dart/utils/SkelParser.h" -#include "dart/math/Helpers.h" -#include "dart/config.h" +#include "dart/dart.h" double testForwardKinematicSpeed(dart::dynamics::SkeletonPtr skel, bool position=true, From 8e5e7ac44cde44bcecc2ac2f4c51f14896dabf61 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 11 Jun 2015 18:49:20 -0400 Subject: [PATCH 10/19] Use std::abs() instead of fabs() or abs() --- dart/collision/dart/DARTCollide.cpp | 44 +++++++++++------------ dart/constraint/ConstrainedGroup.cpp | 1 + dart/constraint/ContactConstraint.cpp | 6 ++-- dart/constraint/DantzigLCPSolver.cpp | 4 +-- dart/constraint/PGSLCPSolver.cpp | 10 +++--- dart/constraint/SoftContactConstraint.cpp | 6 ++-- dart/dynamics/EulerJoint.cpp | 4 +-- dart/dynamics/Inertia.cpp | 8 ++--- dart/dynamics/PlanarJoint.cpp | 4 +-- dart/dynamics/PlaneShape.cpp | 2 +- dart/gui/lodepng.cpp | 6 ++-- dart/lcpsolver/Lemke.cpp | 2 +- dart/lcpsolver/ODELCPSolver.cpp | 2 +- dart/math/Geometry.cpp | 10 +++--- dart/math/Helpers.h | 8 ++--- dart/planning/Path.cpp | 6 ++-- dart/planning/PathFollowingTrajectory.cpp | 18 +++++----- unittests/testJoints.cpp | 6 ++-- unittests/testSoftDynamics.cpp | 2 +- 19 files changed, 75 insertions(+), 74 deletions(-) diff --git a/dart/collision/dart/DARTCollide.cpp b/dart/collision/dart/DARTCollide.cpp index ed7a54b04266e..0427c03fc88a8 100644 --- a/dart/collision/dart/DARTCollide.cpp +++ b/dart/collision/dart/DARTCollide.cpp @@ -446,9 +446,9 @@ int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1, R21 = Inner44(R1+1,R2+0); R22 = Inner44(R1+1,R2+1); R23 = Inner44(R1+1,R2+2); R31 = Inner44(R1+2,R2+0); R32 = Inner44(R1+2,R2+1); R33 = Inner44(R1+2,R2+2); - Q11 = fabs(R11); Q12 = fabs(R12); Q13 = fabs(R13); - Q21 = fabs(R21); Q22 = fabs(R22); Q23 = fabs(R23); - Q31 = fabs(R31); Q32 = fabs(R32); Q33 = fabs(R33); + Q11 = std::abs(R11); Q12 = std::abs(R12); Q13 = std::abs(R13); + Q21 = std::abs(R21); Q22 = std::abs(R22); Q23 = std::abs(R23); + Q31 = std::abs(R31); Q32 = std::abs(R32); Q33 = std::abs(R33); // for all 15 possible separating axes: // * see if the axis separates the boxes. if so, return 0. @@ -461,7 +461,7 @@ int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1, // the normal should be flipped. #define TST(expr1,expr2,norm,cc) \ - s2 = fabs(expr1) - (expr2); \ + s2 = std::abs(expr1) - (expr2); \ if (s2 > s) { \ s = s2; \ normalR = norm; \ @@ -487,7 +487,7 @@ int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1, // normal (n1,n2,n3) is relative to box 1. #undef TST #define TST(expr1,expr2,n1,n2,n3,cc) \ - s2 = fabs(expr1) - (expr2); \ + s2 = std::abs(expr1) - (expr2); \ l = sqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ if (l > 0) { \ s2 /= l; \ @@ -852,8 +852,8 @@ int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, if ( inside_box ) { // find nearest side from the sphere center - double min = halfSize[0] - fabs(p[0]); - double tmin = halfSize[1] - fabs(p[1]); + double min = halfSize[0] - std::abs(p[0]); + double tmin = halfSize[1] - std::abs(p[1]); int idx = 0; if ( tmin < min ) @@ -861,7 +861,7 @@ int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, min = tmin; idx = 1; } - tmin = halfSize[2] - fabs(p[2]); + tmin = halfSize[2] - std::abs(p[2]); if ( tmin < min ) { min = tmin; @@ -904,8 +904,8 @@ int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, } else { - double min = halfSize[0] - fabs(p[0]); - double tmin = halfSize[1] - fabs(p[1]); + double min = halfSize[0] - std::abs(p[0]); + double tmin = halfSize[1] - std::abs(p[1]); int idx = 0; if ( tmin < min ) @@ -913,7 +913,7 @@ int collideBoxSphere(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0, min = tmin; idx = 1; } - tmin = halfSize[2] - fabs(p[2]); + tmin = halfSize[2] - std::abs(p[2]); if ( tmin < min ) { min = tmin; @@ -960,8 +960,8 @@ int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, if ( inside_box ) { // find nearest side from the sphere center - double min = size[0] - fabs(p[0]); - double tmin = size[1] - fabs(p[1]); + double min = size[0] - std::abs(p[0]); + double tmin = size[1] - std::abs(p[1]); int idx = 0; if ( tmin < min ) @@ -969,7 +969,7 @@ int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, min = tmin; idx = 1; } - tmin = size[2] - fabs(p[2]); + tmin = size[2] - std::abs(p[2]); if ( tmin < min ) { min = tmin; @@ -1010,8 +1010,8 @@ int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, result->push_back(contact);} else { - double min = size[0] - fabs(p[0]); - double tmin = size[1] - fabs(p[1]); + double min = size[0] - std::abs(p[0]); + double tmin = size[1] - std::abs(p[1]); int idx = 0; if ( tmin < min ) @@ -1019,7 +1019,7 @@ int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0, min = tmin; idx = 1; } - tmin = size[2] - fabs(p[2]); + tmin = size[2] - std::abs(p[2]); if ( tmin < min ) { min = tmin; @@ -1093,7 +1093,7 @@ int collideCylinderSphere(const double& cyl_rad, const double& half_height, cons double dist = sqrt(center[0] * center[0] + center[1] * center[1]); - if ( dist < cyl_rad && fabs(center[2]) < half_height + sphere_rad ) + if ( dist < cyl_rad && std::abs(center[2]) < half_height + sphere_rad ) { Contact contact; contact.penetrationDepth = 0.5 * (half_height + sphere_rad - math::sgn(center[2]) * center[2]); @@ -1107,7 +1107,7 @@ int collideCylinderSphere(const double& cyl_rad, const double& half_height, cons double penetration = 0.5 * (cyl_rad + sphere_rad - dist); if ( penetration > 0.0 ) { - if ( fabs(center[2]) > half_height ) + if ( std::abs(center[2]) > half_height ) { Eigen::Vector3d point = (Eigen::Vector3d(center[0], center[1], 0.0).normalized()); point *= cyl_rad; @@ -1158,7 +1158,7 @@ int collideCylinderPlane(const double& cyl_rad, const double& half_height, const Ry.normalize(); if (mag < DART_COLLISION_EPS) { - if (fabs(Rx[2]) > 1.0 - DART_COLLISION_EPS) + if (std::abs(Rx[2]) > 1.0 - DART_COLLISION_EPS) Ry = Eigen::Vector3d::UnitX(); else Ry = (Eigen::Vector3d(Rx[1], -Rx[0], 0.0)).normalized(); @@ -1196,9 +1196,9 @@ int collideCylinderPlane(const double& cyl_rad, const double& half_height, const Eigen::Vector3d point; - if (fabs(depth[found] - depth[(found+1)%4]) < DART_COLLISION_EPS) + if (std::abs(depth[found] - depth[(found+1)%4]) < DART_COLLISION_EPS) point = T * (0.5 * (c[found] + c[(found+1)%4])); - else if (fabs(depth[found] - depth[(found+3)%4]) < DART_COLLISION_EPS) + else if (std::abs(depth[found] - depth[(found+3)%4]) < DART_COLLISION_EPS) point = T * (0.5 * (c[found] + c[(found+3)%4])); else point = T * c[found]; diff --git a/dart/constraint/ConstrainedGroup.cpp b/dart/constraint/ConstrainedGroup.cpp index 06099ebeef0dd..24d726f74f803 100644 --- a/dart/constraint/ConstrainedGroup.cpp +++ b/dart/constraint/ConstrainedGroup.cpp @@ -36,6 +36,7 @@ #include "dart/constraint/ConstrainedGroup.h" +#include #include #include "dart/common/Console.h" diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 4a4823c283028..8cf2c90355709 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -141,11 +141,11 @@ ContactConstraint::ContactConstraint(collision::Contact& _contact, // TODO(JS): Assumed that the number of tangent basis is 2. Eigen::MatrixXd D = getTangentBasisMatrixODE(ct->normal); - assert(std::fabs(ct->normal.dot(D.col(0))) < DART_EPSILON); - assert(std::fabs(ct->normal.dot(D.col(1))) < DART_EPSILON); + assert(std::abs(ct->normal.dot(D.col(0))) < DART_EPSILON); + assert(std::abs(ct->normal.dot(D.col(1))) < DART_EPSILON); // if (D.col(0).dot(D.col(1)) > 0.0) // std::cout << "D.col(0).dot(D.col(1): " << D.col(0).dot(D.col(1)) << std::endl; - assert(fabs(D.col(0).dot(D.col(1))) < DART_EPSILON); + assert(std::abs(D.col(0).dot(D.col(1))) < DART_EPSILON); // std::cout << "D: " << std::endl << D << std::endl; diff --git a/dart/constraint/DantzigLCPSolver.cpp b/dart/constraint/DantzigLCPSolver.cpp index cef79855b0d13..1c3c6d069e416 100644 --- a/dart/constraint/DantzigLCPSolver.cpp +++ b/dart/constraint/DantzigLCPSolver.cpp @@ -198,7 +198,7 @@ bool DantzigLCPSolver::isSymmetric(size_t _n, double* _A) { for (size_t j = 0; j < _n; ++j) { - if (std::fabs(_A[nSkip * i + j] - _A[nSkip * j + i]) > 1e-6) + if (std::abs(_A[nSkip * i + j] - _A[nSkip * j + i]) > 1e-6) { std::cout << "A: " << std::endl; for (size_t k = 0; k < _n; ++k) @@ -229,7 +229,7 @@ bool DantzigLCPSolver::isSymmetric(size_t _n, double* _A, { for (size_t j = _begin; j <= _end; ++j) { - if (std::fabs(_A[nSkip * i + j] - _A[nSkip * j + i]) > 1e-6) + if (std::abs(_A[nSkip * i + j] - _A[nSkip * j + i]) > 1e-6) { std::cout << "A: " << std::endl; for (size_t k = 0; k < _n; ++k) diff --git a/dart/constraint/PGSLCPSolver.cpp b/dart/constraint/PGSLCPSolver.cpp index 4833412c38475..50ea58047fd5c 100644 --- a/dart/constraint/PGSLCPSolver.cpp +++ b/dart/constraint/PGSLCPSolver.cpp @@ -201,7 +201,7 @@ bool PGSLCPSolver::isSymmetric(size_t _n, double* _A) { for (size_t j = 0; j < _n; ++j) { - if (std::fabs(_A[nSkip * i + j] - _A[nSkip * j + i]) > 1e-6) + if (std::abs(_A[nSkip * i + j] - _A[nSkip * j + i]) > 1e-6) { std::cout << "A: " << std::endl; for (size_t k = 0; k < _n; ++k) @@ -232,7 +232,7 @@ bool PGSLCPSolver::isSymmetric(size_t _n, double* _A, { for (size_t j = _begin; j <= _end; ++j) { - if (std::fabs(_A[nSkip * i + j] - _A[nSkip * j + i]) > 1e-6) + if (std::abs(_A[nSkip * i + j] - _A[nSkip * j + i]) > 1e-6) { std::cout << "A: " << std::endl; for (size_t k = 0; k < _n; ++k) @@ -412,7 +412,7 @@ bool solvePGS(int n, int nskip, int /*nub*/, double * A, double * x, double * b, // TEST if (sentinel) { - ea = fabs(x[i] - old_x); + ea = std::abs(x[i] - old_x); if (ea > option->eps_res) sentinel = false; } @@ -492,9 +492,9 @@ bool solvePGS(int n, int nskip, int /*nub*/, double * A, double * x, double * b, x[idx] = new_x; } - if ( sentinel && fabs(x[idx]) > option->eps_div) + if ( sentinel && std::abs(x[idx]) > option->eps_div) { - ea = fabs((x[idx] - old_x)/x[idx]); + ea = std::abs((x[idx] - old_x)/x[idx]); if (ea > option->eps_ea) sentinel = false; } diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp index e24dda5c55197..40c23952c15d0 100644 --- a/dart/constraint/SoftContactConstraint.cpp +++ b/dart/constraint/SoftContactConstraint.cpp @@ -178,11 +178,11 @@ SoftContactConstraint::SoftContactConstraint( // TODO(JS): Assumed that the number of tangent basis is 2. Eigen::MatrixXd D = getTangentBasisMatrixODE(ct->normal); - assert(std::fabs(ct->normal.dot(D.col(0))) < DART_EPSILON); - assert(std::fabs(ct->normal.dot(D.col(1))) < DART_EPSILON); + assert(std::abs(ct->normal.dot(D.col(0))) < DART_EPSILON); + assert(std::abs(ct->normal.dot(D.col(1))) < DART_EPSILON); // if (D.col(0).dot(D.col(1)) > 0.0) // std::cout << "D.col(0).dot(D.col(1): " << D.col(0).dot(D.col(1)) << std::endl; - assert(fabs(D.col(0).dot(D.col(1))) < DART_EPSILON); + assert(std::abs(D.col(0).dot(D.col(1))) < DART_EPSILON); // std::cout << "D: " << std::endl << D << std::endl; diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index f06649ba61584..658b201e15e68 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -220,7 +220,7 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( J2 << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; #ifndef NDEBUG - if (fabs(getPositionsStatic()[1]) == DART_PI * 0.5) + if (std::abs(getPositionsStatic()[1]) == DART_PI * 0.5) std::cout << "Singular configuration in ZYX-euler joint [" << mJointP.mName << "]. (" << _positions[0] << ", " @@ -246,7 +246,7 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( J2 << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; #ifndef NDEBUG - if (fabs(_positions[1]) == DART_PI * 0.5) + if (std::abs(_positions[1]) == DART_PI * 0.5) std::cout << "Singular configuration in ZYX-euler joint [" << mJointP.mName << "]. (" << _positions[0] << ", " diff --git a/dart/dynamics/Inertia.cpp b/dart/dynamics/Inertia.cpp index 62d025e9db116..829026af0d34b 100644 --- a/dart/dynamics/Inertia.cpp +++ b/dart/dynamics/Inertia.cpp @@ -221,7 +221,7 @@ bool Inertia::verifyMoment(const Eigen::Matrix3d& _moment, bool _printWarnings, { for(int j=i+1; j<3; ++j) { - if(fabs(_moment(i,j) - _moment(j,i)) > _tolerance) + if(std::abs(_moment(i,j) - _moment(j,i)) > _tolerance) { valid = false; if(_printWarnings) @@ -264,7 +264,7 @@ bool Inertia::verifySpatialTensor(const Eigen::Matrix6d& _spatial, { for(size_t j=i+1; j<3; ++j) { - if(fabs(_spatial(i,j) - _spatial(j,i)) > _tolerance) + if(std::abs(_spatial(i,j) - _spatial(j,i)) > _tolerance) { valid = false; dtwarn << "[Inertia::verifySpatialTensor] Values for entries (" << i @@ -331,7 +331,7 @@ bool Inertia::verifySpatialTensor(const Eigen::Matrix6d& _spatial, size_t i2 = k==0? j+3 : j; size_t j2 = k==0? i : i+3; - if(fabs(_spatial(i1,j1) + _spatial(i2,j2)) > _tolerance) + if(std::abs(_spatial(i1,j1) + _spatial(i2,j2)) > _tolerance) { valid = false; if(_printWarnings) @@ -359,7 +359,7 @@ bool Inertia::verifySpatialTensor(const Eigen::Matrix6d& _spatial, size_t i2 = i+3; size_t j2 = j; - if(fabs(_spatial(i1,j1) - _spatial(i2,j2)) > _tolerance) + if(std::abs(_spatial(i1,j1) - _spatial(i2,j2)) > _tolerance) { valid = false; if(_printWarnings) diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index faee08aa9e4af..f820ea5d37d96 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/dart/dynamics/PlanarJoint.cpp @@ -116,8 +116,8 @@ void PlanarJoint::UniqueProperties::setArbitraryPlane( // Orthogonalize translational axese double dotProduct = mTransAxis1.dot(mTransAxis2); - assert(std::fabs(dotProduct) < 1.0 - 1e-6); - if (std::fabs(dotProduct) > 1e-6) + assert(std::abs(dotProduct) < 1.0 - 1e-6); + if (std::abs(dotProduct) > 1e-6) mTransAxis2 = (mTransAxis2 - dotProduct * mTransAxis1).normalized(); // Rotational axis diff --git a/dart/dynamics/PlaneShape.cpp b/dart/dynamics/PlaneShape.cpp index 9dbed04c474be..f79b0df1d9ff3 100644 --- a/dart/dynamics/PlaneShape.cpp +++ b/dart/dynamics/PlaneShape.cpp @@ -125,7 +125,7 @@ void PlaneShape::setNormalAndPoint(const Eigen::Vector3d& _normal, //============================================================================== double PlaneShape::computeDistance(const Eigen::Vector3d& _point) const { - return std::fabs(computeSignedDistance(_point)); + return std::abs(computeSignedDistance(_point)); } //============================================================================== diff --git a/dart/gui/lodepng.cpp b/dart/gui/lodepng.cpp index 76a49319c2c56..fee6db0489959 100644 --- a/dart/gui/lodepng.cpp +++ b/dart/gui/lodepng.cpp @@ -3693,9 +3693,9 @@ are only needed to make the paeth calculation correct. */ static unsigned char paethPredictor(short a, short b, short c) { - short pa = abs(b - c); - short pb = abs(a - c); - short pc = abs(a + b - c - c); + short pa = std::abs(b - c); + short pb = std::abs(a - c); + short pc = std::abs(a + b - c - c); if(pc < pa && pc < pb) return (unsigned char)c; else if(pb < pa) return (unsigned char)b; diff --git a/dart/lcpsolver/Lemke.cpp b/dart/lcpsolver/Lemke.cpp index f1e8b4146aebd..78acdf7cfd719 100644 --- a/dart/lcpsolver/Lemke.cpp +++ b/dart/lcpsolver/Lemke.cpp @@ -284,7 +284,7 @@ bool validate(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _z, for (int i = 0; i < n; ++i) { if (w(i) < -threshold || _z(i) < -threshold) return false; - if (abs(w(i) * _z(i)) > threshold) + if (std::abs(w(i) * _z(i)) > threshold) return false; } return true; diff --git a/dart/lcpsolver/ODELCPSolver.cpp b/dart/lcpsolver/ODELCPSolver.cpp index 224c496d3628d..6cba7f3dc18b2 100644 --- a/dart/lcpsolver/ODELCPSolver.cpp +++ b/dart/lcpsolver/ODELCPSolver.cpp @@ -200,7 +200,7 @@ bool ODELCPSolver::checkIfSolution(const Eigen::MatrixXd& _A, for (int i = 0; i < n; ++i) { if (w(i) < -threshold || _x(i) < -threshold) return false; - if (abs(w(i) * _x(i)) > threshold) + if (std::abs(w(i) * _x(i)) > threshold) return false; } return true; diff --git a/dart/math/Geometry.cpp b/dart/math/Geometry.cpp index fc0be49ce9bba..10fc260e6bd66 100644 --- a/dart/math/Geometry.cpp +++ b/dart/math/Geometry.cpp @@ -1402,19 +1402,19 @@ Eigen::Matrix3d parallelAxisTheorem(const Eigen::Matrix3d& _original, bool verifyRotation(const Eigen::Matrix3d& _T) { return !isNan(_T) - && fabs(_T.determinant() - 1.0) <= DART_EPSILON; + && std::abs(_T.determinant() - 1.0) <= DART_EPSILON; } bool verifyTransform(const Eigen::Isometry3d& _T) { return !isNan(_T.matrix().topRows<3>()) - && fabs(_T.linear().determinant() - 1.0) <= DART_EPSILON; + && std::abs(_T.linear().determinant() - 1.0) <= DART_EPSILON; } Eigen::Vector3d fromSkewSymmetric(const Eigen::Matrix3d& _m) { #ifndef NDEBUG - if (fabs(_m(0, 0)) > DART_EPSILON - || fabs(_m(1, 1)) > DART_EPSILON - || fabs(_m(2, 2)) > DART_EPSILON) { + if (std::abs(_m(0, 0)) > DART_EPSILON + || std::abs(_m(1, 1)) > DART_EPSILON + || std::abs(_m(2, 2)) > DART_EPSILON) { std::cout << "Not skew symmetric matrix" << std::endl; std::cerr << _m << std::endl; return Eigen::Vector3d::Zero(); diff --git a/dart/math/Helpers.h b/dart/math/Helpers.h index c62a14eb61859..5915cbd62b8ff 100644 --- a/dart/math/Helpers.h +++ b/dart/math/Helpers.h @@ -86,7 +86,7 @@ inline double Tsinc(double _theta) { } inline bool isZero(double _theta) { - return (fabs(_theta) < DART_EPSILON); + return (std::abs(_theta) < DART_EPSILON); } inline double asinh(double _X) { @@ -126,7 +126,7 @@ inline double round2(double _x) { } inline bool isEqual(double _x, double _y) { - return (std::fabs(_x - _y) < DART_EPSILON); + return (std::abs(_x - _y) < DART_EPSILON); } // check if it is an integer @@ -186,7 +186,7 @@ inline bool isSymmetric(const Eigen::MatrixXd& _m, double _tol = 1e-6) { for (size_t i = 0; i < rows; ++i) { for (size_t j = i + 1; j < cols; ++j) { - if (std::fabs(_m(i, j) - _m(j, i)) > _tol) { + if (std::abs(_m(i, j) - _m(j, i)) > _tol) { std::cout << "A: " << std::endl; for (size_t k = 0; k < rows; ++k) { for (size_t l = 0; l < cols; ++l) @@ -235,7 +235,7 @@ Eigen::Matrix randomVector(double _min, double _max) template Eigen::Matrix randomVector(double _limit) { - return randomVector(-fabs(_limit), fabs(_limit)); + return randomVector(-std::abs(_limit), std::abs(_limit)); } } // namespace math diff --git a/dart/planning/Path.cpp b/dart/planning/Path.cpp index 222615c798bf0..47241fe98a7e9 100644 --- a/dart/planning/Path.cpp +++ b/dart/planning/Path.cpp @@ -130,7 +130,7 @@ class CircularPathSegment : public PathSegment //debug double dotStart = startDirection.dot((intersection - getConfig(0.0)).normalized()); double dotEnd = endDirection.dot((getConfig(length) - intersection).normalized()); - if(abs(dotStart - 1.0) > 0.0001 || abs(dotEnd - 1.0) > 0.0001) { + if(std::abs(dotStart - 1.0) > 0.0001 || std::abs(dotEnd - 1.0) > 0.0001) { std::cout << "Error\n"; } } @@ -205,9 +205,9 @@ Path::Path(const list &path, double maxDeviation) : //debug if(((endConfig - *config1).norm() > 0.000001 && (*config2 - endConfig).norm() > 0.000001 - && abs((endConfig - *config1).normalized().dot((*config2 - endConfig).normalized()) - 1.0) > 0.000001) + && std::abs((endConfig - *config1).normalized().dot((*config2 - endConfig).normalized()) - 1.0) > 0.000001) || ((startConfig - *config2).norm() > 0.000001 && (*config3 - startConfig).norm() > 0.000001 - && abs((startConfig - *config2).normalized().dot((*config3 - startConfig).normalized()) - 1.0) > 0.000001)) { + && std::abs((startConfig - *config2).normalized().dot((*config3 - startConfig).normalized()) - 1.0) > 0.000001)) { cout << "error" << endl; } } diff --git a/dart/planning/PathFollowingTrajectory.cpp b/dart/planning/PathFollowingTrajectory.cpp index d156021bac55f..efc663401c077 100644 --- a/dart/planning/PathFollowingTrajectory.cpp +++ b/dart/planning/PathFollowingTrajectory.cpp @@ -470,7 +470,7 @@ double PathFollowingTrajectory::getMinMaxPathAcceleration(double pathPos, double for(unsigned int i = 0; i < n; i++) { if(configDeriv[i] != 0.0) { maxPathAcceleration = min(maxPathAcceleration, - maxAcceleration[i]/abs(configDeriv[i]) - factor * configDeriv2[i] * pathVel*pathVel / configDeriv[i]); + maxAcceleration[i]/std::abs(configDeriv[i]) - factor * configDeriv2[i] * pathVel*pathVel / configDeriv[i]); } } return factor * maxPathAcceleration; @@ -491,14 +491,14 @@ double PathFollowingTrajectory::getAccelerationMaxPathVelocity(double pathPos) { double A_ij = configDeriv2[i] / configDeriv[i] - configDeriv2[j] / configDeriv[j]; if(A_ij != 0.0) { maxPathVelocity = min(maxPathVelocity, - sqrt((maxAcceleration[i] / abs(configDeriv[i]) + maxAcceleration[j] / abs(configDeriv[j])) - / abs(A_ij))); + sqrt((maxAcceleration[i] / std::abs(configDeriv[i]) + maxAcceleration[j] / std::abs(configDeriv[j])) + / std::abs(A_ij))); } } } } else if(configDeriv2[i] != 0.0) { - maxPathVelocity = min(maxPathVelocity, sqrt(maxAcceleration[i] / abs(configDeriv2[i]))); + maxPathVelocity = min(maxPathVelocity, sqrt(maxAcceleration[i] / std::abs(configDeriv2[i]))); } } return maxPathVelocity; @@ -509,7 +509,7 @@ double PathFollowingTrajectory::getVelocityMaxPathVelocity(double pathPos) { const VectorXd tangent = path.getTangent(pathPos); double maxPathVelocity = numeric_limits::max(); for(unsigned int i = 0; i < n; i++) { - maxPathVelocity = min(maxPathVelocity, maxVelocity[i] / abs(tangent[i])); + maxPathVelocity = min(maxPathVelocity, maxVelocity[i] / std::abs(tangent[i])); } return maxPathVelocity; } @@ -523,14 +523,14 @@ double PathFollowingTrajectory::getVelocityMaxPathVelocityDeriv(double pathPos) double maxPathVelocity = numeric_limits::max(); unsigned int activeConstraint = 0; for(unsigned int i = 0; i < n; i++) { - const double thisMaxPathVelocity = maxVelocity[i] / abs(tangent[i]); + const double thisMaxPathVelocity = maxVelocity[i] / std::abs(tangent[i]); if(thisMaxPathVelocity < maxPathVelocity) { maxPathVelocity = thisMaxPathVelocity; activeConstraint = i; } } return - (maxVelocity[activeConstraint] * path.getCurvature(pathPos)[activeConstraint]) - / (tangent[activeConstraint] * abs(tangent[activeConstraint])); + / (tangent[activeConstraint] * std::abs(tangent[activeConstraint])); } bool PathFollowingTrajectory::isValid() const { @@ -611,8 +611,8 @@ double PathFollowingTrajectory::getMaxAccelerationError() { VectorXd acceleration = path.getTangent(pathPos) * pathAcceleration + path.getCurvature(pathPos) * pathVel * pathVel; for(int i = 0; i < acceleration.size(); i++) { - if(abs(acceleration[i]) > maxAcceleration[i]) { - maxAccelerationError = max(maxAccelerationError, abs(acceleration[i]) / maxAcceleration[i]); + if(std::abs(acceleration[i]) > maxAcceleration[i]) { + maxAccelerationError = max(maxAccelerationError, std::abs(acceleration[i]) / maxAcceleration[i]); } } } diff --git a/unittests/testJoints.cpp b/unittests/testJoints.cpp index b5fbab6caa7bb..9906b6fd68c61 100644 --- a/unittests/testJoints.cpp +++ b/unittests/testJoints.cpp @@ -435,8 +435,8 @@ void testJointCoulombFrictionForce(double _timeStep) double jointVel0 = joint0->getVelocity(0); double jointVel1 = joint1->getVelocity(0); - EXPECT_GE(std::fabs(jointVel0), 0.0); - EXPECT_GE(std::fabs(jointVel1), 0.0); + EXPECT_GE(std::abs(jointVel0), 0.0); + EXPECT_GE(std::abs(jointVel1), 0.0); } // Spend 20 sec waiting the joints to stop @@ -484,7 +484,7 @@ Eigen::Matrix random_vec(double limit=100) { Eigen::Matrix v; for(size_t i=0; i& A, const DenseBase& B, { if (boost::math::isnan(A(i,j)) ^ boost::math::isnan(B(i,j))) return false; - else if (fabs(A(i,j) - B(i,j)) > tol) + else if (std::abs(A(i,j) - B(i,j)) > tol) return false; } } From db8b1df822196417e9f65a8f1b9ee73a720ae4ec Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 11 Jun 2015 19:09:53 -0400 Subject: [PATCH 11/19] Add missing override specifier --- dart/dynamics/BallJoint.h | 10 +-- dart/dynamics/BodyNode.h | 12 +-- dart/dynamics/EulerJoint.h | 8 +- dart/dynamics/FreeJoint.h | 10 +-- dart/dynamics/MultiDofJoint.h | 28 +++---- dart/dynamics/PlanarJoint.h | 8 +- dart/dynamics/PrismaticJoint.h | 6 +- dart/dynamics/ReferentialSkeleton.h | 40 ++++----- dart/dynamics/RevoluteJoint.h | 6 +- dart/dynamics/ScrewJoint.h | 6 +- dart/dynamics/SingleDofJoint.h | 20 ++--- dart/dynamics/Skeleton.h | 4 +- dart/dynamics/SoftBodyNode.h | 6 +- dart/dynamics/TranslationalJoint.h | 8 +- dart/dynamics/UniversalJoint.h | 8 +- dart/dynamics/WeldJoint.h | 12 +-- dart/dynamics/ZeroDofJoint.h | 116 +++++++++++++------------- dart/renderer/OpenGLRenderInterface.h | 58 ++++++------- 18 files changed, 184 insertions(+), 182 deletions(-) diff --git a/dart/dynamics/BallJoint.h b/dart/dynamics/BallJoint.h index a8ab712c1203c..50c8609a25545 100644 --- a/dart/dynamics/BallJoint.h +++ b/dart/dynamics/BallJoint.h @@ -107,19 +107,19 @@ class BallJoint : public MultiDofJoint<3> using MultiDofJoint::getLocalJacobianStatic; // Documentation inherited - virtual void integratePositions(double _dt); + virtual void integratePositions(double _dt) override; // Documentation inherited - virtual void updateDegreeOfFreedomNames(); + virtual void updateDegreeOfFreedomNames() override; // Documentation inherited - virtual void updateLocalTransform() const; + virtual void updateLocalTransform() const override; // Documentation inherited - virtual void updateLocalJacobian(bool =true) const; + virtual void updateLocalJacobian(bool =true) const override; // Documentation inherited - virtual void updateLocalJacobianTimeDeriv() const; + virtual void updateLocalJacobianTimeDeriv() const override; protected: diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index 99c44f71caca4..c2905f455aeef 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -157,10 +157,10 @@ class BodyNode : public Frame, public SkeletonRefCountingBase /// Set name. If the name is already taken, this will return an altered /// version which will be used by the Skeleton - const std::string& setName(const std::string& _name); + const std::string& setName(const std::string& _name) override; /// Return the name of the bodynode - const std::string& getName() const; + const std::string& getName() const override; /// Set whether gravity affects this body /// \param[in] _gravityMode True to enable gravity @@ -506,10 +506,10 @@ class BodyNode : public Frame, public SkeletonRefCountingBase /// Get the transform of this BodyNode with respect to its parent BodyNode, /// which is also its parent Frame. - const Eigen::Isometry3d& getRelativeTransform() const; + const Eigen::Isometry3d& getRelativeTransform() const override; // Documentation inherited - const Eigen::Vector6d& getRelativeSpatialVelocity() const; + const Eigen::Vector6d& getRelativeSpatialVelocity() const override; // Documentation inherited const Eigen::Vector6d& getRelativeSpatialAcceleration() const override; @@ -824,10 +824,10 @@ class BodyNode : public Frame, public SkeletonRefCountingBase /// Separate generic child Entities from child BodyNodes for more efficient /// update notices - void processNewEntity(Entity* _newChildEntity); + void processNewEntity(Entity* _newChildEntity) override; /// Remove this Entity from mChildBodyNodes or mNonBodyNodeEntities - void processRemovedEntity(Entity* _oldChildEntity); + void processRemovedEntity(Entity* _oldChildEntity) override; /// Update transformation virtual void updateTransform(); diff --git a/dart/dynamics/EulerJoint.h b/dart/dynamics/EulerJoint.h index 9d7945fb7d797..05855ffbfcc65 100644 --- a/dart/dynamics/EulerJoint.h +++ b/dart/dynamics/EulerJoint.h @@ -180,16 +180,16 @@ class EulerJoint : public MultiDofJoint<3> /// Set the names of this joint's DegreesOfFreedom. Used during construction /// and when axis order is changed. - virtual void updateDegreeOfFreedomNames(); + virtual void updateDegreeOfFreedomNames() override; // Documentation inherited - virtual void updateLocalTransform() const; + virtual void updateLocalTransform() const override; // Documentation inherited - virtual void updateLocalJacobian(bool =true) const; + virtual void updateLocalJacobian(bool =true) const override; // Documentation inherited - virtual void updateLocalJacobianTimeDeriv() const; + virtual void updateLocalJacobianTimeDeriv() const override; protected: diff --git a/dart/dynamics/FreeJoint.h b/dart/dynamics/FreeJoint.h index 8c354b28c5926..8f61557e5e0db 100644 --- a/dart/dynamics/FreeJoint.h +++ b/dart/dynamics/FreeJoint.h @@ -103,19 +103,19 @@ class FreeJoint : public MultiDofJoint<6> using MultiDofJoint::getLocalJacobianStatic; // Documentation inherited - virtual void integratePositions(double _dt); + virtual void integratePositions(double _dt) override; // Documentation inherited - virtual void updateDegreeOfFreedomNames(); + virtual void updateDegreeOfFreedomNames() override; // Documentation inherited - virtual void updateLocalTransform() const; + virtual void updateLocalTransform() const override; // Documentation inherited - virtual void updateLocalJacobian(bool =true) const; + virtual void updateLocalJacobian(bool =true) const override; // Documentation inherited - virtual void updateLocalJacobianTimeDeriv() const; + virtual void updateLocalJacobianTimeDeriv() const override; protected: diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index 5ccdc463df0ac..2f8f3be7edf0d 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -193,19 +193,19 @@ class MultiDofJoint : public Joint //---------------------------------------------------------------------------- // Documentation inherited - virtual void setCommand(size_t _index, double _command); + virtual void setCommand(size_t _index, double _command) override; // Documentation inherited - virtual double getCommand(size_t _index) const; + virtual double getCommand(size_t _index) const override; // Documentation inherited - virtual void setCommands(const Eigen::VectorXd& _commands); + virtual void setCommands(const Eigen::VectorXd& _commands) override; // Documentation inherited - virtual Eigen::VectorXd getCommands() const; + virtual Eigen::VectorXd getCommands() const override; // Documentation inherited - virtual void resetCommands(); + virtual void resetCommands() override; //---------------------------------------------------------------------------- // Position @@ -364,36 +364,36 @@ class MultiDofJoint : public Joint //---------------------------------------------------------------------------- // Documentation inherited - virtual void setVelocityChange(size_t _index, double _velocityChange); + virtual void setVelocityChange(size_t _index, double _velocityChange) override; // Documentation inherited - virtual double getVelocityChange(size_t _index) const; + virtual double getVelocityChange(size_t _index) const override; // Documentation inherited - virtual void resetVelocityChanges(); + virtual void resetVelocityChanges() override; //---------------------------------------------------------------------------- // Constraint impulse //---------------------------------------------------------------------------- // Documentation inherited - virtual void setConstraintImpulse(size_t _index, double _impulse); + virtual void setConstraintImpulse(size_t _index, double _impulse) override; // Documentation inherited - virtual double getConstraintImpulse(size_t _index) const; + virtual double getConstraintImpulse(size_t _index) const override; // Documentation inherited - virtual void resetConstraintImpulses(); + virtual void resetConstraintImpulses() override; //---------------------------------------------------------------------------- // Integration and finite difference //---------------------------------------------------------------------------- // Documentation inherited - virtual void integratePositions(double _dt); + virtual void integratePositions(double _dt) override; // Documentation inherited - virtual void integrateVelocities(double _dt); + virtual void integrateVelocities(double _dt) override; // Documentation inherited Eigen::VectorXd getPositionDifferences( @@ -436,7 +436,7 @@ class MultiDofJoint : public Joint //---------------------------------------------------------------------------- // Documentation inherited - virtual double getPotentialEnergy() const; + virtual double getPotentialEnergy() const override; // Documentation inherited virtual Eigen::Vector6d getBodyConstraintWrench() const override; diff --git a/dart/dynamics/PlanarJoint.h b/dart/dynamics/PlanarJoint.h index d68d0531d3dcf..e54597ae838a0 100644 --- a/dart/dynamics/PlanarJoint.h +++ b/dart/dynamics/PlanarJoint.h @@ -199,16 +199,16 @@ class PlanarJoint : public MultiDofJoint<3> /// Set the names of this joint's DegreesOfFreedom. Used during construction /// and when the Plane type is changed. - virtual void updateDegreeOfFreedomNames(); + virtual void updateDegreeOfFreedomNames() override; // Documentation inherited - virtual void updateLocalTransform() const; + virtual void updateLocalTransform() const override; // Documentation inherited - virtual void updateLocalJacobian(bool =true) const; + virtual void updateLocalJacobian(bool =true) const override; // Documentation inherited - virtual void updateLocalJacobianTimeDeriv() const; + virtual void updateLocalJacobianTimeDeriv() const override; protected: diff --git a/dart/dynamics/PrismaticJoint.h b/dart/dynamics/PrismaticJoint.h index 37df2040b0499..0fbf41a2d7615 100644 --- a/dart/dynamics/PrismaticJoint.h +++ b/dart/dynamics/PrismaticJoint.h @@ -114,13 +114,13 @@ class PrismaticJoint : public SingleDofJoint virtual Joint* clone() const override; // Documentation inherited - virtual void updateLocalTransform() const; + virtual void updateLocalTransform() const override; // Documentation inherited - virtual void updateLocalJacobian(bool _mandatory=true) const; + virtual void updateLocalJacobian(bool _mandatory=true) const override; // Documentation inherited - virtual void updateLocalJacobianTimeDeriv() const; + virtual void updateLocalJacobianTimeDeriv() const override; protected: diff --git a/dart/dynamics/ReferentialSkeleton.h b/dart/dynamics/ReferentialSkeleton.h index 1d8963684634f..4ee40c0e4742f 100644 --- a/dart/dynamics/ReferentialSkeleton.h +++ b/dart/dynamics/ReferentialSkeleton.h @@ -130,96 +130,98 @@ class ReferentialSkeleton : public MetaSkeleton //---------------------------------------------------------------------------- // Documentation inherited - math::Jacobian getJacobian(const BodyNode *_bodyNode) const; + math::Jacobian getJacobian(const BodyNode *_bodyNode) const override; // Documentation inherited math::Jacobian getJacobian( const BodyNode* _bodyNode, - const Frame* _inCoordinatesOf) const; + const Frame* _inCoordinatesOf) const override; // Documentation inherited math::Jacobian getJacobian( const BodyNode* _bodyNode, - const Eigen::Vector3d& _localOffset) const; + const Eigen::Vector3d& _localOffset) const override; // Documentation inherited math::Jacobian getJacobian( const BodyNode* _bodyNode, const Eigen::Vector3d& _localOffset, - const Frame* _inCoordinatesOf) const; + const Frame* _inCoordinatesOf) const override; // Documentation inherited - math::Jacobian getWorldJacobian(const BodyNode* _bodyNode) const; + math::Jacobian getWorldJacobian(const BodyNode* _bodyNode) const override; // Documentation inherited math::Jacobian getWorldJacobian( const BodyNode* _bodyNode, - const Eigen::Vector3d& _localOffset) const; + const Eigen::Vector3d& _localOffset) const override; // Documentation inherited math::LinearJacobian getLinearJacobian( const BodyNode* _bodyNode, - const Frame* _inCoordinatesOf = Frame::World()) const; + const Frame* _inCoordinatesOf = Frame::World()) const override; // Documentation inherited math::LinearJacobian getLinearJacobian( const BodyNode* _bodyNode, const Eigen::Vector3d& _localOffset, - const Frame* _inCoordinatesOf = Frame::World()) const; + const Frame* _inCoordinatesOf = Frame::World()) const override; // Documentation inherited math::AngularJacobian getAngularJacobian( const BodyNode* _bodyNode, - const Frame* _inCoordinatesOf = Frame::World()) const; + const Frame* _inCoordinatesOf = Frame::World()) const override; // Documentation inherited - math::Jacobian getJacobianSpatialDeriv(const BodyNode* _bodyNode) const; + math::Jacobian getJacobianSpatialDeriv( + const BodyNode* _bodyNode) const override; // Documentation inherited math::Jacobian getJacobianSpatialDeriv( const BodyNode* _bodyNode, - const Frame* _inCoordinatesOf) const; + const Frame* _inCoordinatesOf) const override; // Documentation inherited math::Jacobian getJacobianSpatialDeriv( const BodyNode* _bodyNode, - const Eigen::Vector3d& _localOffset) const; + const Eigen::Vector3d& _localOffset) const override; // Documentation inherited math::Jacobian getJacobianSpatialDeriv( const BodyNode* _bodyNode, const Eigen::Vector3d& _localOffset, - const Frame* _inCoordinatesOf) const; + const Frame* _inCoordinatesOf) const override; // Documentation inherited - math::Jacobian getJacobianClassicDeriv(const BodyNode* _bodyNode) const; + math::Jacobian getJacobianClassicDeriv( + const BodyNode* _bodyNode) const override; // Documentation inherited math::Jacobian getJacobianClassicDeriv( const BodyNode* _bodyNode, - const Frame* _inCoordinatesOf) const; + const Frame* _inCoordinatesOf) const override; // Documentation inherited math::Jacobian getJacobianClassicDeriv( const BodyNode* _bodyNode, const Eigen::Vector3d& _localOffset, - const Frame* _inCoordinatesOf = Frame::World()) const; + const Frame* _inCoordinatesOf = Frame::World()) const override; // Documentation inherited math::LinearJacobian getLinearJacobianDeriv( const BodyNode* _bodyNode, - const Frame* _inCoordinatesOf = Frame::World()) const; + const Frame* _inCoordinatesOf = Frame::World()) const override; // Documentation inherited math::LinearJacobian getLinearJacobianDeriv( const BodyNode* _bodyNode, const Eigen::Vector3d& _localOffset = Eigen::Vector3d::Zero(), - const Frame* _inCoordinatesOf = Frame::World()) const; + const Frame* _inCoordinatesOf = Frame::World()) const override; // Documentation inherited math::AngularJacobian getAngularJacobianDeriv( const BodyNode* _bodyNode, - const Frame* _inCoordinatesOf = Frame::World()) const; + const Frame* _inCoordinatesOf = Frame::World()) const override; /// \} diff --git a/dart/dynamics/RevoluteJoint.h b/dart/dynamics/RevoluteJoint.h index d233ab9ef15f4..c6e43448993d1 100644 --- a/dart/dynamics/RevoluteJoint.h +++ b/dart/dynamics/RevoluteJoint.h @@ -116,13 +116,13 @@ class RevoluteJoint : public SingleDofJoint virtual Joint* clone() const override; // Documentation inherited - virtual void updateLocalTransform() const; + virtual void updateLocalTransform() const override; // Documentation inherited - virtual void updateLocalJacobian(bool _mandatory=true) const; + virtual void updateLocalJacobian(bool _mandatory=true) const override; // Documentation inherited - virtual void updateLocalJacobianTimeDeriv() const; + virtual void updateLocalJacobianTimeDeriv() const override; protected: diff --git a/dart/dynamics/ScrewJoint.h b/dart/dynamics/ScrewJoint.h index 43008a32d00f6..cac82fa3c82a1 100644 --- a/dart/dynamics/ScrewJoint.h +++ b/dart/dynamics/ScrewJoint.h @@ -125,13 +125,13 @@ class ScrewJoint : public SingleDofJoint virtual Joint* clone() const override; // Documentation inherited - virtual void updateLocalTransform() const; + virtual void updateLocalTransform() const override; // Documentation inherited - virtual void updateLocalJacobian(bool _mandatory=true) const; + virtual void updateLocalJacobian(bool _mandatory=true) const override; // Documentation inherited - virtual void updateLocalJacobianTimeDeriv() const; + virtual void updateLocalJacobianTimeDeriv() const override; protected: diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index 3c26ae6ae51c1..4ed8ee99d2b6f 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -353,36 +353,36 @@ class SingleDofJoint : public Joint //---------------------------------------------------------------------------- // Documentation inherited - virtual void setVelocityChange(size_t _index, double _velocityChange); + virtual void setVelocityChange(size_t _index, double _velocityChange) override; // Documentation inherited - virtual double getVelocityChange(size_t _index) const; + virtual double getVelocityChange(size_t _index) const override; // Documentation inherited - virtual void resetVelocityChanges(); + virtual void resetVelocityChanges() override; //---------------------------------------------------------------------------- // Constraint impulse //---------------------------------------------------------------------------- // Documentation inherited - virtual void setConstraintImpulse(size_t _index, double _impulse); + virtual void setConstraintImpulse(size_t _index, double _impulse) override; // Documentation inherited - virtual double getConstraintImpulse(size_t _index) const; + virtual double getConstraintImpulse(size_t _index) const override; // Documentation inherited - virtual void resetConstraintImpulses(); + virtual void resetConstraintImpulses() override; //---------------------------------------------------------------------------- // Integration and finite difference //---------------------------------------------------------------------------- // Documentation inherited - virtual void integratePositions(double _dt); + virtual void integratePositions(double _dt) override; // Documentation inherited - virtual void integrateVelocities(double _dt); + virtual void integrateVelocities(double _dt) override; // Documentation inherited Eigen::VectorXd getPositionDifferences( @@ -424,7 +424,7 @@ class SingleDofJoint : public Joint //---------------------------------------------------------------------------- /// Get potential energy - virtual double getPotentialEnergy() const; + virtual double getPotentialEnergy() const override; // Documentation inherited virtual Eigen::Vector6d getBodyConstraintWrench() const override; @@ -438,7 +438,7 @@ class SingleDofJoint : public Joint void registerDofs() override; // Documentation inherited - virtual void updateDegreeOfFreedomNames(); + virtual void updateDegreeOfFreedomNames() override; //---------------------------------------------------------------------------- /// \{ \name Recursive dynamics routines diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index c924746952d37..0f8a0f18a9f59 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -131,10 +131,10 @@ class Skeleton : public MetaSkeleton const Properties& getSkeletonProperties() const; /// Set name. - const std::string& setName(const std::string& _name); + const std::string& setName(const std::string& _name) override; /// Get name. - const std::string& getName() const; + const std::string& getName() const override; /// Enable self collision check void enableSelfCollision(bool _enableAdjecentBodies = false); diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index 3e2689debd0cd..8dd0e3c2035ad 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -196,7 +196,7 @@ class SoftBodyNode : public BodyNode size_t getNumFaces(); // Documentation inherited. - virtual void clearConstraintImpulse(); + virtual void clearConstraintImpulse() override; protected: @@ -212,7 +212,7 @@ class SoftBodyNode : public BodyNode // Sub-functions for Recursive Kinematics Algorithms //-------------------------------------------------------------------------- // Documentation inherited. - virtual void init(const SkeletonPtr& _skeleton); + virtual void init(const SkeletonPtr& _skeleton) override; // Documentation inherited. // virtual void aggregateGenCoords(std::vector* _genCoords); @@ -342,7 +342,7 @@ class SoftBodyNode : public BodyNode /// \brief Render the entire subtree rooted at this body node. virtual void draw(renderer::RenderInterface* _ri = NULL, const Eigen::Vector4d& _color = Eigen::Vector4d::Ones(), - bool _useDefaultColor = true, int _depth = 0) const; + bool _useDefaultColor = true, int _depth = 0) const override; protected: /// \brief List of point masses composing deformable mesh. diff --git a/dart/dynamics/TranslationalJoint.h b/dart/dynamics/TranslationalJoint.h index f77ce93ba9cc9..9cdfb629293b6 100644 --- a/dart/dynamics/TranslationalJoint.h +++ b/dart/dynamics/TranslationalJoint.h @@ -84,16 +84,16 @@ class TranslationalJoint : public MultiDofJoint<3> using MultiDofJoint::getLocalJacobianStatic; // Documentation inherited - virtual void updateDegreeOfFreedomNames(); + virtual void updateDegreeOfFreedomNames() override; // Documentation inherited - virtual void updateLocalTransform() const; + virtual void updateLocalTransform() const override; // Documentation inherited - virtual void updateLocalJacobian(bool _mandatory = true) const; + virtual void updateLocalJacobian(bool _mandatory = true) const override; // Documentation inherited - virtual void updateLocalJacobianTimeDeriv() const; + virtual void updateLocalJacobianTimeDeriv() const override; public: // To get byte-aligned Eigen vectors diff --git a/dart/dynamics/UniversalJoint.h b/dart/dynamics/UniversalJoint.h index 6b1e0a1a644cb..8d767fd726577 100644 --- a/dart/dynamics/UniversalJoint.h +++ b/dart/dynamics/UniversalJoint.h @@ -127,16 +127,16 @@ class UniversalJoint : public MultiDofJoint<2> using MultiDofJoint::getLocalJacobianStatic; // Documentation inherited - virtual void updateDegreeOfFreedomNames(); + virtual void updateDegreeOfFreedomNames() override; // Documentation inherited - virtual void updateLocalTransform() const; + virtual void updateLocalTransform() const override; // Documentation inherited - virtual void updateLocalJacobian(bool =true) const; + virtual void updateLocalJacobian(bool =true) const override; // Documentation inherited - virtual void updateLocalJacobianTimeDeriv() const; + virtual void updateLocalJacobianTimeDeriv() const override; protected: diff --git a/dart/dynamics/WeldJoint.h b/dart/dynamics/WeldJoint.h index f7c5f1f04db1c..291cae1cd3ccb 100644 --- a/dart/dynamics/WeldJoint.h +++ b/dart/dynamics/WeldJoint.h @@ -90,22 +90,22 @@ class WeldJoint : public ZeroDofJoint //---------------------------------------------------------------------------- // Documentation inherited - virtual void updateLocalTransform() const; + virtual void updateLocalTransform() const override; // Documentation inherited - virtual void updateLocalSpatialVelocity() const; + virtual void updateLocalSpatialVelocity() const override; // Documentation inherited - virtual void updateLocalSpatialAcceleration() const; + virtual void updateLocalSpatialAcceleration() const override; // Documentation inherited - virtual void updateLocalPrimaryAcceleration() const; + virtual void updateLocalPrimaryAcceleration() const override; // Documentation inherited - virtual void updateLocalJacobian(bool =true) const; + virtual void updateLocalJacobian(bool =true) const override; // Documentation inherited - virtual void updateLocalJacobianTimeDeriv() const; + virtual void updateLocalJacobianTimeDeriv() const override; public: // To get byte-aligned Eigen vectors diff --git a/dart/dynamics/ZeroDofJoint.h b/dart/dynamics/ZeroDofJoint.h index 32badc6a00c5a..14d9763dee5f6 100644 --- a/dart/dynamics/ZeroDofJoint.h +++ b/dart/dynamics/ZeroDofJoint.h @@ -99,179 +99,179 @@ class ZeroDofJoint : public Joint //---------------------------------------------------------------------------- // Documentation inherited - virtual void setCommand(size_t _index, double _command); + virtual void setCommand(size_t _index, double _command) override; // Documentation inherited - virtual double getCommand(size_t _index) const; + virtual double getCommand(size_t _index) const override; // Documentation inherited - virtual void setCommands(const Eigen::VectorXd& _commands); + virtual void setCommands(const Eigen::VectorXd& _commands) override; // Documentation inherited - virtual Eigen::VectorXd getCommands() const; + virtual Eigen::VectorXd getCommands() const override; // Documentation inherited - virtual void resetCommands(); + virtual void resetCommands() override; //---------------------------------------------------------------------------- // Position //---------------------------------------------------------------------------- // Documentation inherited - virtual void setPosition(size_t, double); + virtual void setPosition(size_t, double) override; // Documentation inherited - virtual double getPosition(size_t _index) const; + virtual double getPosition(size_t _index) const override; // Documentation inherited - virtual void setPositions(const Eigen::VectorXd& _positions); + virtual void setPositions(const Eigen::VectorXd& _positions) override; // Documentation inherited - virtual Eigen::VectorXd getPositions() const; + virtual Eigen::VectorXd getPositions() const override; // Documentation inherited - virtual void resetPositions(); + virtual void resetPositions() override; // Documentation inherited - virtual void setPositionLowerLimit(size_t _index, double _position); + virtual void setPositionLowerLimit(size_t _index, double _position) override; // Documentation inherited - virtual double getPositionLowerLimit(size_t _index) const; + virtual double getPositionLowerLimit(size_t _index) const override; // Documentation inherited - virtual void setPositionUpperLimit(size_t _index, double _position); + virtual void setPositionUpperLimit(size_t _index, double _position) override; // Documentation inherited - virtual double getPositionUpperLimit(size_t _index) const; + virtual double getPositionUpperLimit(size_t _index) const override; //---------------------------------------------------------------------------- // Velocity //---------------------------------------------------------------------------- // Documentation inherited - virtual void setVelocity(size_t _index, double _velocity); + virtual void setVelocity(size_t _index, double _velocity) override; // Documentation inherited - virtual double getVelocity(size_t _index) const; + virtual double getVelocity(size_t _index) const override; // Documentation inherited - virtual void setVelocities(const Eigen::VectorXd& _velocities); + virtual void setVelocities(const Eigen::VectorXd& _velocities) override; // Documentation inherited - virtual Eigen::VectorXd getVelocities() const; + virtual Eigen::VectorXd getVelocities() const override; // Documentation inherited - virtual void resetVelocities(); + virtual void resetVelocities() override; // Documentation inherited - virtual void setVelocityLowerLimit(size_t _index, double _velocity); + virtual void setVelocityLowerLimit(size_t _index, double _velocity) override; // Documentation inherited - virtual double getVelocityLowerLimit(size_t _index) const; + virtual double getVelocityLowerLimit(size_t _index) const override; // Documentation inherited - virtual void setVelocityUpperLimit(size_t _index, double _velocity); + virtual void setVelocityUpperLimit(size_t _index, double _velocity) override; // Documentation inherited - virtual double getVelocityUpperLimit(size_t _index) const; + virtual double getVelocityUpperLimit(size_t _index) const override; //---------------------------------------------------------------------------- // Acceleration //---------------------------------------------------------------------------- // Documentation inherited - virtual void setAcceleration(size_t _index, double _acceleration); + virtual void setAcceleration(size_t _index, double _acceleration) override; // Documentation inherited - virtual double getAcceleration(size_t _index) const; + virtual double getAcceleration(size_t _index) const override; // Documentation inherited - virtual void setAccelerations(const Eigen::VectorXd& _accelerations); + virtual void setAccelerations(const Eigen::VectorXd& _accelerations) override; // Documentation inherited - virtual Eigen::VectorXd getAccelerations() const; + virtual Eigen::VectorXd getAccelerations() const override; // Documentation inherited - virtual void resetAccelerations(); + virtual void resetAccelerations() override; // Documentation inherited - virtual void setAccelerationLowerLimit(size_t _index, double _acceleration); + virtual void setAccelerationLowerLimit(size_t _index, double _acceleration) override; // Documentation inherited - virtual double getAccelerationLowerLimit(size_t _index) const; + virtual double getAccelerationLowerLimit(size_t _index) const override; // Documentation inherited - virtual void setAccelerationUpperLimit(size_t _index, double _acceleration); + virtual void setAccelerationUpperLimit(size_t _index, double _acceleration) override; // Documentation inherited - virtual double getAccelerationUpperLimit(size_t _index) const; + virtual double getAccelerationUpperLimit(size_t _index) const override; //---------------------------------------------------------------------------- // Force //---------------------------------------------------------------------------- // Documentation inherited - virtual void setForce(size_t _index, double _force); + virtual void setForce(size_t _index, double _force) override; // Documentation inherited - virtual double getForce(size_t _index); + virtual double getForce(size_t _index) override; // Documentation inherited - virtual void setForces(const Eigen::VectorXd& _forces); + virtual void setForces(const Eigen::VectorXd& _forces) override; // Documentation inherited - virtual Eigen::VectorXd getForces() const; + virtual Eigen::VectorXd getForces() const override; // Documentation inherited - virtual void resetForces(); + virtual void resetForces() override; // Documentation inherited - virtual void setForceLowerLimit(size_t _index, double _force); + virtual void setForceLowerLimit(size_t _index, double _force) override; // Documentation inherited - virtual double getForceLowerLimit(size_t _index) const; + virtual double getForceLowerLimit(size_t _index) const override; // Documentation inherited - virtual void setForceUpperLimit(size_t _index, double _force); + virtual void setForceUpperLimit(size_t _index, double _force) override; // Documentation inherited - virtual double getForceUpperLimit(size_t _index) const; + virtual double getForceUpperLimit(size_t _index) const override; //---------------------------------------------------------------------------- // Velocity change //---------------------------------------------------------------------------- // Documentation inherited - virtual void setVelocityChange(size_t _index, double _velocityChange); + virtual void setVelocityChange(size_t _index, double _velocityChange) override; // Documentation inherited - virtual double getVelocityChange(size_t _index) const; + virtual double getVelocityChange(size_t _index) const override; // Documentation inherited - virtual void resetVelocityChanges(); + virtual void resetVelocityChanges() override; //---------------------------------------------------------------------------- // Constraint impulse //---------------------------------------------------------------------------- // Documentation inherited - virtual void setConstraintImpulse(size_t _index, double _impulse); + virtual void setConstraintImpulse(size_t _index, double _impulse) override; // Documentation inherited - virtual double getConstraintImpulse(size_t _index) const; + virtual double getConstraintImpulse(size_t _index) const override; // Documentation inherited - virtual void resetConstraintImpulses(); + virtual void resetConstraintImpulses() override; //---------------------------------------------------------------------------- // Integration and finite difference //---------------------------------------------------------------------------- // Documentation inherited - virtual void integratePositions(double _dt); + virtual void integratePositions(double _dt) override; // Documentation inherited - virtual void integrateVelocities(double _dt); + virtual void integrateVelocities(double _dt) override; // Documentation inherited Eigen::VectorXd getPositionDifferences( @@ -310,7 +310,7 @@ class ZeroDofJoint : public Joint //---------------------------------------------------------------------------- /// Get potential energy - virtual double getPotentialEnergy() const; + virtual double getPotentialEnergy() const override; // Documentation inherited virtual Eigen::Vector6d getBodyConstraintWrench() const override; @@ -324,7 +324,7 @@ class ZeroDofJoint : public Joint void registerDofs() override; // Documentation inherited - virtual void updateDegreeOfFreedomNames(); + virtual void updateDegreeOfFreedomNames() override; //---------------------------------------------------------------------------- /// \{ \name Recursive dynamics routines @@ -438,36 +438,36 @@ class ZeroDofJoint : public Joint virtual void addChildBiasForceForInvMassMatrix( Eigen::Vector6d& _parentBiasForce, const Eigen::Matrix6d& _childArtInertia, - const Eigen::Vector6d& _childBiasForce); + const Eigen::Vector6d& _childBiasForce) override; /// Add child's bias force to parent's one virtual void addChildBiasForceForInvAugMassMatrix( Eigen::Vector6d& _parentBiasForce, const Eigen::Matrix6d& _childArtInertia, - const Eigen::Vector6d& _childBiasForce); + const Eigen::Vector6d& _childBiasForce) override; /// virtual void updateTotalForceForInvMassMatrix( - const Eigen::Vector6d& _bodyForce); + const Eigen::Vector6d& _bodyForce) override; // Documentation inherited virtual void getInvMassMatrixSegment(Eigen::MatrixXd& _invMassMat, const size_t _col, const Eigen::Matrix6d& _artInertia, - const Eigen::Vector6d& _spatialAcc); + const Eigen::Vector6d& _spatialAcc) override; // Documentation inherited virtual void getInvAugMassMatrixSegment(Eigen::MatrixXd& _invMassMat, const size_t _col, const Eigen::Matrix6d& _artInertia, - const Eigen::Vector6d& _spatialAcc); + const Eigen::Vector6d& _spatialAcc) override; // Documentation inherited - virtual void addInvMassMatrixSegmentTo(Eigen::Vector6d& _acc); + virtual void addInvMassMatrixSegmentTo(Eigen::Vector6d& _acc) override; // Documentation inherited virtual Eigen::VectorXd getSpatialToGeneralized( - const Eigen::Vector6d& _spatial); + const Eigen::Vector6d& _spatial) override; /// \} diff --git a/dart/renderer/OpenGLRenderInterface.h b/dart/renderer/OpenGLRenderInterface.h index a93713c6f49bf..ac88e18ac243a 100644 --- a/dart/renderer/OpenGLRenderInterface.h +++ b/dart/renderer/OpenGLRenderInterface.h @@ -57,31 +57,31 @@ class OpenGLRenderInterface : public RenderInterface { OpenGLRenderInterface() {} virtual ~OpenGLRenderInterface(){} - virtual void initialize(); - virtual void destroy(); + virtual void initialize() override; + virtual void destroy() override; - virtual void setViewport(int _x,int _y,int _width,int _height); - virtual void getViewport(int& _x, int& _y, int& _width, int& _height) const; + virtual void setViewport(int _x,int _y,int _width,int _height) override; + virtual void getViewport(int& _x, int& _y, int& _width, int& _height) const override; - virtual void clear(const Eigen::Vector3d& _color); + virtual void clear(const Eigen::Vector3d& _color) override; - virtual void setDefaultLight(); - virtual void turnLightsOff(); - virtual void turnLightsOn(); + virtual void setDefaultLight() override; + virtual void turnLightsOff() override; + virtual void turnLightsOn() override; - virtual void setMaterial(const Eigen::Vector3d& _diffuse, const Eigen::Vector3d& _specular, double _cosinePow); - virtual void getMaterial(Eigen::Vector3d& _diffuse, Eigen::Vector3d& _specular, double& _cosinePow) const; - virtual void setDefaultMaterial(); + virtual void setMaterial(const Eigen::Vector3d& _diffuse, const Eigen::Vector3d& _specular, double _cosinePow) override; + virtual void getMaterial(Eigen::Vector3d& _diffuse, Eigen::Vector3d& _specular, double& _cosinePow) const override; + virtual void setDefaultMaterial() override; - virtual void pushMatrix(); - virtual void popMatrix(); - virtual void pushName(int _id); - virtual void popName(); + virtual void pushMatrix() override; + virtual void popMatrix() override; + virtual void pushName(int _id) override; + virtual void popName() override; - virtual void translate(const Eigen::Vector3d& _offset); //glTranslate - virtual void rotate(const Eigen::Vector3d& _axis, double _rad); //glRotate - virtual void transform(const Eigen::Isometry3d& _transform); //glMultMatrix - virtual void scale(const Eigen::Vector3d& _scale); //glScale + virtual void translate(const Eigen::Vector3d& _offset) override; //glTranslate + virtual void rotate(const Eigen::Vector3d& _axis, double _rad) override; //glRotate + virtual void transform(const Eigen::Isometry3d& _transform) override; //glMultMatrix + virtual void scale(const Eigen::Vector3d& _scale) override; //glScale void compileList(dynamics::Skeleton* _skel); void compileList(dynamics::BodyNode* _node); @@ -92,21 +92,21 @@ class OpenGLRenderInterface : public RenderInterface { virtual void draw(dynamics::BodyNode* _node, bool _vizCol = false, bool _colMesh = false); virtual void draw(dynamics::Shape* _shape); - virtual void drawEllipsoid(const Eigen::Vector3d& _size); - virtual void drawCube(const Eigen::Vector3d& _size); - virtual void drawCylinder(double _radius, double _height); - virtual void drawMesh(const Eigen::Vector3d& _scale, const aiScene* _mesh); - virtual void drawList(GLuint index); + virtual void drawEllipsoid(const Eigen::Vector3d& _size) override; + virtual void drawCube(const Eigen::Vector3d& _size) override; + virtual void drawCylinder(double _radius, double _height) override; + virtual void drawMesh(const Eigen::Vector3d& _scale, const aiScene* _mesh) override; + virtual void drawList(GLuint index) override; virtual void drawLineSegments(const std::vector& _vertices, const std::vector& _connections) override; - virtual void setPenColor(const Eigen::Vector4d& _col); - virtual void setPenColor(const Eigen::Vector3d& _col); + virtual void setPenColor(const Eigen::Vector4d& _col) override; + virtual void setPenColor(const Eigen::Vector3d& _col) override; - virtual void setLineWidth(float _width); + virtual void setLineWidth(float _width) override; - virtual void saveToImage(const char* _filename, DecoBufferType _buffType = BT_Back); - virtual void readFrameBuffer(DecoBufferType _buffType, DecoColorChannel _ch, void* _pixels); + virtual void saveToImage(const char* _filename, DecoBufferType _buffType = BT_Back) override; + virtual void readFrameBuffer(DecoBufferType _buffType, DecoColorChannel _ch, void* _pixels) override; private: void color4_to_float4(const aiColor4D *c, float f[4]); From 672efb89a27f911fbcef825fdb4478634b64a78c Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 12 Jun 2015 13:15:19 -0400 Subject: [PATCH 12/19] Don't try to suppress unused-variable warnings from Bullet on OSX --- dart/collision/bullet/BulletTypes.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dart/collision/bullet/BulletTypes.h b/dart/collision/bullet/BulletTypes.h index 47f324fce01eb..2430a485f1d90 100644 --- a/dart/collision/bullet/BulletTypes.h +++ b/dart/collision/bullet/BulletTypes.h @@ -43,8 +43,8 @@ namespace dart { namespace collision { -#if BT_BULLET_VERSION < 283 -/// Dummy structure to suppress unused-variable warning from bullet +#if !defined(__APPLE__) && BT_BULLET_VERSION < 283 +/// Dummy inline function to suppress unused-variable warning from bullet inline int btGetInfinityMask() { return btInfinityMask; From 32b2fcd33c3419bc26d0b387c3da9b4715d6ccbd Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 12 Jun 2015 13:17:05 -0400 Subject: [PATCH 13/19] Update comment: Disjointment --> Group --- dart/dynamics/ReferentialSkeleton.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dart/dynamics/ReferentialSkeleton.h b/dart/dynamics/ReferentialSkeleton.h index 536090c1c1a16..e0dadd8f80308 100644 --- a/dart/dynamics/ReferentialSkeleton.h +++ b/dart/dynamics/ReferentialSkeleton.h @@ -45,8 +45,8 @@ namespace dart { namespace dynamics { -/// ReferentialSkeleton is a base class used to implement Linkage, Disjointment, -/// and other classes that are used to reference subsections of Skeletons. +/// ReferentialSkeleton is a base class used to implement Linkage, Group, and +/// other classes that are used to reference subsections of Skeletons. class ReferentialSkeleton : public MetaSkeleton { public: From 6929f2ab55608b6c3763e8f43b974657011798de Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 12 Jun 2015 14:13:07 -0400 Subject: [PATCH 14/19] Fix warnings reported by cppcheck 1.67 --- apps/addDeleteSkels/MyWindow.h | 4 ---- apps/atlasSimbicon/Controller.cpp | 13 ++++++------ apps/atlasSimbicon/Controller.h | 6 +++--- apps/atlasSimbicon/State.cpp | 9 ++++---- apps/atlasSimbicon/StateMachine.cpp | 12 +++++------ apps/atlasSimbicon/StateMachine.h | 6 +++--- apps/jointConstraints/MyWindow.h | 3 ++- apps/rigidChain/MyWindow.cpp | 3 +-- apps/rigidCubes/MyWindow.h | 3 --- apps/rigidLoop/MyWindow.cpp | 3 +-- dart/collision/CollisionDetector.cpp | 7 +++--- dart/collision/CollisionDetector.h | 4 ++-- dart/collision/CollisionNode.cpp | 2 +- dart/common/Timer.cpp | 3 +++ dart/constraint/BallJointConstraint.cpp | 2 +- dart/constraint/ConstraintSolver.h | 5 +++++ dart/constraint/ContactConstraint.cpp | 2 +- dart/constraint/SoftContactConstraint.cpp | 2 +- dart/constraint/WeldJointConstraint.cpp | 2 +- dart/dynamics/BodyNode.cpp | 2 +- dart/dynamics/DegreeOfFreedom.cpp | 1 + dart/dynamics/PointMass.cpp | 2 +- dart/dynamics/Skeleton.cpp | 3 +-- dart/dynamics/SoftBodyNode.cpp | 6 +++--- dart/math/Geometry.cpp | 2 +- dart/optimizer/Problem.cpp | 8 ++++--- dart/optimizer/Problem.h | 4 ++-- dart/planning/Path.cpp | 20 ++++++++--------- dart/planning/Path.h | 2 +- dart/planning/PathFollowingTrajectory.cpp | 26 +++++++++++------------ dart/planning/PathFollowingTrajectory.h | 3 ++- dart/planning/PathShortener.cpp | 2 +- dart/planning/RRT.cpp | 2 +- dart/renderer/OpenGLRenderInterface.cpp | 4 ++-- dart/renderer/OpenGLRenderInterface.h | 2 +- dart/utils/C3D.cpp | 16 +++++++++++--- dart/utils/FileInfoC3D.cpp | 1 + dart/utils/FileInfoDof.cpp | 3 ++- dart/utils/FileInfoWorld.cpp | 1 + dart/utils/Parser.cpp | 1 - dart/utils/sdf/SdfParser.cpp | 3 +-- unittests/testDynamics.cpp | 2 +- unittests/testFileInfoWorld.cpp | 1 + unittests/testGeometry.cpp | 6 +++--- unittests/testNameManagement.cpp | 2 ++ 45 files changed, 116 insertions(+), 100 deletions(-) diff --git a/apps/addDeleteSkels/MyWindow.h b/apps/addDeleteSkels/MyWindow.h index 7ea83f4d4c896..8cab84fa13180 100644 --- a/apps/addDeleteSkels/MyWindow.h +++ b/apps/addDeleteSkels/MyWindow.h @@ -60,10 +60,6 @@ class MyWindow : public dart::gui::SimWindow { const Eigen::Vector3d& _position = Eigen::Vector3d(0.0, 1.0, 0.0), const Eigen::Vector3d& _size = Eigen::Vector3d(0.1, 0.1, 0.1), double _mass = 0.1); - - - /// \brief Number of frames for applying external force - int mImpulseDuration; }; #endif // APPS_ADDDELETESKELS_MYWINDOW_H_ diff --git a/apps/atlasSimbicon/Controller.cpp b/apps/atlasSimbicon/Controller.cpp index c62d64f594621..2709d8f6c160a 100644 --- a/apps/atlasSimbicon/Controller.cpp +++ b/apps/atlasSimbicon/Controller.cpp @@ -139,8 +139,7 @@ void Controller::changeStateMachine(const string& _name, double _currentTime) //============================================================================== void Controller::changeStateMachine(size_t _idx, double _currentTime) { - assert(0 <= _idx && _idx <= mStateMachines.size() - && "Invalid index of StateMachine."); + assert(_idx <= mStateMachines.size() && "Invalid index of StateMachine."); changeStateMachine(mStateMachines[_idx], _currentTime); } @@ -827,9 +826,9 @@ BodyNode* Controller::_getRightFoot() const } //============================================================================== -bool Controller::_containStateMachine(StateMachine* _stateMachine) +bool Controller::_containStateMachine(const StateMachine* _stateMachine) const { - for (vector::iterator it = mStateMachines.begin(); + for (vector::const_iterator it = mStateMachines.begin(); it != mStateMachines.end(); ++it) { if (*it == _stateMachine) @@ -840,17 +839,17 @@ bool Controller::_containStateMachine(StateMachine* _stateMachine) } //============================================================================== -bool Controller::_containStateMachine(const string& _name) +bool Controller::_containStateMachine(const string& _name) const { return _containStateMachine(_findStateMachine(_name)); } //============================================================================== -StateMachine* Controller::_findStateMachine(const string& _name) +StateMachine* Controller::_findStateMachine(const string& _name) const { StateMachine* stateMachine = NULL; - for (vector::iterator it = mStateMachines.begin(); + for (vector::const_iterator it = mStateMachines.begin(); it != mStateMachines.end(); ++it) { if ((*it)->getName() == _name) diff --git a/apps/atlasSimbicon/Controller.h b/apps/atlasSimbicon/Controller.h index 860dd223f5411..170cf45d329c1 100644 --- a/apps/atlasSimbicon/Controller.h +++ b/apps/atlasSimbicon/Controller.h @@ -126,14 +126,14 @@ class Controller private: /// \brief Check if this controller contains _stateMachine - bool _containStateMachine(StateMachine* _stateMachine); + bool _containStateMachine(const StateMachine* _stateMachine) const; /// \brief Check if this controller contains a state machine whose name is /// _name - bool _containStateMachine(const std::string& _name); + bool _containStateMachine(const std::string& _name) const; /// \brief Find a state machine whose name is _name - StateMachine* _findStateMachine(const std::string& _name); + StateMachine* _findStateMachine(const std::string& _name) const; /// \brief Build state machines void _buildStateMachines(); diff --git a/apps/atlasSimbicon/State.cpp b/apps/atlasSimbicon/State.cpp index dbd57be270ba7..a2bf3aa77be16 100644 --- a/apps/atlasSimbicon/State.cpp +++ b/apps/atlasSimbicon/State.cpp @@ -273,17 +273,16 @@ Eigen::Isometry3d State::getCOMFrame() const Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); // Y-axis - Eigen::Vector3d yAxis = Eigen::Vector3d::UnitY(); + const Eigen::Vector3d yAxis = Eigen::Vector3d::UnitY(); // X-axis - Eigen::Vector3d xAxis = mPelvis->getTransform().linear().col(0); Eigen::Vector3d pelvisXAxis = mPelvis->getTransform().linear().col(0); - double mag = yAxis.dot(pelvisXAxis); + const double mag = yAxis.dot(pelvisXAxis); pelvisXAxis -= mag * yAxis; - xAxis = pelvisXAxis.normalized(); + const Eigen::Vector3d xAxis = pelvisXAxis.normalized(); // Z-axis - Eigen::Vector3d zAxis = xAxis.cross(yAxis); + const Eigen::Vector3d zAxis = xAxis.cross(yAxis); T.translation() = getCOM(); diff --git a/apps/atlasSimbicon/StateMachine.cpp b/apps/atlasSimbicon/StateMachine.cpp index 6d3e827354b48..e13c5497657a4 100644 --- a/apps/atlasSimbicon/StateMachine.cpp +++ b/apps/atlasSimbicon/StateMachine.cpp @@ -180,15 +180,15 @@ void StateMachine::transiteTo(string& _stateName, double _currentTime) //============================================================================== void StateMachine::transiteTo(size_t _idx, double _currentTime) { - assert(0 <= _idx && _idx <= mStates.size() && "Invalid index of State."); + assert(_idx <= mStates.size() && "Invalid index of State."); transiteTo(mStates[_idx], _currentTime); } //============================================================================== -bool StateMachine::_containState(State* _state) +bool StateMachine::_containState(const State* _state) const { - for (vector::iterator it = mStates.begin(); + for (vector::const_iterator it = mStates.begin(); it != mStates.end(); ++it) { if (*it == _state) @@ -199,17 +199,17 @@ bool StateMachine::_containState(State* _state) } //============================================================================== -bool StateMachine::_containState(const string& _name) +bool StateMachine::_containState(const string& _name) const { return _containState(_findState(_name)); } //============================================================================== -State* StateMachine::_findState(const string& _name) +State* StateMachine::_findState(const string& _name) const { State* state = NULL; - for (vector::iterator it = mStates.begin(); + for (vector::const_iterator it = mStates.begin(); it != mStates.end(); ++it) { if ((*it)->getName() == _name) diff --git a/apps/atlasSimbicon/StateMachine.h b/apps/atlasSimbicon/StateMachine.h index 15dbd213c438c..90b6f29c90ade 100644 --- a/apps/atlasSimbicon/StateMachine.h +++ b/apps/atlasSimbicon/StateMachine.h @@ -120,13 +120,13 @@ class StateMachine private: /// \brief Check if this state machine contains _state - bool _containState(State* _state); + bool _containState(const State* _state) const; /// \brief Check if this state machine contains a state whose name is _name - bool _containState(const std::string& _name); + bool _containState(const std::string& _name) const; /// \brief Find a state whose name is _name - State* _findState(const std::string& _name); + State* _findState(const std::string& _name) const; }; #endif // APPS_ATLASROBOT_STATEMACHINE_H_ diff --git a/apps/jointConstraints/MyWindow.h b/apps/jointConstraints/MyWindow.h index 5e10412df699d..98b49b591da8e 100644 --- a/apps/jointConstraints/MyWindow.h +++ b/apps/jointConstraints/MyWindow.h @@ -50,7 +50,8 @@ class MyWindow : public dart::gui::SimWindow MyWindow(): SimWindow() { mForce = Eigen::Vector3d::Zero(); - mController = NULL; + mController = nullptr; + mWeldJoint = nullptr; mImpulseDuration = 0; mHarnessOn = false; diff --git a/apps/rigidChain/MyWindow.cpp b/apps/rigidChain/MyWindow.cpp index 92a1c189558b3..710f37fc25aae 100644 --- a/apps/rigidChain/MyWindow.cpp +++ b/apps/rigidChain/MyWindow.cpp @@ -53,9 +53,8 @@ void MyWindow::timeStepping() { Eigen::VectorXd MyWindow::computeDamping() { int nDof = mWorld->getSkeleton(0)->getNumDofs(); - Eigen::VectorXd damping = Eigen::VectorXd::Zero(nDof); // add damping to each joint; twist-dof has smaller damping - damping = -0.01 * mWorld->getSkeleton(0)->getVelocities(); + Eigen::VectorXd damping = -0.01 * mWorld->getSkeleton(0)->getVelocities(); for (int i = 0; i < nDof; i++) if (i % 3 == 1) damping[i] *= 0.1; diff --git a/apps/rigidCubes/MyWindow.h b/apps/rigidCubes/MyWindow.h index a285c945aba69..dc09b2aebf788 100644 --- a/apps/rigidCubes/MyWindow.h +++ b/apps/rigidCubes/MyWindow.h @@ -61,9 +61,6 @@ class MyWindow : public dart::gui::SimWindow { private: /// \brief Eigen::Vector3d mForce; - - /// \brief Number of frames for applying external force - int mImpulseDuration; }; #endif // APPS_CUBES_MYWINDOW_H_ diff --git a/apps/rigidLoop/MyWindow.cpp b/apps/rigidLoop/MyWindow.cpp index c4f6fb84e8c15..40fde6006b334 100644 --- a/apps/rigidLoop/MyWindow.cpp +++ b/apps/rigidLoop/MyWindow.cpp @@ -46,9 +46,8 @@ void MyWindow::timeStepping() Eigen::VectorXd MyWindow::computeDamping() { int nDof = mWorld->getSkeleton(0)->getNumDofs(); - Eigen::VectorXd damping = Eigen::VectorXd::Zero(nDof); // add damping to each joint; twist-dof has smaller damping - damping = -0.01 * mWorld->getSkeleton(0)->getVelocities(); + Eigen::VectorXd damping = -0.01 * mWorld->getSkeleton(0)->getVelocities(); for (int i = 0; i < nDof; i++) if (i % 3 == 1) damping[i] *= 0.1; diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index 2d31168631139..ede167c857d1d 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -259,7 +259,7 @@ bool CollisionDetector::isCollidable(const CollisionNode* _node1, } //============================================================================== -bool CollisionDetector::containSkeleton(const dynamics::SkeletonPtr _skeleton) +bool CollisionDetector::containSkeleton(const dynamics::SkeletonPtr& _skeleton) { for (std::vector::const_iterator it = mSkeletons.begin(); it != mSkeletons.end(); ++it) @@ -316,8 +316,9 @@ void CollisionDetector::setPairCollidable(const CollisionNode* _node1, mCollidablePairs[index1][index2] = _val; } -bool CollisionDetector::isAdjacentBodies(const dynamics::BodyNode* _bodyNode1, - const dynamics::BodyNode* _bodyNode2) +bool CollisionDetector::isAdjacentBodies( + const dynamics::BodyNode* _bodyNode1, + const dynamics::BodyNode* _bodyNode2) const { if ((_bodyNode1->getParentBodyNode() == _bodyNode2) || (_bodyNode2->getParentBodyNode() == _bodyNode1)) diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 8e0598ef9df77..25b4d77804421 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -180,7 +180,7 @@ class CollisionDetector private: /// \brief Return true if _skeleton is contained - bool containSkeleton(const dynamics::SkeletonPtr _skeleton); + bool containSkeleton(const dynamics::SkeletonPtr& _skeleton); /// \brief bool getPairCollidable(const CollisionNode* _node1, @@ -193,7 +193,7 @@ class CollisionDetector /// \brief Return true if _bodyNode1 and _bodyNode2 are adjacent bodies bool isAdjacentBodies(const dynamics::BodyNode* _bodyNode1, - const dynamics::BodyNode* _bodyNode2); + const dynamics::BodyNode* _bodyNode2) const; /// \brief CollisionNode* getCollisionNode(const dynamics::BodyNode* _bodyNode); diff --git a/dart/collision/CollisionNode.cpp b/dart/collision/CollisionNode.cpp index 9ef0355889339..77a1fb78cbe3f 100644 --- a/dart/collision/CollisionNode.cpp +++ b/dart/collision/CollisionNode.cpp @@ -41,7 +41,7 @@ namespace dart { namespace collision { CollisionNode::CollisionNode(dynamics::BodyNode* _bodyNode) - : mBodyNode(_bodyNode) { + : mBodyNode(_bodyNode), mIndex(0) { } CollisionNode::~CollisionNode() { diff --git a/dart/common/Timer.cpp b/dart/common/Timer.cpp index 390d97d561a0b..b8f08ad32f642 100644 --- a/dart/common/Timer.cpp +++ b/dart/common/Timer.cpp @@ -59,6 +59,9 @@ Timer::Timer(const std::string& _name) mTimer.start.QuadPart = 0; mTimer.stop.QuadPart = 0; QueryPerformanceFrequency(&mFrequency); +#else + mStartedTime = 0.0; + mStoppedTime = 0.0; #endif } diff --git a/dart/constraint/BallJointConstraint.cpp b/dart/constraint/BallJointConstraint.cpp index b69ab7f75a379..59354064d2901 100644 --- a/dart/constraint/BallJointConstraint.cpp +++ b/dart/constraint/BallJointConstraint.cpp @@ -166,7 +166,7 @@ void BallJointConstraint::getInformation(ConstraintInfo* _lcp) //============================================================================== void BallJointConstraint::applyUnitImpulse(size_t _index) { - assert(0 <= _index && _index < mDim && "Invalid Index."); + assert(_index < mDim && "Invalid Index."); assert(isActive()); if (mBodyNode2) diff --git a/dart/constraint/ConstraintSolver.h b/dart/constraint/ConstraintSolver.h index 12a8e8761702e..b6acac0bd2d3d 100644 --- a/dart/constraint/ConstraintSolver.h +++ b/dart/constraint/ConstraintSolver.h @@ -72,6 +72,11 @@ class ConstraintSolver /// Constructor explicit ConstraintSolver(double _timeStep); + /// Copy constructor + // TODO: implement copy constructor since this class contains a pointer to + // allocated memory. + ConstraintSolver(const ConstraintSolver& _other) = delete; + /// Destructor virtual ~ConstraintSolver(); diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 8cf2c90355709..504a74637f765 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -534,7 +534,7 @@ void ContactConstraint::getInformation(ConstraintInfo* _info) //============================================================================== void ContactConstraint::applyUnitImpulse(size_t _idx) { - assert(0 <= _idx && _idx < mDim && "Invalid Index."); + assert(_idx < mDim && "Invalid Index."); assert(isActive()); assert(mBodyNode1->isReactive() || mBodyNode2->isReactive()); diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp index 40c23952c15d0..1601bfd07193e 100644 --- a/dart/constraint/SoftContactConstraint.cpp +++ b/dart/constraint/SoftContactConstraint.cpp @@ -573,7 +573,7 @@ void SoftContactConstraint::getInformation(ConstraintInfo* _info) //============================================================================== void SoftContactConstraint::applyUnitImpulse(size_t _idx) { - assert(0 <= _idx && _idx < mDim && "Invalid Index."); + assert(_idx < mDim && "Invalid Index."); assert(isActive()); assert(mBodyNode1->isReactive() || mBodyNode2->isReactive()); diff --git a/dart/constraint/WeldJointConstraint.cpp b/dart/constraint/WeldJointConstraint.cpp index f1b69bf0a7313..5fd0a1a32265f 100644 --- a/dart/constraint/WeldJointConstraint.cpp +++ b/dart/constraint/WeldJointConstraint.cpp @@ -179,7 +179,7 @@ void WeldJointConstraint::getInformation(ConstraintInfo* _lcp) //============================================================================== void WeldJointConstraint::applyUnitImpulse(size_t _index) { - assert(0 <= _index && _index < mDim && "Invalid Index."); + assert(_index < mDim && "Invalid Index."); assert(isActive()); if (mBodyNode2) diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 68ea7068e80f0..9333b2dbc258b 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -789,7 +789,7 @@ size_t BodyNode::getNumDependentGenCoords() const //============================================================================== size_t BodyNode::getDependentGenCoordIndex(size_t _arrayIndex) const { - assert(0 <= _arrayIndex && _arrayIndex < mDependentGenCoordIndices.size()); + assert(_arrayIndex < mDependentGenCoordIndices.size()); return mDependentGenCoordIndices[_arrayIndex]; } diff --git a/dart/dynamics/DegreeOfFreedom.cpp b/dart/dynamics/DegreeOfFreedom.cpp index b29f6fb071dae..22eac286f6baa 100644 --- a/dart/dynamics/DegreeOfFreedom.cpp +++ b/dart/dynamics/DegreeOfFreedom.cpp @@ -447,6 +447,7 @@ DegreeOfFreedom::DegreeOfFreedom(Joint* _joint, size_t _indexInJoint) : mIndexInJoint(_indexInJoint), mIndexInSkeleton(0), + mIndexInTree(0), mJoint(_joint) { // Do nothing diff --git a/dart/dynamics/PointMass.cpp b/dart/dynamics/PointMass.cpp index 064da04f84a22..cafd288b7ab18 100644 --- a/dart/dynamics/PointMass.cpp +++ b/dart/dynamics/PointMass.cpp @@ -203,7 +203,7 @@ size_t PointMass::getNumConnectedPointMasses() const //============================================================================== PointMass* PointMass::getConnectedPointMass(size_t _idx) { - assert(0 <= _idx && _idx < getNumConnectedPointMasses()); + assert(_idx < getNumConnectedPointMasses()); return mParentSoftBodyNode->mPointMasses[ mParentSoftBodyNode->mSoftP.mPointProps[mIndex]. diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 0718e5142af4f..d65a620faebd8 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -634,7 +634,6 @@ void Skeleton::setState(const Eigen::VectorXd& _state) assert(_state.size() % 2 == 0); size_t index = 0; - size_t dof = 0; size_t halfSize = _state.size() / 2; Joint* joint; @@ -642,7 +641,7 @@ void Skeleton::setState(const Eigen::VectorXd& _state) { joint = mSkelCache.mBodyNodes[i]->getParentJoint(); - dof = joint->getNumDofs(); + const size_t dof = joint->getNumDofs(); if (dof) { diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index ea2c78267884c..43b7cf37883a5 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -409,8 +409,8 @@ PointMass* SoftBodyNode::addPointMass(const PointMass::Properties& _properties) void SoftBodyNode::connectPointMasses(size_t _idx1, size_t _idx2) { assert(_idx1 != _idx2); - assert(0 <= _idx1 && _idx1 < mPointMasses.size()); - assert(0 <= _idx2 && _idx2 < mPointMasses.size()); + assert(_idx1 < mPointMasses.size()); + assert(_idx2 < mPointMasses.size()); mPointMasses[_idx1]->addConnectedPointMass(mPointMasses[_idx2]); mPointMasses[_idx2]->addConnectedPointMass(mPointMasses[_idx1]); } @@ -424,7 +424,7 @@ void SoftBodyNode::addFace(const Eigen::Vector3i& _face) //============================================================================== const Eigen::Vector3i& SoftBodyNode::getFace(size_t _idx) const { - assert(0 <= _idx && _idx < mSoftP.mFaces.size()); + assert(_idx < mSoftP.mFaces.size()); return mSoftP.mFaces[_idx]; } diff --git a/dart/math/Geometry.cpp b/dart/math/Geometry.cpp index 10fc260e6bd66..e16d374c015a7 100644 --- a/dart/math/Geometry.cpp +++ b/dart/math/Geometry.cpp @@ -570,7 +570,6 @@ Eigen::Vector6d logMap(const Eigen::Isometry3d& _T) { std::acos( std::max( std::min(0.5 * (_T(0, 0) + _T(1, 1) + _T(2, 2) - 1.0), 1.0), -1.0)); - double alpha; double beta; double gamma; Eigen::Vector6d ret; @@ -604,6 +603,7 @@ Eigen::Vector6d logMap(const Eigen::Isometry3d& _T) { beta*_T(1, 3) - 0.5*(w[2]*_T(0, 3) - w[0]*_T(2, 3)) + gamma*w[1], beta*_T(2, 3) - 0.5*(w[0]*_T(1, 3) - w[1]*_T(0, 3)) + gamma*w[2]; } else { + double alpha; if (theta > DART_EPSILON) { alpha = 0.5*theta / sin(theta); beta = (1.0 + cos(theta))*alpha; diff --git a/dart/optimizer/Problem.cpp b/dart/optimizer/Problem.cpp index 2d3555c26200b..0ecd7aaefb837 100644 --- a/dart/optimizer/Problem.cpp +++ b/dart/optimizer/Problem.cpp @@ -49,7 +49,9 @@ namespace optimizer { //============================================================================== Problem::Problem(size_t _dim) - : mDimension(_dim) + : mDimension(_dim), + mObjective(nullptr), + mOptimumValue(0.0) { mInitialGuess = Eigen::VectorXd::Zero(mDimension); mLowerBounds = Eigen::VectorXd::Constant(mDimension, -HUGE_VAL); @@ -136,13 +138,13 @@ void Problem::addIneqConstraint(Function* _ineqConst) } //============================================================================== -size_t Problem::getNumEqConstraints() +size_t Problem::getNumEqConstraints() const { return mEqConstraints.size(); } //============================================================================== -size_t Problem::getNumIneqConstraints() +size_t Problem::getNumIneqConstraints() const { return mIneqConstraints.size(); } diff --git a/dart/optimizer/Problem.h b/dart/optimizer/Problem.h index 939feb828b954..95d27aa0958ae 100644 --- a/dart/optimizer/Problem.h +++ b/dart/optimizer/Problem.h @@ -96,10 +96,10 @@ class Problem void addIneqConstraint(Function* _ineqConst); /// \brief Get number of equality constraints - size_t getNumEqConstraints(); + size_t getNumEqConstraints() const; /// \brief Get number of inequality constraints - size_t getNumIneqConstraints(); + size_t getNumIneqConstraints() const; /// \brief Get equality constraint Function* getEqConstraint(size_t _idx) const; diff --git a/dart/planning/Path.cpp b/dart/planning/Path.cpp index 47241fe98a7e9..8ae5a4bab6c54 100644 --- a/dart/planning/Path.cpp +++ b/dart/planning/Path.cpp @@ -187,12 +187,12 @@ Path::Path(const list &path, double maxDeviation) : return; list::const_iterator config1 = path.begin(); list::const_iterator config2 = config1; - config2++; + ++config2; list::const_iterator config3; VectorXd startConfig = *config1; while(config2 != path.end()) { config3 = config2; - config3++; + ++config3; if(maxDeviation > 0.0 && config3 != path.end()) { CircularPathSegment* blendSegment = new CircularPathSegment(0.5 * (*config1 + *config2), *config2, 0.5 * (*config2 + *config3), maxDeviation); VectorXd endConfig = blendSegment->getConfig(0.0); @@ -216,14 +216,14 @@ Path::Path(const list &path, double maxDeviation) : startConfig = *config2; } config1 = config2; - config2++; + ++config2; } // create list of switching point candidates, calculate total path length and absolute positions of path segments - for(list::iterator segment = pathSegments.begin(); segment != pathSegments.end(); segment++) { + for(list::iterator segment = pathSegments.begin(); segment != pathSegments.end(); ++segment) { (*segment)->position = length; list localSwitchingPoints = (*segment)->getSwitchingPoints(); - for(list::const_iterator point = localSwitchingPoints.begin(); point != localSwitchingPoints.end(); point++) { + for(list::const_iterator point = localSwitchingPoints.begin(); point != localSwitchingPoints.end(); ++point) { switchingPoints.push_back(make_pair(length + *point, false)); } length += (*segment)->getLength(); @@ -236,13 +236,13 @@ Path::Path(const Path &path) : length(path.length), switchingPoints(path.switchingPoints) { - for(list::const_iterator it = path.pathSegments.begin(); it != path.pathSegments.end(); it++) { + for(list::const_iterator it = path.pathSegments.begin(); it != path.pathSegments.end(); ++it) { pathSegments.push_back((*it)->clone()); } } Path::~Path() { - for(list::iterator it = pathSegments.begin(); it != pathSegments.end(); it++) { + for(list::iterator it = pathSegments.begin(); it != pathSegments.end(); ++it) { delete *it; } } @@ -254,10 +254,10 @@ double Path::getLength() const { PathSegment* Path::getPathSegment(double &s) const { list::const_iterator it = pathSegments.begin(); list::const_iterator next = it; - next++; + ++next; while(next != pathSegments.end() && s >= (*next)->position) { it = next; - next++; + ++next; } s -= (*it)->position; return *it; @@ -281,7 +281,7 @@ VectorXd Path::getCurvature(double s) const { double Path::getNextSwitchingPoint(double s, bool &discontinuity) const { list >::const_iterator it = switchingPoints.begin(); while(it != switchingPoints.end() && it->first <= s) { - it++; + ++it; } if(it == switchingPoints.end()) { discontinuity = true; diff --git a/dart/planning/Path.h b/dart/planning/Path.h index f9361adee71ac..23b478deb8ec8 100644 --- a/dart/planning/Path.h +++ b/dart/planning/Path.h @@ -48,7 +48,7 @@ class PathSegment { public: PathSegment(double length = 0.0) : - length(length) + position(0.0), length(length) { } diff --git a/dart/planning/PathFollowingTrajectory.cpp b/dart/planning/PathFollowingTrajectory.cpp index efc663401c077..568a189d5510e 100644 --- a/dart/planning/PathFollowingTrajectory.cpp +++ b/dart/planning/PathFollowingTrajectory.cpp @@ -100,11 +100,11 @@ PathFollowingTrajectory::PathFollowingTrajectory(const Path &path, const VectorX list::iterator previous = trajectory.begin(); list::iterator it = previous; it->time = 0.0; - it++; + ++it; while(it != trajectory.end()) { it->time = previous->time + (it->pathPos - previous->pathPos) / ((it->pathVel + previous->pathVel) / 2.0); previous = it; - it++; + ++it; } // debug @@ -248,8 +248,8 @@ bool PathFollowingTrajectory::integrateForward(list &trajectory, // int test = 48; } - while(nextDiscontinuity != switchingPoints.end() && (nextDiscontinuity->first <= pathPos || !nextDiscontinuity->second)) { - nextDiscontinuity++; + while(nextDiscontinuity != switchingPoints.end() && (nextDiscontinuity->first <= pathPos || !nextDiscontinuity->second)) { + ++nextDiscontinuity; } double oldPathPos = pathPos; @@ -355,7 +355,7 @@ void PathFollowingTrajectory::integrateBackward(list &trajectory } while(before != startTrajectory.rend() && before->pathPos > pathPos) { - before++; + ++before; } bool error = false; @@ -411,10 +411,10 @@ void PathFollowingTrajectory::integrateBackward(list &trajectory if(error) { ofstream file("trajectory.txt"); - for(list::iterator it = startTrajectory.begin(); it != startTrajectory.end(); it++) { + for(list::iterator it = startTrajectory.begin(); it != startTrajectory.end(); ++it) { file << it->pathPos << " " << it->pathVel << endl; } - for(list::iterator it = trajectory.begin(); it != trajectory.end(); it++) { + for(list::iterator it = trajectory.begin(); it != trajectory.end(); ++it) { file << it->pathPos << " " << it->pathVel << endl; } file.close(); @@ -431,7 +431,7 @@ inline double PathFollowingTrajectory::getSlope(const TrajectoryStep &point1, co inline double PathFollowingTrajectory::getSlope(list::const_iterator lineEnd) { list::const_iterator lineStart = lineEnd; - lineStart--; + --lineStart; return getSlope(*lineStart, *lineEnd); } @@ -544,7 +544,7 @@ double PathFollowingTrajectory::getDuration() const { list::const_iterator PathFollowingTrajectory::getTrajectorySegment(double time) const { if(time >= trajectory.back().time) { list::const_iterator last = trajectory.end(); - last--; + --last; return last; } else { @@ -552,7 +552,7 @@ list::const_iterator PathFollowingTraje cachedTrajectorySegment = trajectory.begin(); } while(time >= cachedTrajectorySegment->time) { - cachedTrajectorySegment++; + ++cachedTrajectorySegment; } cachedTime = time; return cachedTrajectorySegment; @@ -562,7 +562,7 @@ list::const_iterator PathFollowingTraje VectorXd PathFollowingTrajectory::getPosition(double time) const { list::const_iterator it = getTrajectorySegment(time); list::const_iterator previous = it; - previous--; + --previous; //const double pathPos = previous->pathPos + (time - previous->time) * (previous->pathVel + it->pathVel) / 2.0; @@ -578,7 +578,7 @@ VectorXd PathFollowingTrajectory::getPosition(double time) const { VectorXd PathFollowingTrajectory::getVelocity(double time) const { list::const_iterator it = getTrajectorySegment(time); list::const_iterator previous = it; - previous--; + --previous; //const double pathPos = previous->pathPos + (time - previous->time) * (previous->pathVel + it->pathVel) / 2.0; @@ -598,7 +598,7 @@ double PathFollowingTrajectory::getMaxAccelerationError() { for(double time = 0.0; time < getDuration(); time += 0.000001) { list::const_iterator it = getTrajectorySegment(time); list::const_iterator previous = it; - previous--; + --previous; double timeStep = it->time - previous->time; const double pathAcceleration = (it->pathPos - previous->pathPos - timeStep * previous->pathVel) / (timeStep * timeStep); diff --git a/dart/planning/PathFollowingTrajectory.h b/dart/planning/PathFollowingTrajectory.h index a22e2895d5794..e00f67b86f212 100644 --- a/dart/planning/PathFollowingTrajectory.h +++ b/dart/planning/PathFollowingTrajectory.h @@ -62,7 +62,8 @@ class PathFollowingTrajectory : public Trajectory TrajectoryStep() {} TrajectoryStep(double pathPos, double pathVel) : pathPos(pathPos), - pathVel(pathVel) + pathVel(pathVel), + time(0.0) {} double pathPos; double pathVel; diff --git a/dart/planning/PathShortener.cpp b/dart/planning/PathShortener.cpp index a54d39d7b26dd..ec85671e9b9a2 100644 --- a/dart/planning/PathShortener.cpp +++ b/dart/planning/PathShortener.cpp @@ -58,7 +58,7 @@ void PathShortener::shortenPath(list &path) list intermediatePoints; if(localPlanner(intermediatePoints, node1Iter, node2Iter)) { list::iterator node1NextIter = node1Iter; - node1NextIter++; + ++node1NextIter; path.erase(node1NextIter, node2Iter); path.splice(node2Iter, intermediatePoints); } diff --git a/dart/planning/RRT.cpp b/dart/planning/RRT.cpp index b3c7b8630ea05..d16f45592f94d 100644 --- a/dart/planning/RRT.cpp +++ b/dart/planning/RRT.cpp @@ -139,7 +139,7 @@ RRT::StepResult RRT::tryStepFromNode(const VectorXd &qtry, int NNidx) { // Add the intermediate nodes and the final new node to the tree list ::iterator it = intermediatePoints.begin(); - for(; it != intermediatePoints.end(); it++) + for(; it != intermediatePoints.end(); ++it) NNidx = addNode(*it, NNidx); addNode(qnew, NNidx); return STEP_PROGRESS; diff --git a/dart/renderer/OpenGLRenderInterface.cpp b/dart/renderer/OpenGLRenderInterface.cpp index 5cbc68162c2de..be3550c7e3802 100644 --- a/dart/renderer/OpenGLRenderInterface.cpp +++ b/dart/renderer/OpenGLRenderInterface.cpp @@ -268,7 +268,7 @@ void OpenGLRenderInterface::applyMaterial(const struct aiMaterial *mtl) float c[4]; GLenum fill_mode; - int ret1, ret2; + int ret1; aiColor4D diffuse; aiColor4D specular; aiColor4D ambient; @@ -302,7 +302,7 @@ void OpenGLRenderInterface::applyMaterial(const struct aiMaterial *mtl) ret1 = aiGetMaterialFloatArray(mtl, AI_MATKEY_SHININESS, &shininess, &max); if(ret1 == AI_SUCCESS) { max = 1; - ret2 = aiGetMaterialFloatArray(mtl, AI_MATKEY_SHININESS_STRENGTH, &strength, &max); + const int ret2 = aiGetMaterialFloatArray(mtl, AI_MATKEY_SHININESS_STRENGTH, &strength, &max); if(ret2 == AI_SUCCESS) glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, shininess * strength); else diff --git a/dart/renderer/OpenGLRenderInterface.h b/dart/renderer/OpenGLRenderInterface.h index ac88e18ac243a..ab352a56ad0b5 100644 --- a/dart/renderer/OpenGLRenderInterface.h +++ b/dart/renderer/OpenGLRenderInterface.h @@ -54,7 +54,7 @@ namespace renderer { class OpenGLRenderInterface : public RenderInterface { public: - OpenGLRenderInterface() {} + OpenGLRenderInterface() : mViewportX(0.0), mViewportY(0.0), mViewportWidth(0.0), mViewportHeight(0.0) {} virtual ~OpenGLRenderInterface(){} virtual void initialize() override; diff --git a/dart/utils/C3D.cpp b/dart/utils/C3D.cpp index 3154cf2008610..170bdcb41172e 100644 --- a/dart/utils/C3D.cpp +++ b/dart/utils/C3D.cpp @@ -87,13 +87,19 @@ bool loadC3DFile(const char* _fileName, Eigen::EIGEN_VV_VEC3D& _pointData, int* //get the header if (!fread(buf, C3D_REC_SIZE, 1, file)) + { + fclose(file); return false; + } memcpy(&hdr, buf, sizeof(hdr)); //get number format if (hdr.rec_start > 2) { if (!fread(buf, C3D_REC_SIZE, 1, file)) + { + fclose(file); return false; + } memcpy(¶m, buf, sizeof(param)); if (param.ftype == 84) bDecFmt = false; @@ -118,6 +124,7 @@ bool loadC3DFile(const char* _fileName, Eigen::EIGEN_VV_VEC3D& _pointData, int* //eat parameter records for (int i = 3; i < hdr.rec_start; i++) { if (!fread(buf, C3D_REC_SIZE, 1, file)) { + fclose(file); return false; } } @@ -135,7 +142,10 @@ bool loadC3DFile(const char* _fileName, Eigen::EIGEN_VV_VEC3D& _pointData, int* _pointData[i].resize(numMarkers); for (int j = 0; j < numMarkers; j++) { if (!fread(buf, iRecSize, 1, file)) + { + fclose(file); return false; + } if (c3dScale < 0) { memcpy(&frame, buf, sizeof(frame)); if(bDecFmt){ @@ -162,9 +172,9 @@ bool loadC3DFile(const char* _fileName, Eigen::EIGEN_VV_VEC3D& _pointData, int* } fclose(file); - const char *pch = strrchr(_fileName, '\\'); //clip leading path - if (pch) - _fileName = pch + 1; + //const char *pch = strrchr(_fileName, '\\'); //clip leading path + //if (pch) + // _fileName = pch + 1; return true; } diff --git a/dart/utils/FileInfoC3D.cpp b/dart/utils/FileInfoC3D.cpp index a032947e09c93..180b92759f13c 100644 --- a/dart/utils/FileInfoC3D.cpp +++ b/dart/utils/FileInfoC3D.cpp @@ -44,6 +44,7 @@ namespace utils { FileInfoC3D::FileInfoC3D() : mNumMarkers(0), mNumFrames(0), mFPS(0){ + std::strcpy(mFileName, ""); } bool FileInfoC3D::loadFile(const char* _fName) diff --git a/dart/utils/FileInfoDof.cpp b/dart/utils/FileInfoDof.cpp index f965c9351addb..dc1e5cb5287bf 100644 --- a/dart/utils/FileInfoDof.cpp +++ b/dart/utils/FileInfoDof.cpp @@ -51,6 +51,7 @@ namespace utils { FileInfoDof::FileInfoDof(dynamics::Skeleton* _skel, double _fps) : mSkel(_skel), mFPS(_fps), mNumFrames(0) { + std::strcpy(mFileName, ""); } //============================================================================== @@ -166,7 +167,7 @@ void FileInfoDof::addDof(const Eigen::VectorXd& _dofs) //============================================================================== double FileInfoDof::getDofAt(size_t _frame, size_t _id) const { - assert(_frame>=0 && _framesetName(name); + newWorld->setName(name); //-------------------------------------------------------------------------- // Load physics diff --git a/unittests/testDynamics.cpp b/unittests/testDynamics.cpp index 3fdf5e1cc943c..84df4bf230326 100644 --- a/unittests/testDynamics.cpp +++ b/unittests/testDynamics.cpp @@ -1149,7 +1149,7 @@ void DynamicsTest::compareEquationsOfMotion(const std::string& _fileName) } //============================================================================== -void compareCOMJacobianToFk(const SkeletonPtr skel, +void compareCOMJacobianToFk(const SkeletonPtr& skel, const Frame* refFrame, double tolerance) { diff --git a/unittests/testFileInfoWorld.cpp b/unittests/testFileInfoWorld.cpp index 092e814aa04cc..14faf4ffa39e1 100644 --- a/unittests/testFileInfoWorld.cpp +++ b/unittests/testFileInfoWorld.cpp @@ -85,6 +85,7 @@ TEST(FileInfoWorld, Basic) // Load the file result = worldFile.loadFile(fileName.c_str()); + EXPECT_TRUE(result); recording2 = worldFile.getRecording(); EXPECT_TRUE(recording2 != nullptr); diff --git a/unittests/testGeometry.cpp b/unittests/testGeometry.cpp index 63033edb298a8..eda4116c93471 100644 --- a/unittests/testGeometry.cpp +++ b/unittests/testGeometry.cpp @@ -405,7 +405,7 @@ TEST(LIE_GROUP_OPERATORS, ADJOINT_MAPPINGS) Eigen::Vector6d t = Eigen::Vector6d::Random(); Eigen::Isometry3d T = math::expMap(t); Eigen::Isometry3d R = Eigen::Isometry3d::Identity(); - R = T.linear(); + R.linear() = T.linear(); Eigen::Vector6d V = Eigen::Vector6d::Random(); Eigen::Vector6d AdTV = AdT(R, V); @@ -487,7 +487,7 @@ TEST(LIE_GROUP_OPERATORS, ADJOINT_MAPPINGS) Eigen::Vector6d V = Eigen::Vector6d::Zero(); V.tail<3>() = v; Eigen::Isometry3d R = Eigen::Isometry3d::Identity(); - R = T.linear(); + R.linear() = T.linear(); Eigen::Vector6d AdT_ = AdT(R.inverse(), V); Eigen::Vector6d AdInvRLinear_ = AdInvRLinear(T, v); @@ -548,7 +548,7 @@ TEST(LIE_GROUP_OPERATORS, ADJOINT_MAPPINGS) Eigen::Isometry3d T = math::expMap(t); Eigen::Isometry3d InvT = T.inverse(); Eigen::Isometry3d InvR = Eigen::Isometry3d::Identity(); - InvR = InvT.linear(); + InvR.linear() = InvT.linear(); Eigen::Vector6d F = Eigen::Vector6d::Random(); Eigen::Vector6d dAdInvR_F = dAdInvR(T, F); diff --git a/unittests/testNameManagement.cpp b/unittests/testNameManagement.cpp index 86c67c16a5060..d3eabae9181eb 100644 --- a/unittests/testNameManagement.cpp +++ b/unittests/testNameManagement.cpp @@ -125,6 +125,7 @@ TEST(NameManagement, Skeleton) // Testing whether unique names get accidentally changed by the NameManager std::string unique_name = body2->setName("a_unique_name"); + EXPECT_TRUE(body2->getName() == unique_name); EXPECT_TRUE(body2->getName() == "a_unique_name"); EXPECT_TRUE(skel->getBodyNode("a_unique_name") == body2); @@ -133,6 +134,7 @@ TEST(NameManagement, Skeleton) EXPECT_FALSE(body3->getName() == body1->getName()); unique_name = joint3->setName("a_unique_name"); + EXPECT_TRUE(joint3->getName() == unique_name); EXPECT_TRUE(joint3->getName() == "a_unique_name"); EXPECT_TRUE(skel->getJoint("a_unique_name") == joint3); From 14cae5890db90c224ecc90fbbc62b0b472e30349 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 12 Jun 2015 16:29:00 -0400 Subject: [PATCH 15/19] Replace NULL with nullptr --- apps/addDeleteSkels/Main.cpp | 2 +- apps/atlasSimbicon/Controller.cpp | 14 +++---- apps/atlasSimbicon/Humanoid.cpp | 22 +++++----- apps/atlasSimbicon/State.cpp | 28 ++++++------- apps/atlasSimbicon/StateMachine.cpp | 12 +++--- apps/atlasSimbicon/TerminalCondition.cpp | 6 +-- apps/bipedStand/MyWindow.cpp | 2 +- apps/hybridDynamics/Main.cpp | 2 +- apps/jointConstraints/Main.cpp | 2 +- apps/operationalSpaceControl/Controller.cpp | 4 +- apps/operationalSpaceControl/MyWindow.cpp | 2 +- apps/rigidChain/Main.cpp | 2 +- apps/rigidCubes/Main.cpp | 2 +- apps/rigidLoop/Main.cpp | 2 +- apps/softBodies/Main.cpp | 2 +- apps/speedTest/Main.cpp | 2 +- dart/collision/CollisionDetector.cpp | 14 +++---- .../bullet/BulletCollisionDetector.cpp | 6 +-- dart/collision/dart/DARTCollisionDetector.cpp | 4 +- dart/collision/fcl/FCLCollisionDetector.cpp | 6 +-- dart/collision/fcl_mesh/CollisionShapes.h | 2 +- .../fcl_mesh/FCLMeshCollisionDetector.cpp | 8 ++-- dart/common/Timer.cpp | 8 ++-- dart/constraint/BallJointConstraint.cpp | 8 ++-- dart/constraint/ConstrainedGroup.cpp | 4 +- dart/constraint/ConstraintSolver.cpp | 6 +-- dart/constraint/ContactConstraint.cpp | 4 +- dart/constraint/JointConstraint.cpp | 2 +- .../JointCoulombFrictionConstraint.cpp | 2 +- dart/constraint/JointLimitConstraint.cpp | 2 +- dart/constraint/SoftContactConstraint.cpp | 10 ++--- dart/constraint/WeldJointConstraint.cpp | 6 +-- dart/dynamics/BodyNode.cpp | 16 ++++---- dart/dynamics/BodyNode.h | 2 +- dart/dynamics/BoxShape.h | 2 +- dart/dynamics/CylinderShape.h | 2 +- dart/dynamics/EllipsoidShape.h | 2 +- dart/dynamics/Entity.cpp | 4 +- dart/dynamics/Entity.h | 4 +- dart/dynamics/Frame.cpp | 6 +-- dart/dynamics/Frame.h | 2 +- dart/dynamics/Marker.h | 2 +- dart/dynamics/MeshShape.cpp | 2 +- dart/dynamics/MeshShape.h | 2 +- dart/dynamics/PlaneShape.h | 2 +- dart/dynamics/PointMass.cpp | 8 ++-- dart/dynamics/PointMass.h | 2 +- dart/dynamics/Shape.h | 2 +- dart/dynamics/Skeleton.h | 4 +- dart/dynamics/SoftBodyNode.cpp | 12 +++--- dart/dynamics/SoftBodyNode.h | 2 +- dart/dynamics/SoftMeshShape.cpp | 4 +- dart/dynamics/SoftMeshShape.h | 2 +- dart/gui/GlutWindow.cpp | 2 +- dart/gui/lodepng.cpp | 26 ++++++------ dart/gui/lodepng.h | 4 +- dart/lcpsolver/lcp.cpp | 12 +++--- dart/lcpsolver/matrix.cpp | 12 +++--- dart/optimizer/Problem.cpp | 2 +- dart/optimizer/ipopt/IpoptSolver.cpp | 8 ++-- dart/optimizer/ipopt/IpoptSolver.h | 8 ++-- dart/optimizer/nlopt/NloptSolver.h | 4 +- dart/planning/PathPlanner.h | 2 +- dart/planning/PathShortener.cpp | 2 +- dart/planning/RRT.cpp | 4 +- dart/renderer/OpenGLRenderInterface.cpp | 6 +-- dart/simulation/World.cpp | 2 +- dart/utils/C3D.cpp | 4 +- dart/utils/FileInfoDof.cpp | 2 +- dart/utils/FileInfoWorld.cpp | 2 +- dart/utils/Parser.cpp | 40 +++++++++---------- dart/utils/SkelParser.cpp | 1 - dart/utils/sdf/SdfParser.cpp | 6 +-- unittests/testCollision.cpp | 26 ++++++------ unittests/testConstraint.cpp | 2 +- unittests/testDynamics.cpp | 16 ++++---- unittests/testInverseKinematics.cpp | 1 - unittests/testJoints.cpp | 8 ++-- unittests/testNameManagement.cpp | 8 ++-- unittests/testSoftDynamics.cpp | 12 +++--- 80 files changed, 251 insertions(+), 253 deletions(-) diff --git a/apps/addDeleteSkels/Main.cpp b/apps/addDeleteSkels/Main.cpp index 7a7b01a7799d3..b31b0bd68b4ec 100644 --- a/apps/addDeleteSkels/Main.cpp +++ b/apps/addDeleteSkels/Main.cpp @@ -46,7 +46,7 @@ int main(int argc, char* argv[]) { dart::simulation::WorldPtr myWorld = dart::utils::SkelParser::readWorld( DART_DATA_PATH"/skel/ground.skel"); - assert(myWorld != NULL); + assert(myWorld != nullptr); Eigen::Vector3d gravity(0.0, -9.81, 0.0); myWorld->setGravity(gravity); diff --git a/apps/atlasSimbicon/Controller.cpp b/apps/atlasSimbicon/Controller.cpp index 2709d8f6c160a..e8ece408cc355 100644 --- a/apps/atlasSimbicon/Controller.cpp +++ b/apps/atlasSimbicon/Controller.cpp @@ -53,13 +53,13 @@ Controller::Controller(SkeletonPtr _atlasRobot, ConstraintSolver* _collisionSolver) : mAtlasRobot(_atlasRobot), mConstratinSolver(_collisionSolver), - mCurrentStateMachine(NULL), + mCurrentStateMachine(nullptr), mPelvisHarnessOn(false), mLeftFootHarnessOn(false), mRightFootHarnessOn(false), - mWeldJointConstraintPelvis(NULL), - mWeldJointConstraintLeftFoot(NULL), - mWeldJointConstraintRightFoot(NULL) + mWeldJointConstraintPelvis(nullptr), + mWeldJointConstraintLeftFoot(nullptr), + mWeldJointConstraintRightFoot(nullptr) { _buildStateMachines(); _setJointDamping(); @@ -131,7 +131,7 @@ void Controller::changeStateMachine(const string& _name, double _currentTime) // _state should be in mStates StateMachine* stateMachine = _findStateMachine(_name); - assert(stateMachine != NULL && "Invaild state machine."); + assert(stateMachine != nullptr && "Invaild state machine."); changeStateMachine(stateMachine, _currentTime); } @@ -210,7 +210,7 @@ void Controller::printDebugInfo() const << joint->getName() << " (" << joint->getNumDofs() << ")" << std::endl; - if (parentBody != NULL) + if (parentBody != nullptr) { std::cout << " Parent body: " << parentBody->getName() << std::endl; } @@ -847,7 +847,7 @@ bool Controller::_containStateMachine(const string& _name) const //============================================================================== StateMachine* Controller::_findStateMachine(const string& _name) const { - StateMachine* stateMachine = NULL; + StateMachine* stateMachine = nullptr; for (vector::const_iterator it = mStateMachines.begin(); it != mStateMachines.end(); ++it) diff --git a/apps/atlasSimbicon/Humanoid.cpp b/apps/atlasSimbicon/Humanoid.cpp index d7b846656118d..1e164d8627e08 100644 --- a/apps/atlasSimbicon/Humanoid.cpp +++ b/apps/atlasSimbicon/Humanoid.cpp @@ -52,12 +52,12 @@ using namespace dart::constraint; //============================================================================== Humanoid::Humanoid(Skeleton* _skeleton) - : mSkeleton(NULL), - mPelvis(NULL), - mLeftThigh(NULL), - mRightThigh(NULL), - mLeftFoot(NULL), - mRightFoot(NULL) + : mSkeleton(nullptr), + mPelvis(nullptr), + mLeftThigh(nullptr), + mRightThigh(nullptr), + mLeftFoot(nullptr), + mRightFoot(nullptr) { } @@ -120,9 +120,9 @@ AtlasRobot::~AtlasRobot() mLeftThigh = mSkeleton->getBodyNode("l_uleg"); mRightThigh = mSkeleton->getBodyNode("r_uleg"); - assert(mPelvis != NULL); - assert(mLeftFoot != NULL); - assert(mRightFoot != NULL); - assert(mLeftThigh != NULL); - assert(mRightThigh != NULL); + assert(mPelvis != nullptr); + assert(mLeftFoot != nullptr); + assert(mRightFoot != nullptr); + assert(mLeftThigh != nullptr); + assert(mRightThigh != nullptr); } diff --git a/apps/atlasSimbicon/State.cpp b/apps/atlasSimbicon/State.cpp index a2bf3aa77be16..006f552bf1f0d 100644 --- a/apps/atlasSimbicon/State.cpp +++ b/apps/atlasSimbicon/State.cpp @@ -127,14 +127,14 @@ State::State(SkeletonPtr _skeleton, const std::string& _name) mRightFoot = mSkeleton->getBodyNode("r_foot"); mLeftThigh = mSkeleton->getBodyNode("l_uleg"); mRightThigh = mSkeleton->getBodyNode("r_uleg"); - mStanceFoot = NULL; + mStanceFoot = nullptr; - assert(mPelvis != NULL); - assert(mLeftFoot != NULL); - assert(mRightFoot != NULL); - assert(mLeftThigh != NULL); - assert(mRightThigh != NULL); -// assert(mStanceFoot != NULL); + assert(mPelvis != nullptr); + assert(mLeftFoot != nullptr); + assert(mRightFoot != nullptr); + assert(mLeftThigh != nullptr); + assert(mRightThigh != nullptr); +// assert(mStanceFoot != nullptr); } //============================================================================== @@ -164,7 +164,7 @@ void State::setNextState(State* _nextState) //============================================================================== void State::setTerminalCondition(TerminalCondition* _condition) { - assert(_condition != NULL); + assert(_condition != nullptr); mTerminalCondition = _condition; } @@ -180,7 +180,7 @@ void State::begin(double _currentTime) //============================================================================== void State::computeControlForce(double _timestep) { - assert(mNextState != NULL && "Next state should be set."); + assert(mNextState != nullptr && "Next state should be set."); int dof = mSkeleton->getNumDofs(); VectorXd q = mSkeleton->getPositions(mDofMapping); @@ -244,7 +244,7 @@ void State::computeControlForce(double _timestep) //============================================================================== bool State::isTerminalConditionSatisfied() const { - assert(mTerminalCondition != NULL && "Invalid terminal condition."); + assert(mTerminalCondition != nullptr && "Invalid terminal condition."); return mTerminalCondition->isSatisfied(); } @@ -332,7 +332,7 @@ double State::getCoronalCOMVelocity() //============================================================================== Eigen::Vector3d State::getStanceAnklePosition() const { - if (mStanceFoot == NULL) + if (mStanceFoot == nullptr) return getCOM(); else return _getJointPosition(mStanceFoot); @@ -589,7 +589,7 @@ void State::setDesiredJointPosition(const string& _jointName, double _val) // TODO(JS) NOT_YET(State::setDesiredJointPosition()); - assert(mSkeleton->getJoint(_jointName) != NULL); + assert(mSkeleton->getJoint(_jointName) != nullptr); mDesiredJointPositions[mJointMap[_jointName]] = _val; } @@ -609,7 +609,7 @@ double State::getDesiredJointPosition(const string& _jointName) const // TODO(JS) NOT_YET(State::getDesiredJointPosition()); - assert(mSkeleton->getJoint(_jointName) != NULL); + assert(mSkeleton->getJoint(_jointName) != nullptr); return mDesiredJointPositions[mJointMap.at(_jointName)]; } @@ -667,7 +667,7 @@ double State::getProportionalGain(const string& _jointName) const // TODO(JS) NOT_YET(State::getProportionalGain()); - assert(mSkeleton->getJoint(_jointName) != NULL); + assert(mSkeleton->getJoint(_jointName) != nullptr); return mKp[mJointMap.at(_jointName)]; } diff --git a/apps/atlasSimbicon/StateMachine.cpp b/apps/atlasSimbicon/StateMachine.cpp index e13c5497657a4..83d6788de149a 100644 --- a/apps/atlasSimbicon/StateMachine.cpp +++ b/apps/atlasSimbicon/StateMachine.cpp @@ -51,7 +51,7 @@ using namespace dart::constraint; //============================================================================== StateMachine::StateMachine(const std::string& _name) : mName(_name), - mCurrentState(NULL), + mCurrentState(nullptr), mBeginTime(0.0), mEndTime(0.0), mFrame(0), @@ -85,7 +85,7 @@ const std::string& StateMachine::getName() const //============================================================================== void StateMachine::addState(State* _state) { - assert(_state != NULL && "Invalid state"); + assert(_state != nullptr && "Invalid state"); assert(!_containState(_state) && "_state shouldn't be in mStates"); mStates.push_back(_state); @@ -94,7 +94,7 @@ void StateMachine::addState(State* _state) //============================================================================== void StateMachine::setInitialState(State* _state) { - assert(_state != NULL); + assert(_state != nullptr); assert(_containState(_state)); mCurrentState = _state; @@ -113,7 +113,7 @@ void StateMachine::begin(double _currentTime) //============================================================================== void StateMachine::computeControlForce(double _dt) { - assert(mCurrentState != NULL && "Invaild current state."); + assert(mCurrentState != nullptr && "Invaild current state."); // Check transition is needed from current state if (mCurrentState->isTerminalConditionSatisfied()) @@ -172,7 +172,7 @@ void StateMachine::transiteTo(string& _stateName, double _currentTime) // _state should be in mStates State* state = _findState(_stateName); - assert(state != NULL && "Invaild state."); + assert(state != nullptr && "Invaild state."); transiteTo(state, _currentTime); } @@ -207,7 +207,7 @@ bool StateMachine::_containState(const string& _name) const //============================================================================== State* StateMachine::_findState(const string& _name) const { - State* state = NULL; + State* state = nullptr; for (vector::const_iterator it = mStates.begin(); it != mStates.end(); ++it) diff --git a/apps/atlasSimbicon/TerminalCondition.cpp b/apps/atlasSimbicon/TerminalCondition.cpp index fb886a6ae3a2e..9d46a0da12e73 100644 --- a/apps/atlasSimbicon/TerminalCondition.cpp +++ b/apps/atlasSimbicon/TerminalCondition.cpp @@ -52,7 +52,7 @@ using namespace dart::constraint; TerminalCondition::TerminalCondition(State* _state) : mState(_state) { - assert(_state != NULL); + assert(_state != nullptr); } //============================================================================== @@ -89,8 +89,8 @@ BodyContactCondition::BodyContactCondition(State* _state, BodyNode* _body) : TerminalCondition(_state), mBodyNode(_body) { - assert(_state != NULL); - assert(_body != NULL); + assert(_state != nullptr); + assert(_body != nullptr); } //============================================================================== diff --git a/apps/bipedStand/MyWindow.cpp b/apps/bipedStand/MyWindow.cpp index 4ce6a3478cd91..0aa9d529d821b 100644 --- a/apps/bipedStand/MyWindow.cpp +++ b/apps/bipedStand/MyWindow.cpp @@ -39,7 +39,7 @@ MyWindow::MyWindow(): SimWindow() { mForce = Eigen::Vector3d::Zero(); - mController = NULL; + mController = nullptr; mImpulseDuration = 0; } diff --git a/apps/hybridDynamics/Main.cpp b/apps/hybridDynamics/Main.cpp index fae61657b9e0a..09b62a2bfd6c3 100644 --- a/apps/hybridDynamics/Main.cpp +++ b/apps/hybridDynamics/Main.cpp @@ -44,7 +44,7 @@ int main(int argc, char* argv[]) dart::simulation::WorldPtr myWorld = dart::utils::SkelParser::readWorld( DART_DATA_PATH"/skel/fullbody1.skel"); - assert(myWorld != NULL); + assert(myWorld != nullptr); Eigen::Vector3d gravity(0.0, -9.81, 0.0); myWorld->setGravity(gravity); diff --git a/apps/jointConstraints/Main.cpp b/apps/jointConstraints/Main.cpp index 5a83a6f7ca7a8..67cdc3d5b0ec9 100644 --- a/apps/jointConstraints/Main.cpp +++ b/apps/jointConstraints/Main.cpp @@ -50,7 +50,7 @@ int main(int argc, char* argv[]) // create and initialize the world dart::simulation::WorldPtr myWorld = utils::SkelParser::readWorld(DART_DATA_PATH"skel/fullbody1.skel"); - assert(myWorld != NULL); + assert(myWorld != nullptr); Eigen::Vector3d gravity(0.0, -9.81, 0.0); myWorld->setGravity(gravity); diff --git a/apps/operationalSpaceControl/Controller.cpp b/apps/operationalSpaceControl/Controller.cpp index 7baa8ace43279..d7cddd70a7610 100644 --- a/apps/operationalSpaceControl/Controller.cpp +++ b/apps/operationalSpaceControl/Controller.cpp @@ -42,8 +42,8 @@ Controller::Controller(dart::dynamics::SkeletonPtr _robot, : mRobot(_robot), mEndEffector(_endEffector) { - assert(_robot != NULL); - assert(_endEffector != NULL); + assert(_robot != nullptr); + assert(_endEffector != nullptr); int dof = mRobot->getNumDofs(); diff --git a/apps/operationalSpaceControl/MyWindow.cpp b/apps/operationalSpaceControl/MyWindow.cpp index 1a55b8d6ff7fe..8ba491e0460d0 100644 --- a/apps/operationalSpaceControl/MyWindow.cpp +++ b/apps/operationalSpaceControl/MyWindow.cpp @@ -44,7 +44,7 @@ MyWindow::MyWindow(Controller* _controller) mController(_controller), mCircleTask(false) { - assert(_controller != NULL); + assert(_controller != nullptr); // Set the initial target positon to the initial position of the end effector mTargetPosition = mController->getEndEffector()->getTransform().translation(); diff --git a/apps/rigidChain/Main.cpp b/apps/rigidChain/Main.cpp index 71803e1305c6d..5c37f0e3e41b8 100644 --- a/apps/rigidChain/Main.cpp +++ b/apps/rigidChain/Main.cpp @@ -43,7 +43,7 @@ int main(int argc, char* argv[]) { // create and initialize the world dart::simulation::WorldPtr myWorld = dart::utils::SkelParser::readWorld(DART_DATA_PATH"/skel/chain.skel"); - assert(myWorld != NULL); + assert(myWorld != nullptr); // create and initialize the world Eigen::Vector3d gravity(0.0, -9.81, 0.0); diff --git a/apps/rigidCubes/Main.cpp b/apps/rigidCubes/Main.cpp index a360039616d40..fcd2f7fc41453 100644 --- a/apps/rigidCubes/Main.cpp +++ b/apps/rigidCubes/Main.cpp @@ -46,7 +46,7 @@ int main(int argc, char* argv[]) { dart::simulation::WorldPtr myWorld = dart::utils::SkelParser::readWorld( DART_DATA_PATH"/skel/cubes.skel"); - assert(myWorld != NULL); + assert(myWorld != nullptr); Eigen::Vector3d gravity(0.0, -9.81, 0.0); myWorld->setGravity(gravity); diff --git a/apps/rigidLoop/Main.cpp b/apps/rigidLoop/Main.cpp index 1222d5a14a94d..831c754656cf7 100644 --- a/apps/rigidLoop/Main.cpp +++ b/apps/rigidLoop/Main.cpp @@ -50,7 +50,7 @@ int main(int argc, char* argv[]) // create and initialize the world dart::simulation::WorldPtr myWorld = utils::SkelParser::readWorld(DART_DATA_PATH"/skel/chain.skel"); - assert(myWorld != NULL); + assert(myWorld != nullptr); // create and initialize the world Eigen::Vector3d gravity(0.0, -9.81, 0.0); diff --git a/apps/softBodies/Main.cpp b/apps/softBodies/Main.cpp index 09f3a1101447e..3674b05abdb97 100644 --- a/apps/softBodies/Main.cpp +++ b/apps/softBodies/Main.cpp @@ -53,7 +53,7 @@ int main(int argc, char* argv[]) dart::simulation::WorldPtr myWorld = dart::utils::SkelParser::readWorld( DART_DATA_PATH"skel/softBodies.skel"); - assert(myWorld != NULL); + assert(myWorld != nullptr); for(size_t i=0; igetNumSkeletons(); ++i) { diff --git a/apps/speedTest/Main.cpp b/apps/speedTest/Main.cpp index 14a9e595e8829..12470af5d8cb7 100644 --- a/apps/speedTest/Main.cpp +++ b/apps/speedTest/Main.cpp @@ -114,7 +114,7 @@ void runKinematicsTest(std::vector& results, double testDynamicsSpeed(dart::simulation::WorldPtr world, size_t numIterations = 10000) { - if(NULL==world) + if(nullptr==world) return 0; for(size_t i=0; igetNumSkeletons(); ++i) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index ede167c857d1d..2b7e31027ac9a 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -61,7 +61,7 @@ CollisionDetector::~CollisionDetector() { //============================================================================== void CollisionDetector::addSkeleton(const dynamics::SkeletonPtr& _skeleton) { - assert(_skeleton != NULL + assert(_skeleton != nullptr && "Null pointer skeleton is not allowed to add to CollisionDetector."); if (containSkeleton(_skeleton) == false) @@ -80,7 +80,7 @@ void CollisionDetector::addSkeleton(const dynamics::SkeletonPtr& _skeleton) //============================================================================== void CollisionDetector::removeSkeleton(const dynamics::SkeletonPtr& _skeleton) { - assert(_skeleton != NULL + assert(_skeleton != nullptr && "Null pointer skeleton is not allowed to add to CollisionDetector."); if (containSkeleton(_skeleton)) @@ -108,11 +108,11 @@ void CollisionDetector::removeAllSkeletons() void CollisionDetector::addCollisionSkeletonNode(dynamics::BodyNode* _bodyNode, bool _isRecursive) { - assert(_bodyNode != NULL && "Invalid body node."); + assert(_bodyNode != nullptr && "Invalid body node."); // If this collision detector already has collision node for _bodyNode, then // we do nothing. - if (getCollisionNode(_bodyNode) != NULL) { + if (getCollisionNode(_bodyNode) != nullptr) { std::cout << "The collision detector already has a collision node for " << "body node [" << _bodyNode->getName() << "]." << std::endl; return; @@ -140,11 +140,11 @@ void CollisionDetector::addCollisionSkeletonNode(dynamics::BodyNode* _bodyNode, void CollisionDetector::removeCollisionSkeletonNode( dynamics::BodyNode* _bodyNode, bool _isRecursive) { - assert(_bodyNode != NULL && "Invalid body node."); + assert(_bodyNode != nullptr && "Invalid body node."); // If a collision node is already created for _bodyNode, then we just return CollisionNode* collNode = getCollisionNode(_bodyNode); - if (collNode == NULL) { + if (collNode == nullptr) { std::cout << "The collision detector does not have any collision node " << "for body node [" << _bodyNode->getName() << "]." << std::endl; @@ -335,7 +335,7 @@ CollisionNode* CollisionDetector::getCollisionNode( if (mBodyCollisionMap.find(_bodyNode) != mBodyCollisionMap.end()) return mBodyCollisionMap[_bodyNode]; else - return NULL; + return nullptr; } } // namespace collision diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index cfcb2c4c25bec..b70933f69d53e 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -70,8 +70,8 @@ struct CollisionFilter : public btOverlapFilterCallback virtual bool needBroadphaseCollision(btBroadphaseProxy* _proxy0, btBroadphaseProxy* _proxy1) const { - assert((_proxy0 != NULL && _proxy1 != NULL) && - "Bullet broadphase overlapping pair proxies are NULL"); + assert((_proxy0 != nullptr && _proxy1 != nullptr) && + "Bullet broadphase overlapping pair proxies are nullptr"); bool collide = (_proxy0->m_collisionFilterGroup & _proxy1->m_collisionFilterMask) != 0; @@ -123,7 +123,7 @@ BulletCollisionDetector::BulletCollisionDetector() : CollisionDetector() btOverlapFilterCallback* filterCallback = new CollisionFilter(); btOverlappingPairCache* pairCache = mBulletCollisionWorld->getPairCache(); - assert(pairCache != NULL); + assert(pairCache != nullptr); pairCache->setOverlapFilterCallback(filterCallback); } diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 36b4881dbe162..f8a4d673072eb 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -97,8 +97,8 @@ bool DARTCollisionDetector::detectCollision(bool /*_checkAllCollisions*/, contactPair = contacts[m]; contactPair.bodyNode1 = BodyNode1; contactPair.bodyNode2 = BodyNode2; - assert(contactPair.bodyNode1.lock() != NULL); - assert(contactPair.bodyNode2.lock() != NULL); + assert(contactPair.bodyNode1.lock() != nullptr); + assert(contactPair.bodyNode2.lock() != nullptr); mContacts.push_back(contactPair); } diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 39f852effacbf..973632f766c27 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -113,8 +113,8 @@ bool FCLCollisionDetector::detectCollision(bool _checkAllCollisions, contactPair.normal(2) = contact.normal[2]; contactPair.bodyNode1 = findCollisionNode(contact.o1)->getBodyNode(); contactPair.bodyNode2 = findCollisionNode(contact.o2)->getBodyNode(); - assert(contactPair.bodyNode1.lock() != NULL); - assert(contactPair.bodyNode2.lock() != NULL); + assert(contactPair.bodyNode1.lock() != nullptr); + assert(contactPair.bodyNode2.lock() != nullptr); // contactPair.bdID1 = // collisionNodePair.collisionNode1->getBodyNodeID(); // contactPair.bdID2 = @@ -174,7 +174,7 @@ CollisionNode* FCLCollisionDetector::findCollisionNode( return mCollisionNodes[i]; } } - return NULL; + return nullptr; } } // namespace collision diff --git a/dart/collision/fcl_mesh/CollisionShapes.h b/dart/collision/fcl_mesh/CollisionShapes.h index b35d852b1e857..e9b6f8e731707 100644 --- a/dart/collision/fcl_mesh/CollisionShapes.h +++ b/dart/collision/fcl_mesh/CollisionShapes.h @@ -350,7 +350,7 @@ fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, if (_slices < 2 || _stacks < 1 || _baseRadius < 0.0 || _topRadius < 0.0 || _height < 0.0) { - return NULL; + return nullptr; } /* Center at CoM */ diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp index 423cef494c0ad..8a4cc4c500ab4 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp +++ b/dart/collision/fcl_mesh/FCLMeshCollisionDetector.cpp @@ -92,8 +92,8 @@ bool FCLMeshCollisionDetector::detectCollision(bool _checkAllCollisions, bool collision = false; - FCLMeshCollisionNode* FCLMeshCollisionNode1 = NULL; - FCLMeshCollisionNode* FCLMeshCollisionNode2 = NULL; + FCLMeshCollisionNode* FCLMeshCollisionNode1 = nullptr; + FCLMeshCollisionNode* FCLMeshCollisionNode2 = nullptr; for (size_t i = 0; i < mCollisionNodes.size(); i++) { @@ -107,7 +107,7 @@ bool FCLMeshCollisionDetector::detectCollision(bool _checkAllCollisions, continue; std::vector* contactPoints - = _calculateContactPoints ? &mContacts : NULL; + = _calculateContactPoints ? &mContacts : nullptr; if (FCLMeshCollisionNode1->detectCollision(FCLMeshCollisionNode2, contactPoints, mNumMaxContacts)) @@ -136,7 +136,7 @@ bool FCLMeshCollisionDetector::detectCollision(CollisionNode* _node1, static_cast(_node2); return collisionNode1->detectCollision( collisionNode2, - _calculateContactPoints ? &mContacts : NULL, + _calculateContactPoints ? &mContacts : nullptr, mNumMaxContacts); } diff --git a/dart/common/Timer.cpp b/dart/common/Timer.cpp index b8f08ad32f642..6fd026167a955 100644 --- a/dart/common/Timer.cpp +++ b/dart/common/Timer.cpp @@ -87,7 +87,7 @@ void Timer::start() #if WIN32 QueryPerformanceCounter(&mTimer.start); #else - gettimeofday(&mTimeVal, NULL); + gettimeofday(&mTimeVal, nullptr); mStartedTime = mTimeVal.tv_sec + (mTimeVal.tv_usec / 1.0e+6); #endif } @@ -102,7 +102,7 @@ void Timer::stop() time.QuadPart = mTimer.stop.QuadPart - mTimer.start.QuadPart; mLastElapsedTime = _convLIToSecs(time); #else - gettimeofday(&mTimeVal, NULL); + gettimeofday(&mTimeVal, nullptr); mStoppedTime = mTimeVal.tv_sec + (mTimeVal.tv_usec / 1.0e+6); mLastElapsedTime = mStoppedTime - mStartedTime; #endif @@ -119,7 +119,7 @@ double Timer::getElapsedTime() time.QuadPart = timenow.QuadPart - mTimer.start.QuadPart; mLastElapsedTime = _convLIToSecs(time); #else - gettimeofday(&mTimeVal, NULL); + gettimeofday(&mTimeVal, nullptr); mLastElapsedTime = mTimeVal.tv_sec + (mTimeVal.tv_usec / 1.0e+6) - mStartedTime; #endif @@ -176,7 +176,7 @@ double Timer::getWallTime() #else // Initialize the lastUpdateTime with the current time in seconds timeval timeVal; - gettimeofday(&timeVal, NULL); + gettimeofday(&timeVal, nullptr); return timeVal.tv_sec + timeVal.tv_usec / 1.0e+6; #endif } diff --git a/dart/constraint/BallJointConstraint.cpp b/dart/constraint/BallJointConstraint.cpp index 59354064d2901..ce3c6981a0371 100644 --- a/dart/constraint/BallJointConstraint.cpp +++ b/dart/constraint/BallJointConstraint.cpp @@ -242,7 +242,7 @@ void BallJointConstraint::applyUnitImpulse(size_t _index) //============================================================================== void BallJointConstraint::getVelocityChange(double* _vel, bool _withCfm) { - assert(_vel != NULL && "Null pointer is not allowed."); + assert(_vel != nullptr && "Null pointer is not allowed."); for (size_t i = 0; i < mDim; ++i) _vel[i] = 0.0; @@ -282,7 +282,7 @@ void BallJointConstraint::excite() if (mBodyNode1->isReactive()) mBodyNode1->getSkeleton()->setImpulseApplied(true); - if (mBodyNode2 == NULL) + if (mBodyNode2 == nullptr) return; if (mBodyNode2->isReactive()) @@ -295,7 +295,7 @@ void BallJointConstraint::unexcite() if (mBodyNode1->isReactive()) mBodyNode1->getSkeleton()->setImpulseApplied(false); - if (mBodyNode2 == NULL) + if (mBodyNode2 == nullptr) return; if (mBodyNode2->isReactive()) @@ -347,7 +347,7 @@ dynamics::SkeletonPtr BallJointConstraint::getRootSkeleton() const //============================================================================== void BallJointConstraint::uniteSkeletons() { - if (mBodyNode2 == NULL) + if (mBodyNode2 == nullptr) return; if (!mBodyNode1->isReactive() || !mBodyNode2->isReactive()) diff --git a/dart/constraint/ConstrainedGroup.cpp b/dart/constraint/ConstrainedGroup.cpp index 24d726f74f803..cd60fda2ed6e2 100644 --- a/dart/constraint/ConstrainedGroup.cpp +++ b/dart/constraint/ConstrainedGroup.cpp @@ -59,7 +59,7 @@ ConstrainedGroup::~ConstrainedGroup() //============================================================================== void ConstrainedGroup::addConstraint(ConstraintBase* _constraint) { - assert(_constraint != NULL && "Null constraint pointer is now allowed."); + assert(_constraint != nullptr && "Null constraint pointer is now allowed."); assert(containConstraint(_constraint) == false && "Don't try to add same constraint multiple times into Community."); assert(_constraint->isActive()); @@ -83,7 +83,7 @@ ConstraintBase* ConstrainedGroup::getConstraint(size_t _index) const //============================================================================== void ConstrainedGroup::removeConstraint(ConstraintBase* _constraint) { - assert(_constraint != NULL && "Null constraint pointer is now allowed."); + assert(_constraint != nullptr && "Null constraint pointer is now allowed."); assert(containConstraint(_constraint) == true && "Don't try to remove a constraint not contained in Community."); diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 30ce3f356ea87..5113966758fd1 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -78,7 +78,7 @@ ConstraintSolver::~ConstraintSolver() //============================================================================== void ConstraintSolver::addSkeleton(const SkeletonPtr& _skeleton) { - assert(_skeleton != NULL + assert(_skeleton != nullptr && "Null pointer skeleton is now allowed to add to ConstraintSover."); if (containSkeleton(_skeleton) == false) @@ -126,7 +126,7 @@ void ConstraintSolver::addSkeletons(const std::vector& _skeletons) void ConstraintSolver::removeSkeleton( const SkeletonPtr& _skeleton) { - assert(_skeleton != NULL + assert(_skeleton != nullptr && "Null pointer skeleton is now allowed to add to ConstraintSover."); if (containSkeleton(_skeleton)) @@ -276,7 +276,7 @@ void ConstraintSolver::solve() //============================================================================== bool ConstraintSolver::containSkeleton(const ConstSkeletonPtr& _skeleton) const { - assert(_skeleton != NULL && "Not allowed to insert null pointer skeleton."); + assert(_skeleton != nullptr && "Not allowed to insert null pointer skeleton."); for (std::vector::const_iterator it = mSkeletons.begin(); it != mSkeletons.end(); ++it) diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 504a74637f765..45fb9c55691f2 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -601,7 +601,7 @@ void ContactConstraint::applyUnitImpulse(size_t _idx) //============================================================================== void ContactConstraint::getVelocityChange(double* _vel, bool _withCfm) { - assert(_vel != NULL && "Null pointer is not allowed."); + assert(_vel != nullptr && "Null pointer is not allowed."); for (size_t i = 0; i < mDim; ++i) { @@ -737,7 +737,7 @@ void ContactConstraint::applyImpulse(double* _lambda) //============================================================================== void ContactConstraint::getRelVelocity(double* _relVel) { - assert(_relVel != NULL && "Null pointer is not allowed."); + assert(_relVel != nullptr && "Null pointer is not allowed."); for (size_t i = 0; i < mDim; ++i) { diff --git a/dart/constraint/JointConstraint.cpp b/dart/constraint/JointConstraint.cpp index d5d7d150d1f51..2b54d4bc7ce47 100644 --- a/dart/constraint/JointConstraint.cpp +++ b/dart/constraint/JointConstraint.cpp @@ -58,7 +58,7 @@ double JointConstraint::mConstraintForceMixing = DART_CFM; JointConstraint::JointConstraint(dynamics::BodyNode* _body) : ConstraintBase(), mBodyNode1(_body), - mBodyNode2(NULL) + mBodyNode2(nullptr) { assert(_body); } diff --git a/dart/constraint/JointCoulombFrictionConstraint.cpp b/dart/constraint/JointCoulombFrictionConstraint.cpp index 37303b99b1a7c..fb255292c72ea 100644 --- a/dart/constraint/JointCoulombFrictionConstraint.cpp +++ b/dart/constraint/JointCoulombFrictionConstraint.cpp @@ -212,7 +212,7 @@ void JointCoulombFrictionConstraint::applyUnitImpulse(size_t _index) void JointCoulombFrictionConstraint::getVelocityChange(double* _delVel, bool _withCfm) { - assert(_delVel != NULL && "Null pointer is not allowed."); + assert(_delVel != nullptr && "Null pointer is not allowed."); size_t localIndex = 0; size_t dof = mJoint->getNumDofs(); diff --git a/dart/constraint/JointLimitConstraint.cpp b/dart/constraint/JointLimitConstraint.cpp index c06e46e2dbb23..6ae4c9c9aefd7 100644 --- a/dart/constraint/JointLimitConstraint.cpp +++ b/dart/constraint/JointLimitConstraint.cpp @@ -320,7 +320,7 @@ void JointLimitConstraint::applyUnitImpulse(size_t _index) //============================================================================== void JointLimitConstraint::getVelocityChange(double* _delVel, bool _withCfm) { - assert(_delVel != NULL && "Null pointer is not allowed."); + assert(_delVel != nullptr && "Null pointer is not allowed."); size_t localIndex = 0; size_t dof = mJoint->getNumDofs(); diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp index 1601bfd07193e..dfa9ae941f94d 100644 --- a/dart/constraint/SoftContactConstraint.cpp +++ b/dart/constraint/SoftContactConstraint.cpp @@ -77,8 +77,8 @@ SoftContactConstraint::SoftContactConstraint( mBodyNode2(_contact.bodyNode2.lock()), mSoftBodyNode1(dynamic_cast(mBodyNode1)), mSoftBodyNode2(dynamic_cast(mBodyNode2)), - mPointMass1(NULL), - mPointMass2(NULL), + mPointMass1(nullptr), + mPointMass2(nullptr), mSoftCollInfo(static_cast(_contact.userData)), mFirstFrictionalDirection(Eigen::Vector3d::UnitZ()), mIsFrictionOn(true), @@ -658,7 +658,7 @@ void SoftContactConstraint::applyUnitImpulse(size_t _idx) //============================================================================== void SoftContactConstraint::getVelocityChange(double* _vel, bool _withCfm) { - assert(_vel != NULL && "Null pointer is not allowed."); + assert(_vel != nullptr && "Null pointer is not allowed."); for (size_t i = 0; i < mDim; ++i) { @@ -880,7 +880,7 @@ void SoftContactConstraint::applyImpulse(double* _lambda) //============================================================================== void SoftContactConstraint::getRelVelocity(double* _vel) { - assert(_vel != NULL && "Null pointer is not allowed."); + assert(_vel != nullptr && "Null pointer is not allowed."); for (size_t i = 0; i < mDim; ++i) { @@ -1007,7 +1007,7 @@ static PointMassT selectCollidingPointMassT( const Eigen::Vector3d& _point, int _faceId) { - PointMassT pointMass = NULL; + PointMassT pointMass = nullptr; const Eigen::Vector3i& face = _softBodyNode->getFace(_faceId); diff --git a/dart/constraint/WeldJointConstraint.cpp b/dart/constraint/WeldJointConstraint.cpp index 5fd0a1a32265f..ce28116605742 100644 --- a/dart/constraint/WeldJointConstraint.cpp +++ b/dart/constraint/WeldJointConstraint.cpp @@ -256,7 +256,7 @@ void WeldJointConstraint::applyUnitImpulse(size_t _index) //============================================================================== void WeldJointConstraint::getVelocityChange(double* _vel, bool _withCfm) { - assert(_vel != NULL && "Null pointer is not allowed."); + assert(_vel != nullptr && "Null pointer is not allowed."); assert(isActive()); Eigen::Vector6d velChange = Eigen::Vector6d::Zero(); @@ -291,7 +291,7 @@ void WeldJointConstraint::excite() if (mBodyNode1->isReactive()) mBodyNode1->getSkeleton()->setImpulseApplied(true); - if (mBodyNode2 == NULL) + if (mBodyNode2 == nullptr) return; if (mBodyNode2->isReactive()) @@ -304,7 +304,7 @@ void WeldJointConstraint::unexcite() if (mBodyNode1->isReactive()) mBodyNode1->getSkeleton()->setImpulseApplied(false); - if (mBodyNode2 == NULL) + if (mBodyNode2 == nullptr) return; if (mBodyNode2->isReactive()) diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 9333b2dbc258b..cf2561b55b02b 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -688,7 +688,7 @@ const BodyNode* BodyNode::getParentBodyNode() const //============================================================================== void BodyNode::addChildBodyNode(BodyNode* _body) { - assert(_body != NULL); + assert(_body != nullptr); if(std::find(mChildBodyNodes.begin(), mChildBodyNodes.end(), _body) != mChildBodyNodes.end()) @@ -1494,7 +1494,7 @@ void BodyNode::updateTransmittedForceID(const Eigen::Vector3d& _gravity, for (const auto& childBodyNode : mChildBodyNodes) { Joint* childJoint = childBodyNode->getParentJoint(); - assert(childJoint != NULL); + assert(childJoint != nullptr); mF += math::dAdInvT(childJoint->getLocalTransform(), childBodyNode->getBodyForce()); @@ -1670,7 +1670,7 @@ void BodyNode::updateJointForceID(double _timeStep, double _withDampingForces, double _withSpringForces) { - assert(mParentJoint != NULL); + assert(mParentJoint != nullptr); mParentJoint->updateForceID(mF, _timeStep, _withDampingForces, _withSpringForces); } @@ -1680,7 +1680,7 @@ void BodyNode::updateJointForceFD(double _timeStep, double _withDampingForces, double _withSpringForces) { - assert(mParentJoint != NULL); + assert(mParentJoint != nullptr); mParentJoint->updateForceFD(mF, _timeStep, _withDampingForces, _withSpringForces); } @@ -1688,7 +1688,7 @@ void BodyNode::updateJointForceFD(double _timeStep, //============================================================================== void BodyNode::updateJointImpulseFD() { - assert(mParentJoint != NULL); + assert(mParentJoint != nullptr); mParentJoint->updateImpulseFD(mF); } @@ -1831,7 +1831,7 @@ bool BodyNode::isReactive() const { // Check if all the ancestor joints are motion prescribed. const BodyNode* body = this; - while (body != NULL) + while (body != nullptr) { if (body->mParentJoint->isDynamic()) return true; @@ -2178,7 +2178,7 @@ void BodyNode::updateBodyJacobian() const // n: number of dependent coordinates //-------------------------------------------------------------------------- - if(NULL == mParentJoint) + if(nullptr == mParentJoint) return; const size_t localDof = mParentJoint->getNumDofs(); @@ -2278,7 +2278,7 @@ void BodyNode::updateWorldJacobianClassicDeriv() const // w: Total angular velocity of this Frame // p: Offset from origin of parent Frame - if(NULL == mParentJoint) + if(nullptr == mParentJoint) return; const size_t numLocalDOFs = mParentJoint->getNumDofs(); diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index 5c3094c07ef19..e6edd695962ad 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -766,7 +766,7 @@ class BodyNode : public Frame, public SkeletonRefCountingBase //---------------------------------------------------------------------------- /// Render the markers - void drawMarkers(renderer::RenderInterface* _ri = NULL, + void drawMarkers(renderer::RenderInterface* _ri = nullptr, const Eigen::Vector4d& _color = Eigen::Vector4d::Ones(), bool _useDefaultColor = true) const; diff --git a/dart/dynamics/BoxShape.h b/dart/dynamics/BoxShape.h index a9c86fe30390b..22d0184360506 100644 --- a/dart/dynamics/BoxShape.h +++ b/dart/dynamics/BoxShape.h @@ -58,7 +58,7 @@ class BoxShape : public Shape { const Eigen::Vector3d& getSize() const; // Documentation inherited. - void draw(renderer::RenderInterface* _ri = NULL, + void draw(renderer::RenderInterface* _ri = nullptr, const Eigen::Vector4d& _col = Eigen::Vector4d::Ones(), bool _default = true) const; diff --git a/dart/dynamics/CylinderShape.h b/dart/dynamics/CylinderShape.h index 19879cdc42892..6e7a857ab6cdf 100644 --- a/dart/dynamics/CylinderShape.h +++ b/dart/dynamics/CylinderShape.h @@ -63,7 +63,7 @@ class CylinderShape : public Shape { void setHeight(double _height); // Documentation inherited. - void draw(renderer::RenderInterface* _ri = NULL, + void draw(renderer::RenderInterface* _ri = nullptr, const Eigen::Vector4d& _color = Eigen::Vector4d::Ones(), bool _useDefaultColor = true) const; diff --git a/dart/dynamics/EllipsoidShape.h b/dart/dynamics/EllipsoidShape.h index b065621864f59..3368e5cb7d707 100644 --- a/dart/dynamics/EllipsoidShape.h +++ b/dart/dynamics/EllipsoidShape.h @@ -58,7 +58,7 @@ class EllipsoidShape : public Shape { const Eigen::Vector3d& getSize() const; // Documentation inherited. - void draw(renderer::RenderInterface* _ri = NULL, + void draw(renderer::RenderInterface* _ri = nullptr, const Eigen::Vector4d& _col = Eigen::Vector4d::Ones(), bool _useDefaultColor = true) const; diff --git a/dart/dynamics/Entity.cpp b/dart/dynamics/Entity.cpp index ef3b959dd015a..905078fe901c6 100644 --- a/dart/dynamics/Entity.cpp +++ b/dart/dynamics/Entity.cpp @@ -224,7 +224,7 @@ ConstShapePtr Entity::getVisualizationShape(size_t _index) const void Entity::draw(renderer::RenderInterface *_ri, const Eigen::Vector4d &_color, bool _useDefaultColor, int) const { - if(NULL == _ri) + if(nullptr == _ri) return; // _ri->pushMatrix(); @@ -258,7 +258,7 @@ const Frame* Entity::getParentFrame() const //============================================================================== bool Entity::descendsFrom(const Frame *_someFrame) const { - if(NULL == _someFrame) + if(nullptr == _someFrame) return false; if(this == _someFrame) diff --git a/dart/dynamics/Entity.h b/dart/dynamics/Entity.h index 3229a668ec631..063d154e68492 100644 --- a/dart/dynamics/Entity.h +++ b/dart/dynamics/Entity.h @@ -145,7 +145,7 @@ class Entity : public virtual common::Subject ConstShapePtr getVisualizationShape(size_t _index) const; /// Render this Entity - virtual void draw(renderer::RenderInterface* _ri = NULL, + virtual void draw(renderer::RenderInterface* _ri = nullptr, const Eigen::Vector4d& _color = Eigen::Vector4d::Ones(), bool _useDefaultColor = true, int _depth = 0) const; @@ -156,7 +156,7 @@ class Entity : public virtual common::Subject const Frame* getParentFrame() const; /// True iff this Entity depends on (i.e. kinematically descends from) - /// _someFrame. If _someFrame is NULL, this returns false. + /// _someFrame. If _someFrame is nullptr, this returns false. bool descendsFrom(const Frame* _someFrame) const; /// Returns true if this Entity is set to be quiet. diff --git a/dart/dynamics/Frame.cpp b/dart/dynamics/Frame.cpp index a82e9fc185b05..278a7078eb231 100644 --- a/dart/dynamics/Frame.cpp +++ b/dart/dynamics/Frame.cpp @@ -62,7 +62,7 @@ Frame::~Frame() if(isWorld()) return; - changeParentFrame(NULL); + changeParentFrame(nullptr); // Inform all child entities that this Frame is disappearing by setting their // reference frames to the World frame. @@ -429,7 +429,7 @@ bool Frame::isWorld() const void Frame::draw(renderer::RenderInterface* _ri, const Eigen::Vector4d& _color, bool _useDefaultColor, int _depth) const { - if(NULL == _ri) + if(nullptr == _ri) return; _ri->pushMatrix(); @@ -600,7 +600,7 @@ const Eigen::Vector6d& WorldFrame::getPartialAcceleration() const //============================================================================== WorldFrame::WorldFrame() - : Entity(NULL, "World", true), + : Entity(nullptr, "World", true), Frame(), mRelativeTf(Eigen::Isometry3d::Identity()), mZero(Eigen::Vector6d::Zero()) diff --git a/dart/dynamics/Frame.h b/dart/dynamics/Frame.h index 82d32c88f0b97..9432038033318 100644 --- a/dart/dynamics/Frame.h +++ b/dart/dynamics/Frame.h @@ -233,7 +233,7 @@ class Frame : public virtual Entity // Render this Frame as well as any Entities it contains virtual void draw( - renderer::RenderInterface *_ri = NULL, + renderer::RenderInterface *_ri = nullptr, const Eigen::Vector4d &_color = Eigen::Vector4d::Ones(), bool _useDefaultColor = true, int _depth = 0) const override; diff --git a/dart/dynamics/Marker.h b/dart/dynamics/Marker.h index 48f9ab2e632b5..fbfc86d3f5b85 100644 --- a/dart/dynamics/Marker.h +++ b/dart/dynamics/Marker.h @@ -79,7 +79,7 @@ class Marker { virtual ~Marker(); /// \brief - void draw(renderer::RenderInterface* _ri = NULL, bool _offset = true, + void draw(renderer::RenderInterface* _ri = nullptr, bool _offset = true, const Eigen::Vector4d& _color = Eigen::Vector4d::Identity(), bool _useDefaultColor = true) const; diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index 794158fe8380f..666c0c8f17261 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -271,7 +271,7 @@ const aiScene* MeshShape::loadMesh(const std::string& _fileName) { aiProcess_JoinIdenticalVertices | aiProcess_SortByPType | aiProcess_OptimizeMeshes, - NULL, propertyStore); + nullptr, propertyStore); if(!scene) dtwarn << "[MeshShape] Assimp could not load file: '" << _fileName << "'. " << "This will likely result in a segmentation fault " diff --git a/dart/dynamics/MeshShape.h b/dart/dynamics/MeshShape.h index 126bde7cef55b..c8de0bc36f8ac 100644 --- a/dart/dynamics/MeshShape.h +++ b/dart/dynamics/MeshShape.h @@ -79,7 +79,7 @@ class MeshShape : public Shape { void setDisplayList(int _index); // Documentation inherited. - void draw(renderer::RenderInterface* _ri = NULL, + void draw(renderer::RenderInterface* _ri = nullptr, const Eigen::Vector4d& _col = Eigen::Vector4d::Ones(), bool _default = true) const; diff --git a/dart/dynamics/PlaneShape.h b/dart/dynamics/PlaneShape.h index 204f982eb54e4..e077dc500b15e 100644 --- a/dart/dynamics/PlaneShape.h +++ b/dart/dynamics/PlaneShape.h @@ -54,7 +54,7 @@ class PlaneShape : public Shape // Documentation inherited. // TODO(JS): Not implemented yet - void draw(renderer::RenderInterface* _ri = NULL, + void draw(renderer::RenderInterface* _ri = nullptr, const Eigen::Vector4d& _col = Eigen::Vector4d::Ones(), bool _default = true) const; diff --git a/dart/dynamics/PointMass.cpp b/dart/dynamics/PointMass.cpp index cafd288b7ab18..3002c81f4ef1d 100644 --- a/dart/dynamics/PointMass.cpp +++ b/dart/dynamics/PointMass.cpp @@ -127,7 +127,7 @@ PointMass::PointMass(SoftBodyNode* _softBodyNode) mNotifier(_softBodyNode->mNotifier), mShape(new EllipsoidShape(Eigen::Vector3d(0.01, 0.01, 0.01))) { - assert(mParentSoftBodyNode != NULL); + assert(mParentSoftBodyNode != nullptr); mNotifier->notifyTransformUpdate(); } @@ -187,7 +187,7 @@ double PointMass::getImplicitPi() const //============================================================================== void PointMass::addConnectedPointMass(PointMass* _pointMass) { - assert(_pointMass != NULL); + assert(_pointMass != nullptr); mParentSoftBodyNode->mSoftP.mPointProps[mIndex]. mConnectedPointMassIndices.push_back(_pointMass->mIndex); @@ -561,7 +561,7 @@ const Eigen::Vector3d& PointMass::getWorldPosition() const //============================================================================== Eigen::Matrix PointMass::getBodyJacobian() { - assert(mParentSoftBodyNode != NULL); + assert(mParentSoftBodyNode != nullptr); int dof = mParentSoftBodyNode->getNumDependentGenCoords(); int totalDof = mParentSoftBodyNode->getNumDependentGenCoords() + 3; @@ -1014,7 +1014,7 @@ void PointMass::draw(renderer::RenderInterface* _ri, const Eigen::Vector4d& _color, bool _useDefaultColor) const { - if (_ri == NULL) + if (_ri == nullptr) return; _ri->pushMatrix(); diff --git a/dart/dynamics/PointMass.h b/dart/dynamics/PointMass.h index 4fa19ebc2b1ed..22ad4859ad0e2 100644 --- a/dart/dynamics/PointMass.h +++ b/dart/dynamics/PointMass.h @@ -510,7 +510,7 @@ class PointMass : public common::Subject //---------------------------- Rendering ------------------------------------- /// - virtual void draw(renderer::RenderInterface* _ri = NULL, + virtual void draw(renderer::RenderInterface* _ri = nullptr, const Eigen::Vector4d& _color = Eigen::Vector4d::Ones(), bool _useDefaultColor = true) const; diff --git a/dart/dynamics/Shape.h b/dart/dynamics/Shape.h index 44585aecd1d61..569b4cafae420 100644 --- a/dart/dynamics/Shape.h +++ b/dart/dynamics/Shape.h @@ -138,7 +138,7 @@ class Shape : public virtual common::Subject ShapeType getShapeType() const; /// \brief - virtual void draw(renderer::RenderInterface* _ri = NULL, + virtual void draw(renderer::RenderInterface* _ri = nullptr, const Eigen::Vector4d& _color = Eigen::Vector4d::Ones(), bool _useDefaultColor = true) const = 0; diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 0bb3aa7392ffc..8b073469952ef 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -636,12 +636,12 @@ class Skeleton : public MetaSkeleton //---------------------------------------------------------------------------- /// Draw this skeleton - void draw(renderer::RenderInterface* _ri = NULL, + void draw(renderer::RenderInterface* _ri = nullptr, const Eigen::Vector4d& _color = Eigen::Vector4d::Ones(), bool _useDefaultColor = true) const; /// Draw markers in this skeleton - void drawMarkers(renderer::RenderInterface* _ri = NULL, + void drawMarkers(renderer::RenderInterface* _ri = nullptr, const Eigen::Vector4d& _color = Eigen::Vector4d::Ones(), bool _useDefaultColor = true) const; diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index 43b7cf37883a5..4297d9a83d95e 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -291,14 +291,14 @@ void SoftBodyNode::init(const SkeletonPtr& _skeleton) // //---------------------------------------------------------------------------- // // Visualization shape // //---------------------------------------------------------------------------- -// assert(mSoftVisualShape == NULL); +// assert(mSoftVisualShape == nullptr); // mSoftVisualShape = new SoftMeshShape(this); // BodyNode::addVisualizationShape(mSoftVisualShape); // //---------------------------------------------------------------------------- // // Collision shape // //---------------------------------------------------------------------------- -// assert(mSoftCollShape == NULL); +// assert(mSoftCollShape == nullptr); // mSoftCollShape = new SoftMeshShape(this); // BodyNode::addCollisionShape(mSoftCollShape); } @@ -530,7 +530,7 @@ void SoftBodyNode::updateTransmittedForceID(const Eigen::Vector3d& _gravity, for (const auto& childBodyNode : mChildBodyNodes) { Joint* childJoint = childBodyNode->getParentJoint(); - assert(childJoint != NULL); + assert(childJoint != nullptr); mF += math::dAdInvT(childJoint->getLocalTransform(), childBodyNode->getBodyForce()); @@ -1140,7 +1140,7 @@ void SoftBodyNode::draw(renderer::RenderInterface* _ri, bool _useDefaultColor, int _depth) const { - if (_ri == NULL) + if (_ri == nullptr) return; _ri->pushMatrix(); @@ -2494,7 +2494,7 @@ void SoftBodyNodeHelper::setSinglePointMass(SoftBodyNode* _softBodyNode, double _edgeStiffness, double _dampingCoeff) { - assert(_softBodyNode != NULL); + assert(_softBodyNode != nullptr); _softBodyNode->setProperties(makeSinglePointMassProperties( _totalMass, _vertexStiffness, @@ -2672,7 +2672,7 @@ void SoftBodyNodeHelper::setEllipsoid(SoftBodyNode* _softBodyNode, double _edgeStiffness, double _dampingCoeff) { - assert(_softBodyNode != NULL); + assert(_softBodyNode != nullptr); _softBodyNode->setProperties(makeEllipsoidProperties(_size, _nSlices, _nStacks, diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index 8dd0e3c2035ad..85894590154a5 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -340,7 +340,7 @@ class SoftBodyNode : public BodyNode // Rendering //-------------------------------------------------------------------------- /// \brief Render the entire subtree rooted at this body node. - virtual void draw(renderer::RenderInterface* _ri = NULL, + virtual void draw(renderer::RenderInterface* _ri = nullptr, const Eigen::Vector4d& _color = Eigen::Vector4d::Ones(), bool _useDefaultColor = true, int _depth = 0) const override; diff --git a/dart/dynamics/SoftMeshShape.cpp b/dart/dynamics/SoftMeshShape.cpp index 9a568a4a150ab..53db426dccfdc 100644 --- a/dart/dynamics/SoftMeshShape.cpp +++ b/dart/dynamics/SoftMeshShape.cpp @@ -47,9 +47,9 @@ namespace dynamics { SoftMeshShape::SoftMeshShape(SoftBodyNode* _softBodyNode) : Shape(SOFT_MESH), mSoftBodyNode(_softBodyNode), - mAssimpMesh(NULL) + mAssimpMesh(nullptr) { - assert(_softBodyNode != NULL); + assert(_softBodyNode != nullptr); // Build mesh here using soft body node // TODO(JS): Not implemented. _buildMesh(); diff --git a/dart/dynamics/SoftMeshShape.h b/dart/dynamics/SoftMeshShape.h index 73765508bf40d..ae129cf28927c 100644 --- a/dart/dynamics/SoftMeshShape.h +++ b/dart/dynamics/SoftMeshShape.h @@ -67,7 +67,7 @@ class SoftMeshShape : public Shape // Documentation inherited. virtual void draw( - renderer::RenderInterface* _ri = NULL, + renderer::RenderInterface* _ri = nullptr, const Eigen::Vector4d& _col = Eigen::Vector4d::Ones(), bool _default = true) const; diff --git a/dart/gui/GlutWindow.cpp b/dart/gui/GlutWindow.cpp index 5b4eded72cd32..69355899374b7 100644 --- a/dart/gui/GlutWindow.cpp +++ b/dart/gui/GlutWindow.cpp @@ -67,7 +67,7 @@ GlutWindow::GlutWindow() { mBackground[1] = 0.3; mBackground[2] = 0.3; mBackground[3] = 1.0; - mRI = NULL; + mRI = nullptr; } GlutWindow::~GlutWindow() { diff --git a/dart/gui/lodepng.cpp b/dart/gui/lodepng.cpp index fee6db0489959..3d0ce61a8ded4 100644 --- a/dart/gui/lodepng.cpp +++ b/dart/gui/lodepng.cpp @@ -125,7 +125,7 @@ static void uivector_cleanup(void* p) { ((uivector*)p)->size = ((uivector*)p)->allocsize = 0; myfree(((uivector*)p)->data); - ((uivector*)p)->data = NULL; + ((uivector*)p)->data = nullptr; } /*returns 1 if success, 0 if failure ==> nothing done*/ @@ -158,7 +158,7 @@ static unsigned uivector_resizev(uivector* p, size_t size, unsigned value) static void uivector_init(uivector* p) { - p->data = NULL; + p->data = nullptr; p->size = p->allocsize = 0; } @@ -226,12 +226,12 @@ static void ucvector_cleanup(void* p) { ((ucvector*)p)->size = ((ucvector*)p)->allocsize = 0; myfree(((ucvector*)p)->data); - ((ucvector*)p)->data = NULL; + ((ucvector*)p)->data = nullptr; } static void ucvector_init(ucvector* p) { - p->data = NULL; + p->data = nullptr; p->size = p->allocsize = 0; } @@ -287,7 +287,7 @@ static unsigned string_resize(char** out, size_t size) /*init a {char*, size_t} pair for use as string*/ static void string_init(char** out) { - *out = NULL; + *out = nullptr; string_resize(out, 0); } @@ -295,7 +295,7 @@ static void string_init(char** out) static void string_cleanup(char** out) { myfree(*out); - *out = NULL; + *out = nullptr; } static void string_set(char** out, const char* in) @@ -2131,7 +2131,7 @@ static unsigned zlib_decompress(unsigned char** out, size_t* outsize, const unsi unsigned lodepng_zlib_compress(unsigned char** out, size_t* outsize, const unsigned char* in, size_t insize, const LodePNGCompressSettings* settings) { - /*initially, *out must be NULL and outsize 0, if you just give some random *out + /*initially, *out must be nullptr and outsize 0, if you just give some random *out that's pointing to a non allocated buffer, this'll crash*/ ucvector outv; size_t i; @@ -2680,8 +2680,8 @@ static unsigned LodePNGUnknownChunks_copy(LodePNGInfo* dest, const LodePNGInfo* static void LodePNGText_init(LodePNGInfo* info) { info->text_num = 0; - info->text_keys = NULL; - info->text_strings = NULL; + info->text_keys = nullptr; + info->text_strings = nullptr; } static void LodePNGText_cleanup(LodePNGInfo* info) @@ -2743,10 +2743,10 @@ unsigned lodepng_add_text(LodePNGInfo* info, const char* key, const char* str) static void LodePNGIText_init(LodePNGInfo* info) { info->itext_num = 0; - info->itext_keys = NULL; - info->itext_langtags = NULL; - info->itext_transkeys = NULL; - info->itext_strings = NULL; + info->itext_keys = nullptr; + info->itext_langtags = nullptr; + info->itext_transkeys = nullptr; + info->itext_strings = nullptr; } static void LodePNGIText_cleanup(LodePNGInfo* info) diff --git a/dart/gui/lodepng.h b/dart/gui/lodepng.h index 4ee0a0cf40dc5..d03d7cce929cc 100644 --- a/dart/gui/lodepng.h +++ b/dart/gui/lodepng.h @@ -708,7 +708,7 @@ unsigned lodepng_inflate(unsigned char** out, size_t* outsize, /* Decompresses Zlib data. Reallocates the out buffer and appends the data. The data must be according to the zlib specification. -Either, *out must be NULL and *outsize must be 0, or, *out must be a valid +Either, *out must be nullptr and *outsize must be 0, or, *out must be a valid buffer and *outsize its size in bytes. out must be freed by user after usage. */ unsigned lodepng_zlib_decompress(unsigned char** out, size_t* outsize, @@ -721,7 +721,7 @@ unsigned lodepng_zlib_decompress(unsigned char** out, size_t* outsize, Compresses data with Zlib. Reallocates the out buffer and appends the data. Zlib adds a small header and trailer around the deflate data. The data is output in the format of the zlib specification. -Either, *out must be NULL and *outsize must be 0, or, *out must be a valid +Either, *out must be nullptr and *outsize must be 0, or, *out must be a valid buffer and *outsize its size in bytes. out must be freed by user after usage. */ unsigned lodepng_zlib_compress(unsigned char** out, size_t* outsize, diff --git a/dart/lcpsolver/lcp.cpp b/dart/lcpsolver/lcp.cpp index fc5c3f7daf2c3..ca5f370b6c54e 100644 --- a/dart/lcpsolver/lcp.cpp +++ b/dart/lcpsolver/lcp.cpp @@ -778,7 +778,7 @@ void dLCP::unpermute() // an optimized Dantzig LCP driver routine for the lo-hi LCP problem. void dSolveLCP (int n, dReal *A, dReal *x, dReal *b, - dReal *outer_w/*=NULL*/, int nub, dReal *lo, dReal *hi, int *findex) + dReal *outer_w/*=nullptr*/, int nub, dReal *lo, dReal *hi, int *findex) { dAASSERT (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n); # ifndef dNODEBUG @@ -814,7 +814,7 @@ void dSolveLCP (int n, dReal *A, dReal *x, dReal *b, #ifdef ROWPTRS dReal **Arows = new dReal* [n]; #else - dReal **Arows = NULL; + dReal **Arows = nullptr; #endif int *p = new int[n]; int *C = new int[n]; @@ -1045,12 +1045,12 @@ void dSolveLCP (int n, dReal *A, dReal *x, dReal *b, case 5: // keep going x[si] = lo[si]; state[si] = false; - lcp.transfer_i_from_C_to_N (si, NULL); + lcp.transfer_i_from_C_to_N (si, nullptr); break; case 6: // keep going x[si] = hi[si]; state[si] = true; - lcp.transfer_i_from_C_to_N (si, NULL); + lcp.transfer_i_from_C_to_N (si, nullptr); break; } @@ -1129,8 +1129,8 @@ extern "C" ODE_API int dTestSolveLCP() const int n = 100; //size_t memreq = EstimateTestSolveLCPMemoryReq(n); - //dxWorldProcessMemArena *arena = dxAllocateTemporaryWorldProcessMemArena(memreq, NULL, NULL); - //if (arena == NULL) { + //dxWorldProcessMemArena *arena = dxAllocateTemporaryWorldProcessMemArena(memreq, nullptr, nullptr); + //if (arena == nullptr) { // return 0; //} diff --git a/dart/lcpsolver/matrix.cpp b/dart/lcpsolver/matrix.cpp index fa7a2f1135c80..787d6cd2f3dde 100644 --- a/dart/lcpsolver/matrix.cpp +++ b/dart/lcpsolver/matrix.cpp @@ -509,22 +509,22 @@ void dMultiply2 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r) int dFactorCholesky (dReal *A, int n) { - return _dFactorCholesky (A, n, NULL); + return _dFactorCholesky (A, n, nullptr); } void dSolveCholesky (const dReal *L, dReal *b, int n) { - _dSolveCholesky (L, b, n, NULL); + _dSolveCholesky (L, b, n, nullptr); } int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n) { - return _dInvertPDMatrix (A, Ainv, n, NULL); + return _dInvertPDMatrix (A, Ainv, n, nullptr); } int dIsPositiveDefinite (const dReal *A, int n) { - return _dIsPositiveDefinite (A, n, NULL); + return _dIsPositiveDefinite (A, n, nullptr); } // void dFactorLDLT (dReal *A, dReal *d, int n, int nskip); @@ -543,12 +543,12 @@ void dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip) void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip) { - _dLDLTAddTL (L, d, a, n, nskip, NULL); + _dLDLTAddTL (L, d, a, n, nskip, nullptr); } void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d, int n1, int n2, int r, int nskip) { - _dLDLTRemove (A, p, L, d, n1, n2, r, nskip, NULL); + _dLDLTRemove (A, p, L, d, n1, n2, r, nskip, nullptr); } void dRemoveRowCol (dReal *A, int n, int nskip, int r) diff --git a/dart/optimizer/Problem.cpp b/dart/optimizer/Problem.cpp index 0ecd7aaefb837..e815a45086ff9 100644 --- a/dart/optimizer/Problem.cpp +++ b/dart/optimizer/Problem.cpp @@ -113,7 +113,7 @@ const Eigen::VectorXd& Problem::getUpperBounds() const //============================================================================== void Problem::setObjective(Function* _obj) { - assert(_obj && "NULL pointer is not allowed."); + assert(_obj && "nullptr pointer is not allowed."); mObjective = _obj; } diff --git a/dart/optimizer/ipopt/IpoptSolver.cpp b/dart/optimizer/ipopt/IpoptSolver.cpp index 775129943634b..767802ca41f06 100644 --- a/dart/optimizer/ipopt/IpoptSolver.cpp +++ b/dart/optimizer/ipopt/IpoptSolver.cpp @@ -287,11 +287,11 @@ bool DartTNLP::eval_jac_g(Ipopt::Index _n, Ipopt::Index* _jCol, Ipopt::Number* _values) { - // If the iRow and jCol arguments are not NULL, then IPOPT wants you to fill + // If the iRow and jCol arguments are not nullptr, then IPOPT wants you to fill // in the sparsity structure of the Jacobian (the row and column indices - // only). At this time, the x argument and the values argument will be NULL. + // only). At this time, the x argument and the values argument will be nullptr. - if (_values == NULL) + if (_values == nullptr) { // return the structure of the Jacobian @@ -312,7 +312,7 @@ bool DartTNLP::eval_jac_g(Ipopt::Index _n, // return the values of the Jacobian of the constraints size_t idx = 0; Eigen::Map x(_x, _n); - Eigen::Map grad(NULL, 0); + Eigen::Map grad(nullptr, 0); // Evaluate function values for equality constraints for (size_t i = 0; i < mProblem->getNumEqConstraints(); ++i) diff --git a/dart/optimizer/ipopt/IpoptSolver.h b/dart/optimizer/ipopt/IpoptSolver.h index 81c05272151eb..142e26c330425 100644 --- a/dart/optimizer/ipopt/IpoptSolver.h +++ b/dart/optimizer/ipopt/IpoptSolver.h @@ -134,8 +134,8 @@ class DartTNLP : public Ipopt::TNLP Ipopt::Number* _g); /// \brief Method to return: - /// 1) The structure of the jacobian (if "values" is NULL) - /// 2) The values of the jacobian (if "values" is not NULL) + /// 1) The structure of the jacobian (if "values" is nullptr) + /// 2) The values of the jacobian (if "values" is not nullptr) virtual bool eval_jac_g(Ipopt::Index _n, const Ipopt::Number* _x, bool _new_x, @@ -147,9 +147,9 @@ class DartTNLP : public Ipopt::TNLP /// \brief Method to return: /// 1) The structure of the hessian of the lagrangian (if "values" is - /// NULL) + /// nullptr) /// 2) The values of the hessian of the lagrangian (if "values" is not - /// NULL) + /// nullptr) virtual bool eval_h(Ipopt::Index _n, const Ipopt::Number* _x, bool _new_x, diff --git a/dart/optimizer/nlopt/NloptSolver.h b/dart/optimizer/nlopt/NloptSolver.h index b86655fa53ef8..39b72cf63c100 100644 --- a/dart/optimizer/nlopt/NloptSolver.h +++ b/dart/optimizer/nlopt/NloptSolver.h @@ -69,7 +69,7 @@ class NloptSolver : public Solver /// \brief Wrapping function for nlopt callback function, nlopt_func static double _nlopt_func(unsigned _n, const double* _x, - double* _gradient, // NULL if not needed + double* _gradient, // nullptr if not needed void* _func_data); /// \brief Wrapping function for nlopt callback function, nlopt_mfunc @@ -77,7 +77,7 @@ class NloptSolver : public Solver double* _result, unsigned _n, const double* _x, - double* _gradient, // NULL if not needed + double* _gradient, // nullptr if not needed void* _func_data); /// \brief NLOPT data structure diff --git a/dart/planning/PathPlanner.h b/dart/planning/PathPlanner.h index b8d490a7ca8de..df1139c0bbfab 100644 --- a/dart/planning/PathPlanner.h +++ b/dart/planning/PathPlanner.h @@ -43,7 +43,7 @@ class PathPlanner { public: /// The default constructor - PathPlanner() : world(NULL) {} + PathPlanner() : world(nullptr) {} /// The desired constructor - you should use this one. PathPlanner(simulation::World& world, bool bidirectional_ = true, bool connect_ = true, double stepSize_ = 0.1, diff --git a/dart/planning/PathShortener.cpp b/dart/planning/PathShortener.cpp index ec85671e9b9a2..81688a885746a 100644 --- a/dart/planning/PathShortener.cpp +++ b/dart/planning/PathShortener.cpp @@ -31,7 +31,7 @@ PathShortener::~PathShortener() void PathShortener::shortenPath(list &path) { printf("--> Start Brute Force Shortener \n"); - srand(time(NULL)); + srand(time(nullptr)); VectorXd savedDofs = robot->getPositions(dofs); diff --git a/dart/planning/RRT.cpp b/dart/planning/RRT.cpp index d16f45592f94d..401fa73ab4f04 100644 --- a/dart/planning/RRT.cpp +++ b/dart/planning/RRT.cpp @@ -66,7 +66,7 @@ RRT::RRT(WorldPtr world, SkeletonPtr robot, const std::vector &dofs, index(new flann::Index >(flann::KDTreeSingleIndexParams())) { // Reset the random number generator and add the given start configuration to the flann structure - srand(time(NULL)); + srand(time(nullptr)); addNode(root, -1); } @@ -80,7 +80,7 @@ RRT::RRT(WorldPtr world, SkeletonPtr robot, const std::vector &dofs, con index(new flann::Index >(flann::KDTreeSingleIndexParams())) { // Reset the random number generator and add the given start configurations to the flann structure - srand(time(NULL)); + srand(time(nullptr)); for(size_t i = 0; i < roots.size(); i++) { addNode(roots[i], -1); } diff --git a/dart/renderer/OpenGLRenderInterface.cpp b/dart/renderer/OpenGLRenderInterface.cpp index be3550c7e3802..48059315de16f 100644 --- a/dart/renderer/OpenGLRenderInterface.cpp +++ b/dart/renderer/OpenGLRenderInterface.cpp @@ -347,7 +347,7 @@ void OpenGLRenderInterface::recursiveRender(const struct aiScene *sc, const stru glPushAttrib(GL_POLYGON_BIT | GL_LIGHTING_BIT); // for applyMaterial() applyMaterial(sc->mMaterials[mesh->mMaterialIndex]); - if(mesh->mNormals == NULL) { + if(mesh->mNormals == nullptr) { glDisable(GL_LIGHTING); } else { glEnable(GL_LIGHTING); @@ -368,9 +368,9 @@ void OpenGLRenderInterface::recursiveRender(const struct aiScene *sc, const stru for (i = 0; i < face->mNumIndices; i++) { int index = face->mIndices[i]; - if(mesh->mColors[0] != NULL) + if(mesh->mColors[0] != nullptr) glColor4fv((GLfloat*)&mesh->mColors[0][index]); - if(mesh->mNormals != NULL) + if(mesh->mNormals != nullptr) glNormal3fv(&mesh->mNormals[index].x); glVertex3fv(&mesh->mVertices[index].x); } diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 0cad3034b27f9..6857d1e5576a1 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -277,7 +277,7 @@ size_t World::getNumSkeletons() const //============================================================================== std::string World::addSkeleton(dynamics::SkeletonPtr _skeleton) { - assert(_skeleton != nullptr && "Attempted to add NULL skeleton to world."); + assert(_skeleton != nullptr && "Attempted to add nullptr skeleton to world."); if(nullptr == _skeleton) { diff --git a/dart/utils/C3D.cpp b/dart/utils/C3D.cpp index 170bdcb41172e..f6951e59ba5d4 100644 --- a/dart/utils/C3D.cpp +++ b/dart/utils/C3D.cpp @@ -82,7 +82,7 @@ bool loadC3DFile(const char* _fileName, Eigen::EIGEN_VV_VEC3D& _pointData, int* c3d_frame frame; bool bDecFmt = true; - if ((file = fopen( _fileName, "rb" )) == NULL) + if ((file = fopen( _fileName, "rb" )) == nullptr) return false; //get the header @@ -186,7 +186,7 @@ bool saveC3DFile(const char* _fileName, Eigen::EIGEN_VV_VEC3D& _pointData, int _ c3d_param parm; c3d_frame frame; - if ((file = fopen(_fileName, "wb")) == NULL) + if ((file = fopen(_fileName, "wb")) == nullptr) return false; int mrkrCnt = _nMarker; diff --git a/dart/utils/FileInfoDof.cpp b/dart/utils/FileInfoDof.cpp index dc1e5cb5287bf..7efbd42d0d7c2 100644 --- a/dart/utils/FileInfoDof.cpp +++ b/dart/utils/FileInfoDof.cpp @@ -81,7 +81,7 @@ bool FileInfoDof::loadFile(const char* _fName) inFile >> buffer; inFile >> nDof; - if (mSkel == NULL || mSkel->getNumDofs() != nDof) + if (mSkel == nullptr || mSkel->getNumDofs() != nDof) return false; mDofs.resize(mNumFrames); diff --git a/dart/utils/FileInfoWorld.cpp b/dart/utils/FileInfoWorld.cpp index 236272513b158..5ecc91df339b3 100644 --- a/dart/utils/FileInfoWorld.cpp +++ b/dart/utils/FileInfoWorld.cpp @@ -46,7 +46,7 @@ namespace utils { //============================================================================== FileInfoWorld::FileInfoWorld() - : mRecord(NULL) + : mRecord(nullptr) { std::strcpy(mFileName, ""); } diff --git a/dart/utils/Parser.cpp b/dart/utils/Parser.cpp index caca517a5cf4f..7f6b0cea06bfd 100644 --- a/dart/utils/Parser.cpp +++ b/dart/utils/Parser.cpp @@ -406,7 +406,7 @@ Eigen::Isometry3d toIsometry3dWithExtrinsicRotation(const std::string& _str) std::string getValueString(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); @@ -416,7 +416,7 @@ std::string getValueString(tinyxml2::XMLElement* _parentElement, const std::stri bool getValueBool(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); @@ -439,7 +439,7 @@ bool getValueBool(tinyxml2::XMLElement* _parentElement, const std::string& _name int getValueInt(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); @@ -449,7 +449,7 @@ int getValueInt(tinyxml2::XMLElement* _parentElement, const std::string& _name) unsigned int getValueUInt(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); @@ -459,7 +459,7 @@ unsigned int getValueUInt(tinyxml2::XMLElement* _parentElement, const std::strin float getValueFloat(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); @@ -469,7 +469,7 @@ float getValueFloat(tinyxml2::XMLElement* _parentElement, const std::string& _na double getValueDouble(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); @@ -479,7 +479,7 @@ double getValueDouble(tinyxml2::XMLElement* _parentElement, const std::string& _ char getValueChar(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); @@ -489,7 +489,7 @@ char getValueChar(tinyxml2::XMLElement* _parentElement, const std::string& _name Eigen::Vector2d getValueVector2d(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); @@ -499,7 +499,7 @@ Eigen::Vector2d getValueVector2d(tinyxml2::XMLElement* _parentElement, const std Eigen::Vector3d getValueVector3d(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); @@ -509,7 +509,7 @@ Eigen::Vector3d getValueVector3d(tinyxml2::XMLElement* _parentElement, const std Eigen::Vector3i getValueVector3i(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); @@ -519,7 +519,7 @@ Eigen::Vector3i getValueVector3i(tinyxml2::XMLElement* _parentElement, const std Eigen::Vector6d getValueVector6d(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); @@ -531,7 +531,7 @@ Eigen::Vector6d getValueVector6d(tinyxml2::XMLElement* _parentElement, const std Eigen::VectorXd getValueVectorXd(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); @@ -541,7 +541,7 @@ Eigen::VectorXd getValueVectorXd(tinyxml2::XMLElement* _parentElement, Eigen::Vector3d getValueVec3(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); @@ -551,7 +551,7 @@ Eigen::Vector3d getValueVec3(tinyxml2::XMLElement* _parentElement, const std::st Eigen::Isometry3d getValueIsometry3d(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); @@ -561,7 +561,7 @@ Eigen::Isometry3d getValueIsometry3d(tinyxml2::XMLElement* _parentElement, const Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); @@ -571,10 +571,10 @@ Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(tinyxml2::XMLElement* bool hasElement(tinyxml2::XMLElement* _parentElement, const std::string& _name) { - assert(_parentElement != NULL); + assert(_parentElement != nullptr); assert(!_name.empty()); - return _parentElement->FirstChildElement(_name.c_str()) == NULL ? false : true; + return _parentElement->FirstChildElement(_name.c_str()) == nullptr ? false : true; } tinyxml2::XMLElement* getElement(tinyxml2::XMLElement* _parentElement, @@ -642,7 +642,7 @@ ElementEnumerator::ElementEnumerator(tinyxml2::XMLElement* _parent, const std::string& _name) : m_name(_name), m_parent(_parent), - m_current(NULL) + m_current(nullptr) { } @@ -652,7 +652,7 @@ ElementEnumerator::~ElementEnumerator() bool ElementEnumerator::valid() const { - return m_current != NULL; + return m_current != nullptr; } bool ElementEnumerator::next() @@ -666,7 +666,7 @@ bool ElementEnumerator::next() m_current = m_parent->FirstChildElement(m_name.c_str()); if(!valid()) - m_parent = NULL; + m_parent = nullptr; return valid(); } diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 5f87bb8203cd2..ac10a33a738a9 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -48,7 +48,6 @@ #include "dart/collision/dart/DARTCollisionDetector.h" #include "dart/collision/fcl/FCLCollisionDetector.h" #include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" -//#include "dart/constraint/OldConstraintDynamics.h" #include "dart/constraint/ConstraintSolver.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/SoftBodyNode.h" diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 29dd9f4be2dcb..80324cdcad105 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -161,7 +161,7 @@ simulation::WorldPtr SdfParser::readWorld( std::function skeletonReader) { - assert(_worldElement != NULL); + assert(_worldElement != nullptr); // Create a world simulation::WorldPtr newWorld(new simulation::World); @@ -673,7 +673,7 @@ SdfParser::SDFJoint SdfParser::readJoint(tinyxml2::XMLElement* _jointElement, const BodyMap& _sdfBodyNodes, const Eigen::Isometry3d& _skeletonFrame) { - assert(_jointElement != NULL); + assert(_jointElement != nullptr); //-------------------------------------------------------------------------- // Type attribute @@ -893,7 +893,7 @@ dynamics::PrismaticJoint::Properties SdfParser::readPrismaticJoint( const Eigen::Isometry3d& _parentModelFrame, const std::string& _name) { - assert(_jointElement != NULL); + assert(_jointElement != nullptr); dynamics::PrismaticJoint::Properties newPrismaticJoint; diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index dc1bc27e3516a..245b755a8a153 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -520,7 +520,7 @@ TEST_F(COLLISION, CollisionOfPrescribedJoints) WorldPtr world = SkelParser::readWorld( DART_DATA_PATH"/skel/test/collision_of_prescribed_joints_test.skel"); world->setTimeStep(timeStep); - EXPECT_TRUE(world != NULL); + EXPECT_TRUE(world != nullptr); EXPECT_NEAR(world->getTimeStep(), timeStep, tol); SkeletonPtr skel1 = world->getSkeleton("skeleton 1"); @@ -529,12 +529,12 @@ TEST_F(COLLISION, CollisionOfPrescribedJoints) SkeletonPtr skel4 = world->getSkeleton("skeleton 4"); SkeletonPtr skel5 = world->getSkeleton("skeleton 5"); SkeletonPtr skel6 = world->getSkeleton("skeleton 6"); - EXPECT_TRUE(skel1 != NULL); - EXPECT_TRUE(skel2 != NULL); - EXPECT_TRUE(skel3 != NULL); - EXPECT_TRUE(skel4 != NULL); - EXPECT_TRUE(skel5 != NULL); - EXPECT_TRUE(skel6 != NULL); + EXPECT_TRUE(skel1 != nullptr); + EXPECT_TRUE(skel2 != nullptr); + EXPECT_TRUE(skel3 != nullptr); + EXPECT_TRUE(skel4 != nullptr); + EXPECT_TRUE(skel5 != nullptr); + EXPECT_TRUE(skel6 != nullptr); Joint* joint1 = skel1->getJoint(0); Joint* joint2 = skel2->getJoint(0); @@ -542,12 +542,12 @@ TEST_F(COLLISION, CollisionOfPrescribedJoints) Joint* joint4 = skel4->getJoint(0); Joint* joint5 = skel5->getJoint(0); Joint* joint6 = skel6->getJoint(0); - EXPECT_TRUE(joint1 != NULL); - EXPECT_TRUE(joint2 != NULL); - EXPECT_TRUE(joint3 != NULL); - EXPECT_TRUE(joint4 != NULL); - EXPECT_TRUE(joint5 != NULL); - EXPECT_TRUE(joint6 != NULL); + EXPECT_TRUE(joint1 != nullptr); + EXPECT_TRUE(joint2 != nullptr); + EXPECT_TRUE(joint3 != nullptr); + EXPECT_TRUE(joint4 != nullptr); + EXPECT_TRUE(joint5 != nullptr); + EXPECT_TRUE(joint6 != nullptr); EXPECT_EQ(joint1->getActuatorType(), Joint::FORCE); EXPECT_EQ(joint2->getActuatorType(), Joint::PASSIVE); EXPECT_EQ(joint3->getActuatorType(), Joint::SERVO); diff --git a/unittests/testConstraint.cpp b/unittests/testConstraint.cpp index 22a83282b4671..c9761faf8de53 100644 --- a/unittests/testConstraint.cpp +++ b/unittests/testConstraint.cpp @@ -122,7 +122,7 @@ void ConstraintTest::SingleContactTest(const std::string& _fileName) #endif WorldPtr world(new World); - EXPECT_TRUE(world != NULL); + EXPECT_TRUE(world != nullptr); world->setGravity(Vector3d(0.0, -10.00, 0.0)); world->setTimeStep(0.001); world->getConstraintSolver()->setCollisionDetector( diff --git a/unittests/testDynamics.cpp b/unittests/testDynamics.cpp index 84df4bf230326..8aa3779299704 100644 --- a/unittests/testDynamics.cpp +++ b/unittests/testDynamics.cpp @@ -237,8 +237,8 @@ MatrixXd DynamicsTest::getAugMassMatrix(dynamics::SkeletonPtr _skel) dynamics::BodyNode* body = _skel->getBodyNode(i); dynamics::Joint* joint = body->getParentJoint(); - EXPECT_TRUE(body != NULL); - EXPECT_TRUE(joint != NULL); + EXPECT_TRUE(body != nullptr); + EXPECT_TRUE(joint != nullptr); int dof = joint->getNumDofs(); @@ -702,7 +702,7 @@ void DynamicsTest::testFiniteDifferenceGeneralizedCoordinates( // load skeleton WorldPtr world = SkelParser::readWorld(_fileName); - assert(world != NULL); + assert(world != nullptr); world->setGravity(gravity); world->setTimeStep(timeStep); @@ -710,7 +710,7 @@ void DynamicsTest::testFiniteDifferenceGeneralizedCoordinates( for (size_t i = 0; i < world->getNumSkeletons(); ++i) { SkeletonPtr skeleton = world->getSkeleton(i); - assert(skeleton != NULL); + assert(skeleton != nullptr); int dof = skeleton->getNumDofs(); for (int j = 0; j < nRandomItr; ++j) @@ -804,7 +804,7 @@ void DynamicsTest::testFiniteDifferenceBodyNodeAcceleration( // load skeleton WorldPtr world = SkelParser::readWorld(_fileName); - assert(world != NULL); + assert(world != nullptr); world->setGravity(gravity); world->setTimeStep(timeStep); @@ -812,7 +812,7 @@ void DynamicsTest::testFiniteDifferenceBodyNodeAcceleration( for (size_t i = 0; i < world->getNumSkeletons(); ++i) { SkeletonPtr skeleton = world->getSkeleton(i); - assert(skeleton != NULL); + assert(skeleton != nullptr); int dof = skeleton->getNumDofs(); for (int j = 0; j < nRandomItr; ++j) @@ -1783,11 +1783,11 @@ TEST_F(DynamicsTest, HybridDynamics) WorldPtr world = utils::SkelParser::readWorld( DART_DATA_PATH"/skel/test/hybrid_dynamics_test.skel"); world->setTimeStep(timeStep); - EXPECT_TRUE(world != NULL); + EXPECT_TRUE(world != nullptr); EXPECT_NEAR(world->getTimeStep(), timeStep, tol); SkeletonPtr skel = world->getSkeleton("skeleton 1"); - EXPECT_TRUE(skel != NULL); + EXPECT_TRUE(skel != nullptr); EXPECT_NEAR(skel->getTimeStep(), timeStep, tol); const size_t numDofs = skel->getNumDofs(); diff --git a/unittests/testInverseKinematics.cpp b/unittests/testInverseKinematics.cpp index c039c73b51eda..d851698f0c89a 100644 --- a/unittests/testInverseKinematics.cpp +++ b/unittests/testInverseKinematics.cpp @@ -102,7 +102,6 @@ SkeletonPtr createFreeFloatingTwoLinkRobot(Vector3d dim1, // SkeletonPtr robot = createFreeFloatingTwoLinkRobot( // Vector3d(0.3, 0.3, l1), // Vector3d(0.3, 0.3, l2), DOF_ROLL); -// robot->init(); // size_t dof = robot->getNumDofs(); // VectorXd oldConfig = robot->getPositions(); diff --git a/unittests/testJoints.cpp b/unittests/testJoints.cpp index 9906b6fd68c61..5c7a533a0f1ed 100644 --- a/unittests/testJoints.cpp +++ b/unittests/testJoints.cpp @@ -363,20 +363,20 @@ void testJointCoulombFrictionForce(double _timeStep) simulation::WorldPtr myWorld = utils::SkelParser::readWorld( DART_DATA_PATH"/skel/test/joint_friction_test.skel"); - EXPECT_TRUE(myWorld != NULL); + EXPECT_TRUE(myWorld != nullptr); myWorld->setGravity(Eigen::Vector3d(0.0, 0.0, 0.0)); myWorld->setTimeStep(_timeStep); dynamics::SkeletonPtr pendulum = myWorld->getSkeleton("double_pendulum"); - EXPECT_TRUE(pendulum != NULL); + EXPECT_TRUE(pendulum != nullptr); pendulum->disableSelfCollision(); dynamics::Joint* joint0 = pendulum->getJoint("joint0"); dynamics::Joint* joint1 = pendulum->getJoint("joint1"); - EXPECT_TRUE(joint0 != NULL); - EXPECT_TRUE(joint1 != NULL); + EXPECT_TRUE(joint0 != nullptr); + EXPECT_TRUE(joint1 != nullptr); double frictionForce = 5.0; diff --git a/unittests/testNameManagement.cpp b/unittests/testNameManagement.cpp index d3eabae9181eb..403863a3a1803 100644 --- a/unittests/testNameManagement.cpp +++ b/unittests/testNameManagement.cpp @@ -175,10 +175,10 @@ TEST(NameManagement, Skeleton) EXPECT_FALSE(joint2->getName() == joint3->getName()); EXPECT_FALSE(joint3->getName() == joint1->getName()); - // Make sure that the Skeleton gives back NULL for non existent names - EXPECT_TRUE(skel->getBodyNode("nonexistent_name") == NULL); - EXPECT_TRUE(skel->getJoint("nonexistent_name") == NULL); - EXPECT_TRUE(skel->getSoftBodyNode("nonexistent_name") == NULL); + // Make sure that the Skeleton gives back nullptr for non existent names + EXPECT_TRUE(skel->getBodyNode("nonexistent_name") == nullptr); + EXPECT_TRUE(skel->getJoint("nonexistent_name") == nullptr); + EXPECT_TRUE(skel->getSoftBodyNode("nonexistent_name") == nullptr); } //============================================================================== diff --git a/unittests/testSoftDynamics.cpp b/unittests/testSoftDynamics.cpp index 0d033e20c1a59..c4c60abaeeb77 100644 --- a/unittests/testSoftDynamics.cpp +++ b/unittests/testSoftDynamics.cpp @@ -191,8 +191,8 @@ MatrixXd SoftDynamicsTest::getAugMassMatrix(dynamics::SkeletonPtr _skel) dynamics::BodyNode* body = _skel->getBodyNode(i); dynamics::Joint* joint = body->getParentJoint(); - EXPECT_TRUE(body != NULL); - EXPECT_TRUE(joint != NULL); + EXPECT_TRUE(body != nullptr); + EXPECT_TRUE(joint != nullptr); int dof = joint->getNumDofs(); @@ -208,7 +208,7 @@ MatrixXd SoftDynamicsTest::getAugMassMatrix(dynamics::SkeletonPtr _skel) // dynamics::SkeletonPtr softSkel // = dynamic_cast(_skel); -// if (softSkel != NULL) +// if (softSkel != nullptr) // { // for (int i = 0; i < softSkel->getNumSoftBodyNodes(); ++i) // { @@ -271,7 +271,7 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) // Check whether multiplication of mass matrix and its inverse is identity // matrix. myWorld = utils::SkelParser::readWorld(_fileName); - EXPECT_TRUE(myWorld != NULL); + EXPECT_TRUE(myWorld != nullptr); for (size_t i = 0; i < myWorld->getNumSkeletons(); ++i) { @@ -394,7 +394,7 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) vector oldD(nSoftBodyNodes, 0.0); for (int k = 0; k < nSoftBodyNodes; ++k) { - assert(softSkel != NULL); + assert(softSkel != nullptr); dynamics::SoftBodyNode* sbn = softSkel->getSoftBodyNode(k); oldKv[k] = sbn->getVertexSpringStiffness(); oldKe[k] = sbn->getEdgeSpringStiffness(); @@ -406,7 +406,7 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) softSkel->setAccelerations(VectorXd::Zero(dof)); for (int k = 0; k < nSoftBodyNodes; ++k) { - assert(softSkel != NULL); + assert(softSkel != nullptr); dynamics::SoftBodyNode* sbn = softSkel->getSoftBodyNode(k); sbn->setVertexSpringStiffness(0.0); sbn->setEdgeSpringStiffness(0.0); From ef5af9c7bdb8a9baf34da15abbcf1a3173c99f6f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 12 Jun 2015 17:03:34 -0400 Subject: [PATCH 16/19] Remove unnecessary lines --- apps/atlasSimbicon/State.h | 3 --- dart/gui/SimWindow.cpp | 1 - dart/utils/SkelParser.cpp | 13 ++----------- dart/utils/SkelParser.h | 2 -- dart/utils/sdf/SoftSdfParser.cpp | 1 - unittests/testInverseKinematics.cpp | 1 - 6 files changed, 2 insertions(+), 19 deletions(-) diff --git a/apps/atlasSimbicon/State.h b/apps/atlasSimbicon/State.h index 693a2d3d1a0d9..be74f54590a84 100644 --- a/apps/atlasSimbicon/State.h +++ b/apps/atlasSimbicon/State.h @@ -60,9 +60,6 @@ class BodyNode; class Joint; class Skeleton; } // namespace dynamics -namespace constraint { -class OldConstraintDynamics; -} // namespace constraint } // namespace dart class TerminalCondition; diff --git a/dart/gui/SimWindow.cpp b/dart/gui/SimWindow.cpp index 2cba0755cefc0..8a04d38445d1f 100644 --- a/dart/gui/SimWindow.cpp +++ b/dart/gui/SimWindow.cpp @@ -47,7 +47,6 @@ #include "dart/simulation/World.h" #include "dart/dynamics/Skeleton.h" -//#include "dart/constraint/OldConstraintDynamics.h" #include "dart/constraint/ConstraintSolver.h" #include "dart/collision/CollisionDetector.h" #include "dart/gui/LoadGlut.h" diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index ac10a33a738a9..20108ca5e17a2 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -193,9 +193,6 @@ dynamics::SkeletonPtr SkelParser::readSkeleton(const std::string& _filename) dynamics::SkeletonPtr newSkeleton = readSkeleton(skeletonElement); - // Initialize skeleton to be ready for use -// newSkeleton->init(); // No longer needed - return newSkeleton; } @@ -338,7 +335,7 @@ static NextResult getNextJointAndNodePair( } } - // Find the child node of this Joint, se we can create them together + // Find the child node of this Joint, so we can create them together child = bodyNodes.find(joint.childName); if(child == bodyNodes.end()) { @@ -475,9 +472,7 @@ dynamics::SkeletonPtr SkelParser::readSkeleton( ElementEnumerator xmlBodies(_skeletonElement, "body"); while (xmlBodies.next()) { - SkelBodyNode newBodyNode = readSoftBodyNode(xmlBodies.get(), - newSkeleton, - skeletonFrame); + SkelBodyNode newBodyNode = readSoftBodyNode(xmlBodies.get(), skeletonFrame); BodyMap::const_iterator it = bodyNodes.find(newBodyNode.properties->mName); if(it != bodyNodes.end()) @@ -548,11 +543,9 @@ dynamics::SkeletonPtr SkelParser::readSkeleton( //============================================================================== SkelParser::SkelBodyNode SkelParser::readBodyNode( tinyxml2::XMLElement* _bodyNodeElement, - dynamics::SkeletonPtr _skeleton, const Eigen::Isometry3d& _skeletonFrame) { assert(_bodyNodeElement != nullptr); - assert(_skeleton != nullptr); BodyPropPtr newBodyNode(new dynamics::BodyNode::Properties); Eigen::Isometry3d initTransform = Eigen::Isometry3d::Identity(); @@ -672,7 +665,6 @@ SkelParser::SkelBodyNode SkelParser::readBodyNode( //============================================================================== SkelParser::SkelBodyNode SkelParser::readSoftBodyNode( tinyxml2::XMLElement* _softBodyNodeElement, - dynamics::SkeletonPtr _Skeleton, const Eigen::Isometry3d& _skeletonFrame) { //---------------------------------- Note ------------------------------------ @@ -684,7 +676,6 @@ SkelParser::SkelBodyNode SkelParser::readSoftBodyNode( assert(_Skeleton != nullptr); SkelBodyNode standardBodyNode = readBodyNode(_softBodyNodeElement, - _Skeleton, _skeletonFrame); // If _softBodyNodeElement has no , return rigid body node diff --git a/dart/utils/SkelParser.h b/dart/utils/SkelParser.h index efa509f4be299..bc1161535a392 100644 --- a/dart/utils/SkelParser.h +++ b/dart/utils/SkelParser.h @@ -142,12 +142,10 @@ class SkelParser /// static SkelBodyNode readBodyNode(tinyxml2::XMLElement* _bodyElement, - dynamics::SkeletonPtr _skeleton, const Eigen::Isometry3d& _skeletonFrame); /// static SkelBodyNode readSoftBodyNode(tinyxml2::XMLElement* _softBodyNodeElement, - dynamics::SkeletonPtr _Skeleton, const Eigen::Isometry3d& _skeletonFrame); /// diff --git a/dart/utils/sdf/SoftSdfParser.cpp b/dart/utils/sdf/SoftSdfParser.cpp index 4385866b80428..24523aea24e2b 100644 --- a/dart/utils/sdf/SoftSdfParser.cpp +++ b/dart/utils/sdf/SoftSdfParser.cpp @@ -43,7 +43,6 @@ #include "dart/common/Console.h" #include "dart/collision/dart/DARTCollisionDetector.h" #include "dart/collision/fcl/FCLCollisionDetector.h" -//#include "dart/constraint/OldConstraintDynamics.h" // #include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" #include "dart/dynamics/Shape.h" #include "dart/dynamics/BoxShape.h" diff --git a/unittests/testInverseKinematics.cpp b/unittests/testInverseKinematics.cpp index d851698f0c89a..7682a6ac4ae69 100644 --- a/unittests/testInverseKinematics.cpp +++ b/unittests/testInverseKinematics.cpp @@ -216,7 +216,6 @@ SkeletonPtr createFreeFloatingTwoLinkRobot(Vector3d dim1, // SkeletonPtr robot = createFreeFloatingTwoLinkRobot( // Vector3d(0.3, 0.3, l1), // Vector3d(0.3, 0.3, l2), DOF_ROLL); -// robot->init(); // BodyNode* body1 = robot->getBodyNode(0); //// BodyNode* body2 = robot->getBodyNode(1); From 99378bd8b6732c5f37d3ab5a3187f8ebbfc53e66 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 13 Jun 2015 05:11:30 -0400 Subject: [PATCH 17/19] Fix build error introduced by the previous commit --- dart/dynamics/InvalidIndex.h | 5 +++-- dart/utils/SkelParser.cpp | 1 - 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/dart/dynamics/InvalidIndex.h b/dart/dynamics/InvalidIndex.h index 4cb9ae8f406fe..9040a563ef8f3 100644 --- a/dart/dynamics/InvalidIndex.h +++ b/dart/dynamics/InvalidIndex.h @@ -43,12 +43,13 @@ namespace dart { namespace dynamics { #if defined(_MSC_VER) +// TODO: Change to constexpr once Visual Studio supports it const size_t INVALID_INDEX = static_cast(-1); #else constexpr size_t INVALID_INDEX = static_cast(-1); #endif -} // dynamics -} // dart +} // namespace dynamics +} // namespace dart #endif // DART_DYNAMICS_INVALIDINDEX_H_ diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 20108ca5e17a2..f47e9eed2426c 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -673,7 +673,6 @@ SkelParser::SkelBodyNode SkelParser::readSoftBodyNode( //---------------------------------------------------------------------------- assert(_softBodyNodeElement != nullptr); - assert(_Skeleton != nullptr); SkelBodyNode standardBodyNode = readBodyNode(_softBodyNodeElement, _skeletonFrame); From 06594279624a79f7725ca3bb45b446d4a633fded Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 14 Jun 2015 19:26:45 -0400 Subject: [PATCH 18/19] Use setZero() instead of setConstant(0.0) --- dart/dynamics/EulerJoint.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index 658b201e15e68..83099f708b62c 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -389,7 +389,7 @@ void EulerJoint::updateLocalJacobianTimeDeriv() const dJ0 << -(dq1*c2*s1) - dq2*c1*s2, -(dq2*c1*c2) + dq1*s1*s2, dq1*c1, 0, 0, 0; dJ1 << dq2*c2, -(dq2*s2), 0.0, 0.0, 0.0, 0.0; - dJ2.setConstant(0.0); + dJ2.setZero(); break; } @@ -407,7 +407,7 @@ void EulerJoint::updateLocalJacobianTimeDeriv() const 0.0, 0.0, 0.0; dJ1 << 0.0, -s2*dq2, -c2*dq2, 0.0, 0.0, 0.0; - dJ2.setConstant(0.0); + dJ2.setZero(); break; } default: From fc87927662acbb1b2f16ce367a978e885aa1e0b9 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 14 Jun 2015 23:32:23 -0400 Subject: [PATCH 19/19] Restore Skeleton::computeForwardKinematics() --- dart/dynamics/Skeleton.cpp | 34 ++++++++++++++++++++++++++++++++++ dart/dynamics/Skeleton.h | 36 ++++++++++++++++++++++++++++++++++++ 2 files changed, 70 insertions(+) diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index d65a620faebd8..03d8871009998 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -2445,6 +2445,40 @@ const Eigen::VectorXd& Skeleton::computeConstraintForces(DataCache& cache) const return cache.mFc; } +//============================================================================== +void Skeleton::computeForwardKinematics(bool _updateTransforms, + bool _updateVels, + bool _updateAccs) +{ + if (_updateTransforms) + { + for (std::vector::iterator it = mSkelCache.mBodyNodes.begin(); + it != mSkelCache.mBodyNodes.end(); ++it) + { + (*it)->updateTransform(); + } + } + + if (_updateVels) + { + for (std::vector::iterator it = mSkelCache.mBodyNodes.begin(); + it != mSkelCache.mBodyNodes.end(); ++it) + { + (*it)->updateVelocity(); + (*it)->updatePartialAcceleration(); + } + } + + if (_updateAccs) + { + for (std::vector::iterator it = mSkelCache.mBodyNodes.begin(); + it != mSkelCache.mBodyNodes.end(); ++it) + { + (*it)->updateAccelerationID(); + } + } +} + //============================================================================== void Skeleton::computeForwardDynamics() { diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 8b073469952ef..312671201c865 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -335,6 +335,42 @@ class Skeleton : public MetaSkeleton /// Get the state of this skeleton described in generalized coordinates Eigen::VectorXd getState() const; + //---------------------------------------------------------------------------- + // Kinematics algorithms + //---------------------------------------------------------------------------- + + /// Compute forward kinematics + /// + /// In general, this function doesn't need to be called for forward kinematics + /// to update. Forward kinematics will always be computed when it's needed and + /// will only perform the computations that are necessary for what the user + /// requests. This works by performing some bookkeeping internally with dirty + /// flags whenever a position, velocity, or acceleration is set, either + /// internally or by the user. + /// + /// On one hand, this results in some overhead due to the extra effort of + /// bookkeeping, but on the other hand we have much greater code safety, and + /// in some cases performance can be dramatically improved with the auto- + /// updating. For example, this function is inefficient when only one portion + /// of the BodyNodes needed to be updated rather than the entire Skeleton, + /// which is common when performing inverse kinematics on a limb or on some + /// subsection of a Skeleton. + /// + /// This function might be useful in a case where the user wants to perform + /// all the forward kinematics computations during a particular time window + /// rather than waiting for it to be computed at the exact time that it's + /// needed. + /// + /// One example would be a real time controller. Let's say a controller gets + /// encoder data at time t0 but needs to wait until t1 before it receives the + /// force-torque sensor data that it needs in order to compute the output for + /// an operational space controller. Instead of being idle from t0 to t1, it + /// could use that time to compute the forward kinematics by calling this + /// function. + void computeForwardKinematics(bool _updateTransforms = true, + bool _updateVels = true, + bool _updateAccs = true); + //---------------------------------------------------------------------------- // Dynamics algorithms //----------------------------------------------------------------------------